| /* |
| * Copyright 2017-2018 NXP |
| */ |
| /* |
| * The code contained herein is licensed under the GNU General Public |
| * License. You may obtain a copy of the GNU General Public License |
| * Version 2 or later at the following locations: |
| * |
| * http://www.opensource.org/licenses/gpl-license.html |
| * http://www.gnu.org/copyleft/gpl.html |
| */ |
| #include <linux/module.h> |
| #include <linux/kernel.h> |
| #include <linux/types.h> |
| #include <linux/errno.h> |
| #include <linux/bug.h> |
| #include <linux/interrupt.h> |
| #include <linux/device.h> |
| #include <linux/pm_runtime.h> |
| #include <linux/list.h> |
| #include <linux/slab.h> |
| #include <linux/of_graph.h> |
| |
| #include <linux/videodev2.h> |
| #include <media/v4l2-device.h> |
| #include <media/v4l2-ioctl.h> |
| #include <media/v4l2-mem2mem.h> |
| #include <media/videobuf2-core.h> |
| #include <media/videobuf2-dma-contig.h> |
| #include <soc/imx8/sc/sci.h> |
| |
| #include "mxc-isi-core.h" |
| #include "mxc-isi-hw.h" |
| #include "mxc-media-dev.h" |
| |
| #define to_isi_buffer(x) \ |
| container_of((x), struct mxc_isi_buffer, v4l2_buf) |
| |
| #define file_to_ctx(file) \ |
| container_of(file->private_data, struct mxc_isi_ctx, fh); |
| |
| extern struct mxc_isi_fmt mxc_isi_out_formats[9]; |
| |
| struct mxc_isi_fmt mxc_isi_input_formats[] = { |
| /* Pixel link input format */ |
| { |
| .name = "XBGR32", |
| .fourcc = V4L2_PIX_FMT_XBGR32, |
| .depth = { 32 }, |
| .color = MXC_ISI_M2M_IN_FMT_XRGB8, |
| .memplanes = 1, |
| .colplanes = 1, |
| }, { |
| .name = "RGB565", |
| .fourcc = V4L2_PIX_FMT_RGB565, |
| .depth = { 16 }, |
| .color = MXC_ISI_M2M_IN_FMT_RGB565, |
| .memplanes = 1, |
| .colplanes = 1, |
| }, { |
| .name = "YUV32 (X-Y-U-V)", |
| .fourcc = V4L2_PIX_FMT_YUV32, |
| .depth = { 32 }, |
| .color = MXC_ISI_M2M_IN_FMT_YUV444_1P8P, |
| .memplanes = 1, |
| .colplanes = 1, |
| }, { |
| .name = "YUV16 (X-Y-U-V)", |
| .fourcc = V4L2_PIX_FMT_YUYV, |
| .depth = { 16 }, |
| .color = MXC_ISI_M2M_IN_FMT_YUV422_1P8P, |
| .memplanes = 1, |
| .colplanes = 1, |
| }, { |
| .name = "RGBA (R-G-B-A)", |
| .fourcc = V4L2_PIX_FMT_RGBA, |
| .depth = { 32 }, |
| .color = MXC_ISI_M2M_IN_FMT_XBGR8, |
| .memplanes = 1, |
| .colplanes = 1, |
| } |
| }; |
| |
| static struct mxc_isi_buffer *vb2_to_isi_buffer(struct vb2_buffer *vb2) |
| { |
| struct vb2_v4l2_buffer *v4l2_buf; |
| struct mxc_isi_buffer *buf; |
| |
| v4l2_buf = to_vb2_v4l2_buffer(vb2); |
| buf = to_isi_buffer(v4l2_buf); |
| |
| return buf; |
| } |
| |
| static struct v4l2_m2m_buffer *to_v4l2_m2m_buffer(struct vb2_buffer *vb2) |
| { |
| struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb2); |
| struct v4l2_m2m_buffer *b; |
| |
| b = container_of(vbuf, struct v4l2_m2m_buffer, vb); |
| return b; |
| } |
| |
| static void mxc_isi_m2m_device_run(void *priv) |
| { |
| struct mxc_isi_ctx *mxc_ctx = priv; |
| struct mxc_isi_dev *mxc_isi = mxc_ctx->isi_dev; |
| struct v4l2_fh *fh = &mxc_ctx->fh; |
| struct vb2_buffer *src_vb2; |
| struct mxc_isi_buffer *src_buf; |
| unsigned long flags; |
| |
| dev_dbg(&mxc_isi->pdev->dev, "%s enter\n", __func__); |
| |
| spin_lock_irqsave(&mxc_isi->slock, flags); |
| |
| /* SRC */ |
| src_vb2 = v4l2_m2m_next_src_buf(fh->m2m_ctx); |
| if (!src_vb2) { |
| dev_err(&mxc_isi->pdev->dev, "Null src buf\n"); |
| goto unlock; |
| } |
| |
| src_buf = vb2_to_isi_buffer(src_vb2); |
| |
| |
| mxc_isi_channel_set_m2m_src_addr(mxc_isi, src_buf); |
| mxc_isi_m2m_channel_enable(mxc_isi); |
| |
| unlock: |
| spin_unlock_irqrestore(&mxc_isi->slock, flags); |
| } |
| |
| static int mxc_isi_m2m_job_ready(void *priv) |
| { |
| struct mxc_isi_ctx *mxc_ctx = priv; |
| struct mxc_isi_dev *mxc_isi = mxc_ctx->isi_dev; |
| struct v4l2_fh *fh = &mxc_ctx->fh; |
| unsigned int num_src_bufs_ready; |
| unsigned int num_dst_bufs_ready; |
| unsigned long flags; |
| |
| dev_dbg(&mxc_isi->pdev->dev, "%s\n", __func__); |
| |
| spin_lock_irqsave(&mxc_isi->slock, flags); |
| num_src_bufs_ready = v4l2_m2m_num_src_bufs_ready(fh->m2m_ctx); |
| num_dst_bufs_ready = v4l2_m2m_num_dst_bufs_ready(fh->m2m_ctx); |
| spin_unlock_irqrestore(&mxc_isi->slock, flags); |
| |
| if (num_src_bufs_ready >= 1 && num_dst_bufs_ready >= 1) |
| return 1; |
| return 0; |
| } |
| |
| static void mxc_isi_m2m_job_abort(void *priv) |
| { |
| struct mxc_isi_ctx *mxc_ctx = priv; |
| struct mxc_isi_dev *mxc_isi = mxc_ctx->isi_dev; |
| |
| mxc_isi->m2m.aborting = 1; |
| dev_dbg(&mxc_isi->pdev->dev, "Abort requested\n"); |
| } |
| |
| static struct v4l2_m2m_ops mxc_isi_m2m_ops = { |
| .device_run = mxc_isi_m2m_device_run, |
| .job_ready = mxc_isi_m2m_job_ready, |
| .job_abort = mxc_isi_m2m_job_abort, |
| }; |
| |
| static int m2m_vb2_queue_setup(struct vb2_queue *q, |
| unsigned int *num_buffers, unsigned int *num_planes, |
| unsigned int sizes[], struct device *alloc_devs[]) |
| { |
| struct mxc_isi_ctx *mxc_ctx = vb2_get_drv_priv(q); |
| struct mxc_isi_dev *mxc_isi = mxc_ctx->isi_dev; |
| struct device *dev = &mxc_isi->pdev->dev; |
| struct mxc_isi_frame *frame; |
| struct mxc_isi_fmt *fmt; |
| unsigned long wh; |
| int i; |
| |
| dev_dbg(&mxc_isi->pdev->dev, "%s\n", __func__); |
| |
| if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) { |
| if (*num_buffers < 3) { |
| dev_err(dev, "%s at least need 3 buffer\n", __func__); |
| return -EINVAL; |
| } |
| frame = &mxc_isi->m2m.dst_f; |
| } else { |
| if (*num_buffers < 1) { |
| dev_err(dev, "%s at least need one buffer\n", __func__); |
| return -EINVAL; |
| } |
| frame = &mxc_isi->m2m.src_f; |
| } |
| |
| fmt = frame->fmt; |
| if (fmt == NULL) |
| return -EINVAL; |
| |
| for (i = 0; i < fmt->memplanes; i++) |
| alloc_devs[i] = &mxc_isi->pdev->dev; |
| |
| *num_planes = fmt->memplanes; |
| wh = frame->width * frame->height; |
| |
| for (i = 0; i < fmt->memplanes; i++) { |
| unsigned int size = (wh * fmt->depth[i]) >> 3; |
| |
| if (i == 1 && fmt->fourcc == V4L2_PIX_FMT_NV12) |
| size >>= 1; |
| sizes[i] = max_t(u32, size, frame->sizeimage[i]); |
| |
| dev_dbg(&mxc_isi->pdev->dev, "%s, buf_n=%d, planes[%d]->size=%d\n", |
| __func__, *num_buffers, i, sizes[i]); |
| } |
| |
| return 0; |
| } |
| |
| static int m2m_vb2_buffer_prepare(struct vb2_buffer *vb2) |
| { |
| struct vb2_queue *vq = vb2->vb2_queue; |
| struct mxc_isi_ctx *mxc_ctx = vb2_get_drv_priv(vq); |
| struct mxc_isi_dev *mxc_isi = mxc_ctx->isi_dev; |
| struct mxc_isi_frame *frame; |
| int i; |
| |
| if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) |
| frame = &mxc_isi->m2m.dst_f; |
| else |
| frame = &mxc_isi->m2m.src_f; |
| |
| if (frame == NULL) |
| return -EINVAL; |
| |
| for (i = 0; i < frame->fmt->memplanes; i++) { |
| unsigned long size = frame->sizeimage[i]; |
| |
| if (vb2_plane_size(vb2, i) < size) { |
| dev_err(&mxc_isi->pdev->dev, |
| "User buffer too small (%ld < %ld)\n", |
| vb2_plane_size(vb2, i), size); |
| return -EINVAL; |
| } |
| vb2_set_plane_payload(vb2, i, size); |
| } |
| |
| return 0; |
| } |
| |
| static void m2m_vb2_buffer_queue(struct vb2_buffer *vb2) |
| { |
| struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb2); |
| struct mxc_isi_ctx *mxc_ctx = vb2_get_drv_priv(vb2->vb2_queue); |
| struct v4l2_fh *fh = &mxc_ctx->fh; |
| |
| v4l2_m2m_buf_queue(fh->m2m_ctx, vbuf); |
| } |
| |
| static int m2m_vb2_start_streaming(struct vb2_queue *q, unsigned int count) |
| { |
| struct mxc_isi_ctx *mxc_ctx = vb2_get_drv_priv(q); |
| struct mxc_isi_dev *mxc_isi = mxc_ctx->isi_dev; |
| struct v4l2_fh *fh = &mxc_ctx->fh; |
| struct vb2_buffer *dst_vb2; |
| struct v4l2_m2m_buffer *b; |
| struct mxc_isi_buffer *dst_buf; |
| unsigned long flags; |
| |
| dev_dbg(&mxc_isi->pdev->dev, "%s\n", __func__); |
| |
| if (V4L2_TYPE_IS_OUTPUT(q->type)) |
| return 0; |
| |
| if (count < 2) { |
| dev_err(&mxc_isi->pdev->dev, "Need to at leas 2 buffers\n"); |
| return -EINVAL; |
| } |
| |
| spin_lock_irqsave(&mxc_isi->slock, flags); |
| |
| /* BUF1 */ |
| dst_vb2 = v4l2_m2m_next_dst_buf(fh->m2m_ctx); |
| if (!dst_vb2) { |
| dev_err(&mxc_isi->pdev->dev, "%d: Null dst buf\n", __LINE__); |
| goto unlock; |
| } |
| dst_vb2->state = VB2_BUF_STATE_ACTIVE; |
| dst_buf = vb2_to_isi_buffer(dst_vb2); |
| dst_buf->v4l2_buf.sequence = 0; |
| mxc_isi_channel_set_m2m_out_addr(mxc_isi, dst_buf); |
| v4l2_m2m_dst_buf_remove(fh->m2m_ctx); |
| b = to_v4l2_m2m_buffer(dst_vb2); |
| list_add_tail(&b->list, &mxc_isi->m2m.out_active); |
| |
| /* BUF2 */ |
| dst_vb2 = v4l2_m2m_next_dst_buf(fh->m2m_ctx); |
| if (!dst_vb2) { |
| dev_err(&mxc_isi->pdev->dev, "%d: Null dst buf\n", __LINE__); |
| goto unlock; |
| } |
| dst_vb2->state = VB2_BUF_STATE_ACTIVE; |
| dst_buf = vb2_to_isi_buffer(dst_vb2); |
| dst_buf->v4l2_buf.sequence = 1; |
| mxc_isi_channel_set_m2m_out_addr(mxc_isi, dst_buf); |
| v4l2_m2m_dst_buf_remove(fh->m2m_ctx); |
| b = to_v4l2_m2m_buffer(dst_vb2); |
| list_add_tail(&b->list, &mxc_isi->m2m.out_active); |
| |
| mxc_isi->m2m.frame_count = 1; |
| mxc_isi->m2m.aborting = 0; |
| unlock: |
| spin_unlock_irqrestore(&mxc_isi->slock, flags); |
| |
| return 0; |
| } |
| |
| static void m2m_vb2_stop_streaming(struct vb2_queue *q) |
| { |
| struct mxc_isi_ctx *mxc_ctx = vb2_get_drv_priv(q); |
| struct mxc_isi_dev *mxc_isi = mxc_ctx->isi_dev; |
| struct vb2_v4l2_buffer *vb2; |
| struct mxc_isi_buffer *buf; |
| unsigned long flags; |
| |
| spin_lock_irqsave(&mxc_isi->slock, flags); |
| |
| while ((vb2 = v4l2_m2m_src_buf_remove(mxc_ctx->fh.m2m_ctx)) != NULL) |
| v4l2_m2m_buf_done(vb2, VB2_BUF_STATE_ERROR); |
| |
| while ((vb2 = v4l2_m2m_dst_buf_remove(mxc_ctx->fh.m2m_ctx)) != NULL) |
| v4l2_m2m_buf_done(vb2, VB2_BUF_STATE_ERROR); |
| |
| while (!list_empty(&mxc_isi->m2m.out_active)) { |
| buf = list_entry(mxc_isi->m2m.out_active.next, struct mxc_isi_buffer, list); |
| list_del(&buf->list); |
| vb2_buffer_done(&buf->v4l2_buf.vb2_buf, VB2_BUF_STATE_ERROR); |
| } |
| |
| INIT_LIST_HEAD(&mxc_isi->isi_cap.out_active); |
| |
| spin_unlock_irqrestore(&mxc_isi->slock, flags); |
| } |
| |
| static struct vb2_ops mxc_m2m_vb2_qops = { |
| .queue_setup = m2m_vb2_queue_setup, |
| .buf_prepare = m2m_vb2_buffer_prepare, |
| .buf_queue = m2m_vb2_buffer_queue, |
| .wait_prepare = vb2_ops_wait_prepare, |
| .wait_finish = vb2_ops_wait_finish, |
| .start_streaming = m2m_vb2_start_streaming, |
| .stop_streaming = m2m_vb2_stop_streaming, |
| }; |
| |
| |
| static int mxc_m2m_queue_init(void *priv, struct vb2_queue *src_vq, |
| struct vb2_queue *dst_vq) |
| { |
| struct mxc_isi_ctx *mxc_ctx = priv; |
| struct mxc_isi_dev *mxc_isi = mxc_ctx->isi_dev; |
| int ret; |
| |
| src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE; |
| src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF; |
| src_vq->drv_priv = mxc_ctx; |
| src_vq->buf_struct_size = sizeof(struct mxc_isi_buffer); |
| src_vq->ops = &mxc_m2m_vb2_qops; |
| src_vq->mem_ops = &vb2_dma_contig_memops; |
| src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY; |
| src_vq->lock = &mxc_isi->m2m_lock; |
| src_vq->dev = &mxc_isi->pdev->dev; |
| |
| ret = vb2_queue_init(src_vq); |
| if (ret) |
| return ret; |
| |
| dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; |
| dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF; |
| dst_vq->drv_priv = mxc_ctx; |
| dst_vq->buf_struct_size = sizeof(struct mxc_isi_buffer); |
| dst_vq->ops = &mxc_m2m_vb2_qops; |
| dst_vq->mem_ops = &vb2_dma_contig_memops; |
| dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY; |
| dst_vq->lock = &mxc_isi->m2m_lock; |
| dst_vq->dev = &mxc_isi->pdev->dev; |
| |
| ret = vb2_queue_init(dst_vq); |
| return ret; |
| } |
| |
| static int mxc_isi_m2m_open(struct file *file) |
| { |
| struct video_device *vdev = video_devdata(file); |
| struct mxc_isi_dev *mxc_isi = video_drvdata(file); |
| struct mxc_isi_m2m_dev *isi_m2m = &mxc_isi->m2m; |
| struct device *dev = &mxc_isi->pdev->dev; |
| struct mxc_isi_ctx *mxc_ctx = NULL; |
| int ret = 0; |
| |
| dev_dbg(dev, "%s, ISI%d\n", __func__, mxc_isi->id); |
| |
| if (atomic_read(&mxc_isi->open_count) > 0) { |
| dev_err(dev, "%s: ISI channel[%d] is busy\n", __func__, mxc_isi->id); |
| return -EBUSY; |
| } |
| |
| if (mutex_lock_interruptible(&mxc_isi->m2m_lock)) |
| return -ERESTARTSYS; |
| mxc_ctx = kzalloc(sizeof(*mxc_ctx), GFP_KERNEL); |
| if (!mxc_ctx) { |
| ret = -ENOMEM; |
| goto unlock; |
| } |
| |
| mxc_ctx->isi_dev = mxc_isi; |
| |
| v4l2_fh_init(&mxc_ctx->fh, vdev); |
| file->private_data = &mxc_ctx->fh; |
| |
| mxc_ctx->fh.m2m_ctx = |
| v4l2_m2m_ctx_init(isi_m2m->m2m_dev, mxc_ctx, mxc_m2m_queue_init); |
| if (IS_ERR(mxc_ctx->fh.m2m_ctx)) { |
| dev_err(dev, "%s v4l2_m2m_ctx_init fail\n", __func__); |
| ret = PTR_ERR(mxc_ctx->fh.m2m_ctx); |
| v4l2_fh_exit(&mxc_ctx->fh); |
| kfree(mxc_ctx); |
| goto unlock; |
| } |
| v4l2_fh_add(&mxc_ctx->fh); |
| |
| pm_runtime_get_sync(dev); |
| if (atomic_inc_return(&mxc_isi->open_count) == 1) |
| mxc_isi_m2m_channel_init(mxc_isi); |
| |
| mxc_isi->is_m2m = 1; |
| unlock: |
| mutex_unlock(&mxc_isi->m2m_lock); |
| return ret; |
| } |
| |
| static int mxc_isi_m2m_release(struct file *file) |
| { |
| struct mxc_isi_dev *mxc_isi = video_drvdata(file); |
| struct device *dev = &mxc_isi->pdev->dev; |
| struct mxc_isi_ctx *mxc_ctx = file_to_ctx(file); |
| |
| dev_dbg(&mxc_isi->pdev->dev, "%s\n", __func__); |
| |
| v4l2_fh_del(&mxc_ctx->fh); |
| v4l2_fh_exit(&mxc_ctx->fh); |
| |
| if (mutex_is_locked(&mxc_isi->m2m_lock)) |
| mutex_unlock(&mxc_isi->m2m_lock); |
| |
| mutex_lock(&mxc_isi->m2m_lock); |
| v4l2_m2m_ctx_release(mxc_ctx->fh.m2m_ctx); |
| mutex_unlock(&mxc_isi->m2m_lock); |
| |
| kfree(mxc_ctx); |
| if (atomic_dec_and_test(&mxc_isi->open_count)) |
| mxc_isi_channel_deinit(mxc_isi); |
| |
| mxc_isi->is_m2m = 0; |
| pm_runtime_put(dev); |
| return 0; |
| } |
| |
| static const struct v4l2_file_operations mxc_isi_m2m_fops = { |
| .owner = THIS_MODULE, |
| .open = mxc_isi_m2m_open, |
| .release = mxc_isi_m2m_release, |
| .poll = v4l2_m2m_fop_poll, |
| .unlocked_ioctl = video_ioctl2, |
| .mmap = v4l2_m2m_fop_mmap, |
| }; |
| |
| static int mxc_isi_m2m_querycap(struct file *file, void *priv, |
| struct v4l2_capability *cap) |
| { |
| struct mxc_isi_dev *mxc_isi = video_drvdata(file); |
| |
| strlcpy(cap->driver, MXC_ISI_DRIVER_NAME, sizeof(cap->driver)); |
| strlcpy(cap->card, MXC_ISI_DRIVER_NAME, sizeof(cap->card)); |
| snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s.%d", |
| dev_name(&mxc_isi->pdev->dev), mxc_isi->id); |
| |
| cap->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M_MPLANE | |
| V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_VIDEO_OUTPUT_MPLANE; |
| cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS; |
| |
| return 0; |
| } |
| |
| static int mxc_isi_m2m_enum_fmt_vid_out(struct file *file, void *priv, |
| struct v4l2_fmtdesc *f) |
| { |
| struct mxc_isi_dev *mxc_isi = video_drvdata(file); |
| struct mxc_isi_fmt *fmt; |
| |
| dev_dbg(&mxc_isi->pdev->dev, "%s\n", __func__); |
| if (f->index >= (int)ARRAY_SIZE(mxc_isi_input_formats)) |
| return -EINVAL; |
| |
| fmt = &mxc_isi_input_formats[f->index]; |
| strncpy(f->description, fmt->name, sizeof(f->description) - 1); |
| |
| f->pixelformat = fmt->fourcc; |
| |
| return 0; |
| } |
| |
| static int mxc_isi_m2m_enum_fmt_vid_cap(struct file *file, void *priv, |
| struct v4l2_fmtdesc *f) |
| { |
| struct mxc_isi_dev *mxc_isi = video_drvdata(file); |
| struct mxc_isi_fmt *fmt; |
| |
| dev_dbg(&mxc_isi->pdev->dev, "%s\n", __func__); |
| if (f->index >= (int)ARRAY_SIZE(mxc_isi_out_formats)) |
| return -EINVAL; |
| |
| fmt = &mxc_isi_out_formats[f->index]; |
| strncpy(f->description, fmt->name, sizeof(f->description) - 1); |
| |
| f->pixelformat = fmt->fourcc; |
| |
| return 0; |
| } |
| |
| static int mxc_isi_m2m_try_fmt_vid_out(struct file *file, void *fh, |
| struct v4l2_format *f) |
| { |
| struct mxc_isi_dev *mxc_isi = video_drvdata(file); |
| struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp; |
| struct mxc_isi_fmt *fmt = NULL; |
| int i; |
| |
| dev_dbg(&mxc_isi->pdev->dev, "%s\n", __func__); |
| |
| if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) |
| return -EINVAL; |
| |
| for (i = 0; i < ARRAY_SIZE(mxc_isi_input_formats); i++) { |
| fmt = &mxc_isi_input_formats[i]; |
| if (fmt->fourcc == pix->pixelformat) |
| break; |
| } |
| |
| if (i >= ARRAY_SIZE(mxc_isi_input_formats)) { |
| v4l2_err(mxc_isi->v4l2_dev, "%s, format is not support!\n", __func__); |
| return -EINVAL; |
| } |
| |
| if (pix->width <= 0 || pix->height <= 0) { |
| v4l2_err(mxc_isi->v4l2_dev, "%s, width %d, height %d is not valid\n" |
| , __func__, pix->width, pix->height); |
| return -EINVAL; |
| } |
| |
| return 0; |
| } |
| |
| static int mxc_isi_m2m_try_fmt_vid_cap(struct file *file, void *fh, |
| struct v4l2_format *f) |
| { |
| struct mxc_isi_dev *mxc_isi = video_drvdata(file); |
| struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp; |
| struct mxc_isi_fmt *fmt = NULL; |
| int i; |
| |
| dev_dbg(&mxc_isi->pdev->dev, "%s\n", __func__); |
| |
| if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) |
| return -EINVAL; |
| |
| for (i = 0; i < ARRAY_SIZE(mxc_isi_out_formats); i++) { |
| fmt = &mxc_isi_out_formats[i]; |
| if (fmt->fourcc == pix->pixelformat) |
| break; |
| } |
| |
| if (i >= ARRAY_SIZE(mxc_isi_out_formats)) { |
| v4l2_err(mxc_isi->v4l2_dev, "%s, format is not support!\n", __func__); |
| return -EINVAL; |
| } |
| |
| if (pix->width <= 0 || pix->height <= 0) { |
| v4l2_err(mxc_isi->v4l2_dev, "%s, width %d, height %d is not valid\n" |
| , __func__, pix->width, pix->height); |
| return -EINVAL; |
| } |
| |
| return 0; |
| } |
| |
| static int mxc_isi_m2m_s_fmt_vid_out(struct file *file, void *priv, |
| struct v4l2_format *f) |
| { |
| struct mxc_isi_dev *mxc_isi = video_drvdata(file); |
| struct v4l2_fh *fh = file->private_data; |
| struct mxc_isi_frame *frame = &mxc_isi->m2m.src_f; |
| struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp; |
| struct mxc_isi_fmt *fmt; |
| struct vb2_queue *vq; |
| int bpl, i; |
| |
| dev_dbg(&mxc_isi->pdev->dev, "%s\n", __func__); |
| |
| if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) |
| return -EINVAL; |
| |
| vq = v4l2_m2m_get_vq(fh->m2m_ctx, f->type); |
| if (!vq) |
| return -EINVAL; |
| |
| if (vb2_is_busy(vq)) { |
| dev_err(&mxc_isi->pdev->dev, "queue busy\n"); |
| return -EBUSY; |
| } |
| |
| for (i = 0; i < ARRAY_SIZE(mxc_isi_input_formats); i++) { |
| fmt = &mxc_isi_input_formats[i]; |
| if (pix && fmt->fourcc == pix->pixelformat) |
| break; |
| } |
| |
| if (i >= ARRAY_SIZE(mxc_isi_input_formats)) { |
| dev_dbg(&mxc_isi->pdev->dev, "%s, format is not support!\n", __func__); |
| return -EINVAL; |
| } |
| |
| /* update out put frame size and formate */ |
| if (pix->height <= 0 || pix->width <= 0) |
| return -EINVAL; |
| |
| frame->fmt = fmt; |
| frame->height = pix->height; |
| frame->width = pix->width; |
| |
| pix->num_planes = fmt->memplanes; |
| for (i = 0; i < pix->num_planes; i++) { |
| bpl = pix->plane_fmt[i].bytesperline; |
| |
| if ((bpl == 0) || (bpl / (fmt->depth[i] >> 3)) < pix->width) |
| pix->plane_fmt[i].bytesperline = |
| (pix->width * fmt->depth[i]) >> 3; |
| |
| if (pix->plane_fmt[i].sizeimage == 0) |
| pix->plane_fmt[i].sizeimage = (pix->width * pix->height * |
| fmt->depth[i] >> 3); |
| } |
| |
| frame->bytesperline[0] = frame->width * frame->fmt->depth[0] / 8; |
| frame->sizeimage[0] = frame->height * frame->bytesperline[0]; |
| |
| set_frame_bounds(frame, pix->width, pix->height); |
| mxc_isi_m2m_config_src(mxc_isi); |
| |
| return 0; |
| } |
| |
| static int mxc_isi_m2m_s_fmt_vid_cap(struct file *file, void *priv, |
| struct v4l2_format *f) |
| { |
| struct mxc_isi_dev *mxc_isi = video_drvdata(file); |
| struct v4l2_fh *fh = file->private_data; |
| struct mxc_isi_frame *frame = &mxc_isi->m2m.dst_f; |
| struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp; |
| struct mxc_isi_fmt *fmt; |
| struct vb2_queue *vq; |
| int bpl, i; |
| |
| dev_dbg(&mxc_isi->pdev->dev, "%s\n", __func__); |
| |
| if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) |
| return -EINVAL; |
| |
| vq = v4l2_m2m_get_vq(fh->m2m_ctx, f->type); |
| if (!vq) |
| return -EINVAL; |
| |
| if (vb2_is_busy(vq)) { |
| dev_err(&mxc_isi->pdev->dev, "queue busy\n"); |
| return -EBUSY; |
| } |
| |
| for (i = 0; i < ARRAY_SIZE(mxc_isi_out_formats); i++) { |
| fmt = &mxc_isi_out_formats[i]; |
| if (pix && fmt->fourcc == pix->pixelformat) |
| break; |
| } |
| |
| if (i >= ARRAY_SIZE(mxc_isi_out_formats)) { |
| dev_err(&mxc_isi->pdev->dev, "%s, format is not support!\n", __func__); |
| return -EINVAL; |
| } |
| |
| /* update out put frame size and formate */ |
| if (pix->height <= 0 || pix->width <= 0) { |
| dev_err(&mxc_isi->pdev->dev, |
| "Invalid width or height(w=%d, h=%d)\n", |
| pix->width, pix->height); |
| return -EINVAL; |
| } |
| |
| if ((pix->pixelformat == V4L2_PIX_FMT_NV12) && ((pix->width / 4) % 2)) { |
| dev_err(&mxc_isi->pdev->dev, |
| "Invalid width or height(w=%d, h=%d) for NV12\n", |
| pix->width, pix->height); |
| return -EINVAL; |
| } else if ((pix->pixelformat != V4L2_PIX_FMT_XBGR32) && (pix->width % 2)) { |
| dev_err(&mxc_isi->pdev->dev, |
| "Invalid width or height(w=%d, h=%d) for %.4s\n", |
| pix->width, pix->height, (char *)&pix->pixelformat); |
| return -EINVAL; |
| } |
| |
| frame->fmt = fmt; |
| frame->height = pix->height; |
| frame->width = pix->width; |
| |
| pix->num_planes = fmt->memplanes; |
| for (i = 0; i < pix->num_planes; i++) { |
| bpl = pix->plane_fmt[i].bytesperline; |
| |
| if ((bpl == 0) || (bpl / (fmt->depth[i] >> 3)) < pix->width) |
| pix->plane_fmt[i].bytesperline = |
| (pix->width * fmt->depth[i]) >> 3; |
| |
| if (pix->plane_fmt[i].sizeimage == 0) { |
| |
| if ((i == 1) && (pix->pixelformat == V4L2_PIX_FMT_NV12)) |
| pix->plane_fmt[i].sizeimage = |
| (pix->width * (pix->height >> 1) * fmt->depth[i] >> 3); |
| else |
| pix->plane_fmt[i].sizeimage = (pix->width * pix->height * |
| fmt->depth[i] >> 3); |
| } |
| } |
| |
| if (pix->num_planes > 1) { |
| for (i = 0; i < pix->num_planes; i++) { |
| frame->bytesperline[i] = pix->plane_fmt[i].bytesperline; |
| frame->sizeimage[i] = pix->plane_fmt[i].sizeimage; |
| } |
| } else { |
| frame->bytesperline[0] = frame->width * frame->fmt->depth[0] / 8; |
| frame->sizeimage[0] = frame->height * frame->bytesperline[0]; |
| } |
| |
| memcpy(&mxc_isi->pix, pix, sizeof(*pix)); |
| |
| set_frame_bounds(frame, pix->width, pix->height); |
| mxc_isi_m2m_config_dst(mxc_isi); |
| |
| return 0; |
| } |
| |
| static int mxc_isi_m2m_g_fmt_vid_cap(struct file *file, void *fh, |
| struct v4l2_format *f) |
| { |
| struct mxc_isi_dev *mxc_isi = video_drvdata(file); |
| struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp; |
| struct mxc_isi_frame *frame = &mxc_isi->m2m.dst_f; |
| int i; |
| |
| dev_dbg(&mxc_isi->pdev->dev, "%s\n", __func__); |
| |
| if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) |
| return -EINVAL; |
| |
| pix->width = frame->o_width; |
| pix->height = frame->o_height; |
| pix->field = V4L2_FIELD_NONE; |
| pix->pixelformat = frame->fmt->fourcc; |
| pix->colorspace = V4L2_COLORSPACE_JPEG; |
| pix->num_planes = frame->fmt->memplanes; |
| |
| for (i = 0; i < pix->num_planes; ++i) { |
| pix->plane_fmt[i].bytesperline = frame->bytesperline[i]; |
| pix->plane_fmt[i].sizeimage = frame->sizeimage[i]; |
| } |
| |
| return 0; |
| } |
| |
| static int mxc_isi_m2m_g_fmt_vid_out(struct file *file, void *fh, |
| struct v4l2_format *f) |
| { |
| struct mxc_isi_dev *mxc_isi = video_drvdata(file); |
| struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp; |
| struct mxc_isi_frame *frame = &mxc_isi->m2m.src_f; |
| int i; |
| |
| dev_dbg(&mxc_isi->pdev->dev, "%s\n", __func__); |
| |
| if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) |
| return -EINVAL; |
| |
| pix->width = frame->o_width; |
| pix->height = frame->o_height; |
| pix->field = V4L2_FIELD_NONE; |
| pix->pixelformat = frame->fmt->fourcc; |
| pix->colorspace = V4L2_COLORSPACE_JPEG; |
| pix->num_planes = frame->fmt->memplanes; |
| |
| for (i = 0; i < pix->num_planes; ++i) { |
| pix->plane_fmt[i].bytesperline = frame->bytesperline[i]; |
| pix->plane_fmt[i].sizeimage = frame->sizeimage[i]; |
| } |
| |
| return 0; |
| } |
| |
| static int mxc_isi_m2m_streamon(struct file *file, void *priv, |
| enum v4l2_buf_type type) |
| { |
| struct mxc_isi_dev *mxc_isi = video_drvdata(file); |
| struct mxc_isi_frame *src_f, *dst_f; |
| int ret; |
| |
| src_f = &mxc_isi->m2m.src_f; |
| dst_f = &mxc_isi->m2m.dst_f; |
| if ((dst_f->width > src_f->width) || |
| (dst_f->height > src_f->height)) { |
| dev_err(&mxc_isi->pdev->dev, "%s Not support upscale\n", __func__); |
| return -EINVAL; |
| } |
| |
| if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) { |
| mxc_isi->m2m.frame_count = 0; |
| mxc_isi_m2m_channel_config(mxc_isi); |
| } |
| |
| ret = v4l2_m2m_ioctl_streamon(file, priv, type); |
| |
| return ret; |
| } |
| |
| static int mxc_isi_m2m_streamoff(struct file *file, void *priv, |
| enum v4l2_buf_type type) |
| { |
| struct mxc_isi_dev *mxc_isi = video_drvdata(file); |
| int ret; |
| |
| if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) |
| mxc_isi_channel_disable(mxc_isi); |
| |
| ret = v4l2_m2m_ioctl_streamoff(file, priv, type); |
| |
| return ret; |
| } |
| |
| static const struct v4l2_ioctl_ops mxc_isi_m2m_ioctl_ops = { |
| .vidioc_querycap = mxc_isi_m2m_querycap, |
| |
| .vidioc_enum_fmt_vid_cap_mplane = mxc_isi_m2m_enum_fmt_vid_cap, |
| .vidioc_enum_fmt_vid_out_mplane = mxc_isi_m2m_enum_fmt_vid_out, |
| |
| .vidioc_try_fmt_vid_cap_mplane = mxc_isi_m2m_try_fmt_vid_cap, |
| .vidioc_try_fmt_vid_out_mplane = mxc_isi_m2m_try_fmt_vid_out, |
| |
| .vidioc_s_fmt_vid_cap_mplane = mxc_isi_m2m_s_fmt_vid_cap, |
| .vidioc_s_fmt_vid_out_mplane = mxc_isi_m2m_s_fmt_vid_out, |
| |
| .vidioc_g_fmt_vid_cap_mplane = mxc_isi_m2m_g_fmt_vid_cap, |
| .vidioc_g_fmt_vid_out_mplane = mxc_isi_m2m_g_fmt_vid_out, |
| |
| .vidioc_reqbufs = v4l2_m2m_ioctl_reqbufs, |
| .vidioc_querybuf = v4l2_m2m_ioctl_querybuf, |
| .vidioc_qbuf = v4l2_m2m_ioctl_qbuf, |
| .vidioc_dqbuf = v4l2_m2m_ioctl_dqbuf, |
| .vidioc_expbuf = v4l2_m2m_ioctl_expbuf, |
| .vidioc_prepare_buf = v4l2_m2m_ioctl_prepare_buf, |
| .vidioc_create_bufs = v4l2_m2m_ioctl_create_bufs, |
| |
| .vidioc_streamon = mxc_isi_m2m_streamon, |
| .vidioc_streamoff = mxc_isi_m2m_streamoff, |
| }; |
| |
| /* |
| * V4L2 controls handling |
| */ |
| #define ctrl_to_mxc_isi_m2m(__ctrl) \ |
| container_of((__ctrl)->handler, struct mxc_isi_dev, m2m.ctrls.handler) |
| |
| static int mxc_isi_m2m_s_ctrl(struct v4l2_ctrl *ctrl) |
| { |
| struct mxc_isi_dev *mxc_isi = ctrl_to_mxc_isi_m2m(ctrl); |
| unsigned long flags; |
| int ret = 0; |
| |
| dev_dbg(&mxc_isi->pdev->dev, "%s\n", __func__); |
| |
| if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE) |
| return 0; |
| |
| spin_lock_irqsave(&mxc_isi->slock, flags); |
| |
| switch (ctrl->id) { |
| case V4L2_CID_HFLIP: |
| if (ctrl->val < 0) { |
| ret = -EINVAL; |
| goto unlock; |
| } |
| mxc_isi->m2m.hflip = (ctrl->val > 0) ? 1 : 0; |
| break; |
| |
| case V4L2_CID_VFLIP: |
| if (ctrl->val < 0) { |
| ret = -EINVAL; |
| goto unlock; |
| } |
| mxc_isi->m2m.vflip = (ctrl->val > 0) ? 1 : 0; |
| break; |
| |
| case V4L2_CID_ALPHA_COMPONENT: |
| if (ctrl->val < 0 || ctrl->val > 255) { |
| ret = -EINVAL; |
| goto unlock; |
| } |
| mxc_isi->m2m.alpha = ctrl->val; |
| mxc_isi->m2m.alphaen = 1; |
| break; |
| |
| default: |
| dev_err(&mxc_isi->pdev->dev, "%s: Not support %d CID\n", __func__, ctrl->id); |
| ret = -EINVAL; |
| } |
| |
| unlock: |
| spin_unlock_irqrestore(&mxc_isi->slock, flags); |
| return ret; |
| } |
| |
| static const struct v4l2_ctrl_ops mxc_isi_m2m_ctrl_ops = { |
| .s_ctrl = mxc_isi_m2m_s_ctrl, |
| }; |
| |
| static int mxc_isi_m2m_ctrls_create(struct mxc_isi_dev *mxc_isi) |
| { |
| struct mxc_isi_ctrls *ctrls = &mxc_isi->m2m.ctrls; |
| struct v4l2_ctrl_handler *handler = &ctrls->handler; |
| |
| if (mxc_isi->m2m.ctrls.ready) |
| return 0; |
| |
| v4l2_ctrl_handler_init(handler, 4); |
| |
| ctrls->hflip = v4l2_ctrl_new_std(handler, &mxc_isi_m2m_ctrl_ops, |
| V4L2_CID_HFLIP, 0, 1, 1, 0); |
| ctrls->vflip = v4l2_ctrl_new_std(handler, &mxc_isi_m2m_ctrl_ops, |
| V4L2_CID_VFLIP, 0, 1, 1, 0); |
| ctrls->alpha = v4l2_ctrl_new_std(handler, &mxc_isi_m2m_ctrl_ops, |
| V4L2_CID_ALPHA_COMPONENT, 0, 0xff, 1, 0); |
| |
| if (!handler->error) |
| ctrls->ready = true; |
| |
| return handler->error; |
| |
| } |
| |
| void mxc_isi_m2m_ctrls_delete(struct mxc_isi_dev *mxc_isi) |
| { |
| struct mxc_isi_ctrls *ctrls = &mxc_isi->m2m.ctrls; |
| |
| if (ctrls->ready) { |
| v4l2_ctrl_handler_free(&ctrls->handler); |
| ctrls->ready = false; |
| ctrls->alpha = NULL; |
| } |
| } |
| |
| int mxc_isi_register_m2m_device(struct mxc_isi_dev *mxc_isi, |
| struct v4l2_device *v4l2_dev) |
| { |
| struct device *dev = &mxc_isi->pdev->dev; |
| struct video_device *vdev = &mxc_isi->m2m.vdev; |
| struct mxc_isi_m2m_dev *isi_m2m = &mxc_isi->m2m; |
| int ret = -ENOMEM; |
| |
| /* Only ISI channel0 support memory to memory */ |
| if (mxc_isi->id != 0) |
| return -EINVAL; |
| |
| dev_dbg(&mxc_isi->pdev->dev, "%s\n", __func__); |
| |
| /* m2m */ |
| isi_m2m->m2m_dev = v4l2_m2m_init(&mxc_isi_m2m_ops); |
| if (IS_ERR(isi_m2m->m2m_dev)) { |
| dev_err(dev, "%s fail to get m2m device\n", __func__); |
| return PTR_ERR(isi_m2m->m2m_dev); |
| } |
| |
| INIT_LIST_HEAD(&isi_m2m->out_active); |
| |
| /* Video device */ |
| memset(vdev, 0, sizeof(*vdev)); |
| snprintf(vdev->name, sizeof(vdev->name), "mxc_isi.%d.m2m", mxc_isi->id); |
| |
| vdev->fops = &mxc_isi_m2m_fops; |
| vdev->ioctl_ops = &mxc_isi_m2m_ioctl_ops; |
| vdev->v4l2_dev = v4l2_dev; |
| vdev->minor = -1; |
| vdev->release = video_device_release_empty; |
| vdev->vfl_dir = VFL_DIR_M2M; |
| vdev->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M; |
| |
| ret = mxc_isi_m2m_ctrls_create(mxc_isi); |
| if (ret) |
| goto free_m2m; |
| |
| ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1); |
| if (ret < 0) { |
| dev_err(dev, "%s fail to register video device\n", __func__); |
| goto ctrl_free; |
| } |
| |
| vdev->ctrl_handler = &mxc_isi->m2m.ctrls.handler; |
| video_set_drvdata(vdev, mxc_isi); |
| |
| return 0; |
| |
| ctrl_free: |
| mxc_isi_m2m_ctrls_delete(mxc_isi); |
| free_m2m: |
| v4l2_m2m_release(isi_m2m->m2m_dev); |
| return ret; |
| } |
| |
| void mxc_isi_unregister_m2m_device(struct mxc_isi_dev *mxc_isi) |
| { |
| struct video_device *vdev; |
| |
| vdev = &mxc_isi->m2m.vdev; |
| if (video_is_registered(vdev)) |
| video_unregister_device(vdev); |
| |
| v4l2_m2m_release(mxc_isi->m2m.m2m_dev); |
| } |
| |
| void mxc_isi_m2m_frame_write_done(struct mxc_isi_dev *mxc_isi) |
| { |
| struct v4l2_fh *fh; |
| struct mxc_isi_ctx *curr_mxc_ctx; |
| struct vb2_buffer *src_vb2, *dst_vb2; |
| struct mxc_isi_buffer *src_buf, *dst_buf; |
| struct v4l2_m2m_buffer *b; |
| |
| dev_dbg(&mxc_isi->pdev->dev, "%s\n", __func__); |
| |
| curr_mxc_ctx = v4l2_m2m_get_curr_priv(mxc_isi->m2m.m2m_dev); |
| if (!curr_mxc_ctx) { |
| dev_err(&mxc_isi->pdev->dev, |
| "Instance released before the end of transaction\n"); |
| return; |
| } |
| fh = &curr_mxc_ctx->fh; |
| |
| if (mxc_isi->m2m.aborting) { |
| mxc_isi_channel_disable(mxc_isi); |
| dev_warn(&mxc_isi->pdev->dev, "Aborting current job\n"); |
| goto job_finish; |
| } |
| |
| src_vb2 = v4l2_m2m_next_src_buf(fh->m2m_ctx); |
| if (!src_vb2) { |
| dev_err(&mxc_isi->pdev->dev, "No enought source buffers\n"); |
| goto job_finish; |
| } |
| src_buf = vb2_to_isi_buffer(src_vb2); |
| v4l2_m2m_src_buf_remove(fh->m2m_ctx); |
| v4l2_m2m_buf_done(to_vb2_v4l2_buffer(src_vb2), VB2_BUF_STATE_DONE); |
| |
| if (!list_empty(&mxc_isi->m2m.out_active)) { |
| dst_buf = list_first_entry(&mxc_isi->m2m.out_active, |
| struct mxc_isi_buffer, list); |
| dst_vb2 = &dst_buf->v4l2_buf.vb2_buf; |
| list_del_init(&dst_buf->list); |
| dst_buf->v4l2_buf.vb2_buf.timestamp = ktime_get_ns(); |
| v4l2_m2m_buf_done(to_vb2_v4l2_buffer(dst_vb2), VB2_BUF_STATE_DONE); |
| |
| #if 0 // for debug |
| char *s_buf, *d_buf; |
| s_buf = vb2_plane_vaddr(src_vb2, 0); |
| if (s_buf) { |
| pr_info("src: 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n", |
| s_buf[0],s_buf[1], s_buf[2],s_buf[3],s_buf[4], |
| s_buf[5],s_buf[6], s_buf[7],s_buf[8],s_buf[9]); |
| } |
| d_buf = vb2_plane_vaddr(dst_vb2, 0); |
| if (d_buf) { |
| pr_info("dst: 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n", |
| d_buf[0],d_buf[1], d_buf[2], d_buf[3],d_buf[4], |
| d_buf[5],d_buf[6], d_buf[7],d_buf[8],d_buf[9]); |
| } |
| #endif |
| } |
| mxc_isi->m2m.frame_count++; |
| |
| dst_vb2 = v4l2_m2m_next_dst_buf(fh->m2m_ctx); |
| if (dst_vb2) { |
| dst_vb2->state = VB2_BUF_STATE_ACTIVE; |
| dst_buf = vb2_to_isi_buffer(dst_vb2); |
| dst_buf->v4l2_buf.sequence = mxc_isi->m2m.frame_count; |
| mxc_isi_channel_set_m2m_out_addr(mxc_isi, dst_buf); |
| v4l2_m2m_dst_buf_remove(fh->m2m_ctx); |
| b = to_v4l2_m2m_buffer(dst_vb2); |
| list_add_tail(&b->list, &mxc_isi->m2m.out_active); |
| } |
| |
| job_finish: |
| v4l2_m2m_job_finish(mxc_isi->m2m.m2m_dev, fh->m2m_ctx); |
| } |