Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  * V4L2 deinterlacing support.
0004  *
0005  * Copyright (c) 2012 Vista Silicon S.L.
0006  * Javier Martin <javier.martin@vista-silicon.com>
0007  */
0008 
0009 #include <linux/module.h>
0010 #include <linux/slab.h>
0011 #include <linux/interrupt.h>
0012 #include <linux/dmaengine.h>
0013 #include <linux/platform_device.h>
0014 
0015 #include <media/v4l2-mem2mem.h>
0016 #include <media/v4l2-device.h>
0017 #include <media/v4l2-ioctl.h>
0018 #include <media/videobuf2-dma-contig.h>
0019 
0020 #define MEM2MEM_TEST_MODULE_NAME "mem2mem-deinterlace"
0021 
0022 MODULE_DESCRIPTION("mem2mem device which supports deinterlacing using dmaengine");
0023 MODULE_AUTHOR("Javier Martin <javier.martin@vista-silicon.com");
0024 MODULE_LICENSE("GPL");
0025 MODULE_VERSION("0.0.1");
0026 
0027 static bool debug;
0028 module_param(debug, bool, 0644);
0029 
0030 /* Flags that indicate a format can be used for capture/output */
0031 #define MEM2MEM_CAPTURE (1 << 0)
0032 #define MEM2MEM_OUTPUT  (1 << 1)
0033 
0034 #define MEM2MEM_NAME        "m2m-deinterlace"
0035 
0036 #define dprintk(dev, fmt, arg...) \
0037     v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
0038 
0039 struct deinterlace_fmt {
0040     u32 fourcc;
0041     /* Types the format can be used for */
0042     u32 types;
0043 };
0044 
0045 static struct deinterlace_fmt formats[] = {
0046     {
0047         .fourcc = V4L2_PIX_FMT_YUV420,
0048         .types  = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
0049     },
0050     {
0051         .fourcc = V4L2_PIX_FMT_YUYV,
0052         .types  = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
0053     },
0054 };
0055 
0056 #define NUM_FORMATS ARRAY_SIZE(formats)
0057 
0058 /* Per-queue, driver-specific private data */
0059 struct deinterlace_q_data {
0060     unsigned int        width;
0061     unsigned int        height;
0062     unsigned int        sizeimage;
0063     struct deinterlace_fmt  *fmt;
0064     enum v4l2_field     field;
0065 };
0066 
0067 enum {
0068     V4L2_M2M_SRC = 0,
0069     V4L2_M2M_DST = 1,
0070 };
0071 
0072 enum {
0073     YUV420_DMA_Y_ODD,
0074     YUV420_DMA_Y_EVEN,
0075     YUV420_DMA_U_ODD,
0076     YUV420_DMA_U_EVEN,
0077     YUV420_DMA_V_ODD,
0078     YUV420_DMA_V_EVEN,
0079     YUV420_DMA_Y_ODD_DOUBLING,
0080     YUV420_DMA_U_ODD_DOUBLING,
0081     YUV420_DMA_V_ODD_DOUBLING,
0082     YUYV_DMA_ODD,
0083     YUYV_DMA_EVEN,
0084     YUYV_DMA_EVEN_DOUBLING,
0085 };
0086 
0087 /* Source and destination queue data */
0088 static struct deinterlace_q_data q_data[2];
0089 
0090 static struct deinterlace_q_data *get_q_data(enum v4l2_buf_type type)
0091 {
0092     switch (type) {
0093     case V4L2_BUF_TYPE_VIDEO_OUTPUT:
0094         return &q_data[V4L2_M2M_SRC];
0095     case V4L2_BUF_TYPE_VIDEO_CAPTURE:
0096         return &q_data[V4L2_M2M_DST];
0097     default:
0098         BUG();
0099     }
0100     return NULL;
0101 }
0102 
0103 static struct deinterlace_fmt *find_format(struct v4l2_format *f)
0104 {
0105     struct deinterlace_fmt *fmt;
0106     unsigned int k;
0107 
0108     for (k = 0; k < NUM_FORMATS; k++) {
0109         fmt = &formats[k];
0110         if ((fmt->types & f->type) &&
0111             (fmt->fourcc == f->fmt.pix.pixelformat))
0112             break;
0113     }
0114 
0115     if (k == NUM_FORMATS)
0116         return NULL;
0117 
0118     return &formats[k];
0119 }
0120 
0121 struct deinterlace_dev {
0122     struct v4l2_device  v4l2_dev;
0123     struct video_device vfd;
0124 
0125     atomic_t        busy;
0126     struct mutex        dev_mutex;
0127     spinlock_t      irqlock;
0128 
0129     struct dma_chan     *dma_chan;
0130 
0131     struct v4l2_m2m_dev *m2m_dev;
0132 };
0133 
0134 struct deinterlace_ctx {
0135     struct v4l2_fh      fh;
0136     struct deinterlace_dev  *dev;
0137 
0138     /* Abort requested by m2m */
0139     int         aborting;
0140     enum v4l2_colorspace    colorspace;
0141     dma_cookie_t        cookie;
0142     struct dma_interleaved_template *xt;
0143 };
0144 
0145 /*
0146  * mem2mem callbacks
0147  */
0148 static int deinterlace_job_ready(void *priv)
0149 {
0150     struct deinterlace_ctx *ctx = priv;
0151     struct deinterlace_dev *pcdev = ctx->dev;
0152 
0153     if (v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) > 0 &&
0154         v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx) > 0 &&
0155         !atomic_read(&ctx->dev->busy)) {
0156         dprintk(pcdev, "Task ready\n");
0157         return 1;
0158     }
0159 
0160     dprintk(pcdev, "Task not ready to run\n");
0161 
0162     return 0;
0163 }
0164 
0165 static void deinterlace_job_abort(void *priv)
0166 {
0167     struct deinterlace_ctx *ctx = priv;
0168     struct deinterlace_dev *pcdev = ctx->dev;
0169 
0170     ctx->aborting = 1;
0171 
0172     dprintk(pcdev, "Aborting task\n");
0173 
0174     v4l2_m2m_job_finish(pcdev->m2m_dev, ctx->fh.m2m_ctx);
0175 }
0176 
0177 static void dma_callback(void *data)
0178 {
0179     struct deinterlace_ctx *curr_ctx = data;
0180     struct deinterlace_dev *pcdev = curr_ctx->dev;
0181     struct vb2_v4l2_buffer *src_vb, *dst_vb;
0182 
0183     atomic_set(&pcdev->busy, 0);
0184 
0185     src_vb = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
0186     dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
0187 
0188     dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
0189     dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
0190     dst_vb->flags |=
0191         src_vb->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
0192     dst_vb->timecode = src_vb->timecode;
0193 
0194     v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
0195     v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
0196 
0197     v4l2_m2m_job_finish(pcdev->m2m_dev, curr_ctx->fh.m2m_ctx);
0198 
0199     dprintk(pcdev, "dma transfers completed.\n");
0200 }
0201 
0202 static void deinterlace_issue_dma(struct deinterlace_ctx *ctx, int op,
0203                   int do_callback)
0204 {
0205     struct deinterlace_q_data *s_q_data;
0206     struct vb2_v4l2_buffer *src_buf, *dst_buf;
0207     struct deinterlace_dev *pcdev = ctx->dev;
0208     struct dma_chan *chan = pcdev->dma_chan;
0209     struct dma_device *dmadev = chan->device;
0210     struct dma_async_tx_descriptor *tx;
0211     unsigned int s_width, s_height;
0212     unsigned int s_size;
0213     dma_addr_t p_in, p_out;
0214     enum dma_ctrl_flags flags;
0215 
0216     src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
0217     dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
0218 
0219     s_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_OUTPUT);
0220     s_width = s_q_data->width;
0221     s_height = s_q_data->height;
0222     s_size = s_width * s_height;
0223 
0224     p_in = (dma_addr_t)vb2_dma_contig_plane_dma_addr(&src_buf->vb2_buf, 0);
0225     p_out = (dma_addr_t)vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf,
0226                               0);
0227     if (!p_in || !p_out) {
0228         v4l2_err(&pcdev->v4l2_dev,
0229              "Acquiring kernel pointers to buffers failed\n");
0230         return;
0231     }
0232 
0233     switch (op) {
0234     case YUV420_DMA_Y_ODD:
0235         ctx->xt->numf = s_height / 2;
0236         ctx->xt->sgl[0].size = s_width;
0237         ctx->xt->sgl[0].icg = s_width;
0238         ctx->xt->src_start = p_in;
0239         ctx->xt->dst_start = p_out;
0240         break;
0241     case YUV420_DMA_Y_EVEN:
0242         ctx->xt->numf = s_height / 2;
0243         ctx->xt->sgl[0].size = s_width;
0244         ctx->xt->sgl[0].icg = s_width;
0245         ctx->xt->src_start = p_in + s_size / 2;
0246         ctx->xt->dst_start = p_out + s_width;
0247         break;
0248     case YUV420_DMA_U_ODD:
0249         ctx->xt->numf = s_height / 4;
0250         ctx->xt->sgl[0].size = s_width / 2;
0251         ctx->xt->sgl[0].icg = s_width / 2;
0252         ctx->xt->src_start = p_in + s_size;
0253         ctx->xt->dst_start = p_out + s_size;
0254         break;
0255     case YUV420_DMA_U_EVEN:
0256         ctx->xt->numf = s_height / 4;
0257         ctx->xt->sgl[0].size = s_width / 2;
0258         ctx->xt->sgl[0].icg = s_width / 2;
0259         ctx->xt->src_start = p_in + (9 * s_size) / 8;
0260         ctx->xt->dst_start = p_out + s_size + s_width / 2;
0261         break;
0262     case YUV420_DMA_V_ODD:
0263         ctx->xt->numf = s_height / 4;
0264         ctx->xt->sgl[0].size = s_width / 2;
0265         ctx->xt->sgl[0].icg = s_width / 2;
0266         ctx->xt->src_start = p_in + (5 * s_size) / 4;
0267         ctx->xt->dst_start = p_out + (5 * s_size) / 4;
0268         break;
0269     case YUV420_DMA_V_EVEN:
0270         ctx->xt->numf = s_height / 4;
0271         ctx->xt->sgl[0].size = s_width / 2;
0272         ctx->xt->sgl[0].icg = s_width / 2;
0273         ctx->xt->src_start = p_in + (11 * s_size) / 8;
0274         ctx->xt->dst_start = p_out + (5 * s_size) / 4 + s_width / 2;
0275         break;
0276     case YUV420_DMA_Y_ODD_DOUBLING:
0277         ctx->xt->numf = s_height / 2;
0278         ctx->xt->sgl[0].size = s_width;
0279         ctx->xt->sgl[0].icg = s_width;
0280         ctx->xt->src_start = p_in;
0281         ctx->xt->dst_start = p_out + s_width;
0282         break;
0283     case YUV420_DMA_U_ODD_DOUBLING:
0284         ctx->xt->numf = s_height / 4;
0285         ctx->xt->sgl[0].size = s_width / 2;
0286         ctx->xt->sgl[0].icg = s_width / 2;
0287         ctx->xt->src_start = p_in + s_size;
0288         ctx->xt->dst_start = p_out + s_size + s_width / 2;
0289         break;
0290     case YUV420_DMA_V_ODD_DOUBLING:
0291         ctx->xt->numf = s_height / 4;
0292         ctx->xt->sgl[0].size = s_width / 2;
0293         ctx->xt->sgl[0].icg = s_width / 2;
0294         ctx->xt->src_start = p_in + (5 * s_size) / 4;
0295         ctx->xt->dst_start = p_out + (5 * s_size) / 4 + s_width / 2;
0296         break;
0297     case YUYV_DMA_ODD:
0298         ctx->xt->numf = s_height / 2;
0299         ctx->xt->sgl[0].size = s_width * 2;
0300         ctx->xt->sgl[0].icg = s_width * 2;
0301         ctx->xt->src_start = p_in;
0302         ctx->xt->dst_start = p_out;
0303         break;
0304     case YUYV_DMA_EVEN:
0305         ctx->xt->numf = s_height / 2;
0306         ctx->xt->sgl[0].size = s_width * 2;
0307         ctx->xt->sgl[0].icg = s_width * 2;
0308         ctx->xt->src_start = p_in + s_size;
0309         ctx->xt->dst_start = p_out + s_width * 2;
0310         break;
0311     case YUYV_DMA_EVEN_DOUBLING:
0312     default:
0313         ctx->xt->numf = s_height / 2;
0314         ctx->xt->sgl[0].size = s_width * 2;
0315         ctx->xt->sgl[0].icg = s_width * 2;
0316         ctx->xt->src_start = p_in;
0317         ctx->xt->dst_start = p_out + s_width * 2;
0318         break;
0319     }
0320 
0321     /* Common parameters for al transfers */
0322     ctx->xt->frame_size = 1;
0323     ctx->xt->dir = DMA_MEM_TO_MEM;
0324     ctx->xt->src_sgl = false;
0325     ctx->xt->dst_sgl = true;
0326     flags = DMA_CTRL_ACK | DMA_PREP_INTERRUPT;
0327 
0328     tx = dmadev->device_prep_interleaved_dma(chan, ctx->xt, flags);
0329     if (tx == NULL) {
0330         v4l2_warn(&pcdev->v4l2_dev, "DMA interleaved prep error\n");
0331         return;
0332     }
0333 
0334     if (do_callback) {
0335         tx->callback = dma_callback;
0336         tx->callback_param = ctx;
0337     }
0338 
0339     ctx->cookie = dmaengine_submit(tx);
0340     if (dma_submit_error(ctx->cookie)) {
0341         v4l2_warn(&pcdev->v4l2_dev,
0342               "DMA submit error %d with src=0x%x dst=0x%x len=0x%x\n",
0343               ctx->cookie, (unsigned)p_in, (unsigned)p_out,
0344               s_size * 3/2);
0345         return;
0346     }
0347 
0348     dma_async_issue_pending(chan);
0349 }
0350 
0351 static void deinterlace_device_run(void *priv)
0352 {
0353     struct deinterlace_ctx *ctx = priv;
0354     struct deinterlace_q_data *dst_q_data;
0355 
0356     atomic_set(&ctx->dev->busy, 1);
0357 
0358     dprintk(ctx->dev, "%s: DMA try issue.\n", __func__);
0359 
0360     dst_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_CAPTURE);
0361 
0362     /*
0363      * 4 possible field conversions are possible at the moment:
0364      *  V4L2_FIELD_SEQ_TB --> V4L2_FIELD_INTERLACED_TB:
0365      *  two separate fields in the same input buffer are interlaced
0366      *  in the output buffer using weaving. Top field comes first.
0367      *  V4L2_FIELD_SEQ_TB --> V4L2_FIELD_NONE:
0368      *  top field from the input buffer is copied to the output buffer
0369      *  using line doubling. Bottom field from the input buffer is discarded.
0370      * V4L2_FIELD_SEQ_BT --> V4L2_FIELD_INTERLACED_BT:
0371      *  two separate fields in the same input buffer are interlaced
0372      *  in the output buffer using weaving. Bottom field comes first.
0373      * V4L2_FIELD_SEQ_BT --> V4L2_FIELD_NONE:
0374      *  bottom field from the input buffer is copied to the output buffer
0375      *  using line doubling. Top field from the input buffer is discarded.
0376      */
0377     switch (dst_q_data->fmt->fourcc) {
0378     case V4L2_PIX_FMT_YUV420:
0379         switch (dst_q_data->field) {
0380         case V4L2_FIELD_INTERLACED_TB:
0381         case V4L2_FIELD_INTERLACED_BT:
0382             dprintk(ctx->dev, "%s: yuv420 interlaced tb.\n",
0383                 __func__);
0384             deinterlace_issue_dma(ctx, YUV420_DMA_Y_ODD, 0);
0385             deinterlace_issue_dma(ctx, YUV420_DMA_Y_EVEN, 0);
0386             deinterlace_issue_dma(ctx, YUV420_DMA_U_ODD, 0);
0387             deinterlace_issue_dma(ctx, YUV420_DMA_U_EVEN, 0);
0388             deinterlace_issue_dma(ctx, YUV420_DMA_V_ODD, 0);
0389             deinterlace_issue_dma(ctx, YUV420_DMA_V_EVEN, 1);
0390             break;
0391         case V4L2_FIELD_NONE:
0392         default:
0393             dprintk(ctx->dev, "%s: yuv420 interlaced line doubling.\n",
0394                 __func__);
0395             deinterlace_issue_dma(ctx, YUV420_DMA_Y_ODD, 0);
0396             deinterlace_issue_dma(ctx, YUV420_DMA_Y_ODD_DOUBLING, 0);
0397             deinterlace_issue_dma(ctx, YUV420_DMA_U_ODD, 0);
0398             deinterlace_issue_dma(ctx, YUV420_DMA_U_ODD_DOUBLING, 0);
0399             deinterlace_issue_dma(ctx, YUV420_DMA_V_ODD, 0);
0400             deinterlace_issue_dma(ctx, YUV420_DMA_V_ODD_DOUBLING, 1);
0401             break;
0402         }
0403         break;
0404     case V4L2_PIX_FMT_YUYV:
0405     default:
0406         switch (dst_q_data->field) {
0407         case V4L2_FIELD_INTERLACED_TB:
0408         case V4L2_FIELD_INTERLACED_BT:
0409             dprintk(ctx->dev, "%s: yuyv interlaced_tb.\n",
0410                 __func__);
0411             deinterlace_issue_dma(ctx, YUYV_DMA_ODD, 0);
0412             deinterlace_issue_dma(ctx, YUYV_DMA_EVEN, 1);
0413             break;
0414         case V4L2_FIELD_NONE:
0415         default:
0416             dprintk(ctx->dev, "%s: yuyv interlaced line doubling.\n",
0417                 __func__);
0418             deinterlace_issue_dma(ctx, YUYV_DMA_ODD, 0);
0419             deinterlace_issue_dma(ctx, YUYV_DMA_EVEN_DOUBLING, 1);
0420             break;
0421         }
0422         break;
0423     }
0424 
0425     dprintk(ctx->dev, "%s: DMA issue done.\n", __func__);
0426 }
0427 
0428 /*
0429  * video ioctls
0430  */
0431 static int vidioc_querycap(struct file *file, void *priv,
0432                struct v4l2_capability *cap)
0433 {
0434     strscpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver));
0435     strscpy(cap->card, MEM2MEM_NAME, sizeof(cap->card));
0436     strscpy(cap->bus_info, MEM2MEM_NAME, sizeof(cap->bus_info));
0437     return 0;
0438 }
0439 
0440 static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
0441 {
0442     int i, num;
0443     struct deinterlace_fmt *fmt;
0444 
0445     num = 0;
0446 
0447     for (i = 0; i < NUM_FORMATS; ++i) {
0448         if (formats[i].types & type) {
0449             /* index-th format of type type found ? */
0450             if (num == f->index)
0451                 break;
0452             /* Correct type but haven't reached our index yet,
0453              * just increment per-type index */
0454             ++num;
0455         }
0456     }
0457 
0458     if (i < NUM_FORMATS) {
0459         /* Format found */
0460         fmt = &formats[i];
0461         f->pixelformat = fmt->fourcc;
0462         return 0;
0463     }
0464 
0465     /* Format not found */
0466     return -EINVAL;
0467 }
0468 
0469 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
0470                    struct v4l2_fmtdesc *f)
0471 {
0472     return enum_fmt(f, MEM2MEM_CAPTURE);
0473 }
0474 
0475 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
0476                    struct v4l2_fmtdesc *f)
0477 {
0478     return enum_fmt(f, MEM2MEM_OUTPUT);
0479 }
0480 
0481 static int vidioc_g_fmt(struct deinterlace_ctx *ctx, struct v4l2_format *f)
0482 {
0483     struct vb2_queue *vq;
0484     struct deinterlace_q_data *q_data;
0485 
0486     vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
0487     if (!vq)
0488         return -EINVAL;
0489 
0490     q_data = get_q_data(f->type);
0491 
0492     f->fmt.pix.width    = q_data->width;
0493     f->fmt.pix.height   = q_data->height;
0494     f->fmt.pix.field    = q_data->field;
0495     f->fmt.pix.pixelformat  = q_data->fmt->fourcc;
0496 
0497     switch (q_data->fmt->fourcc) {
0498     case V4L2_PIX_FMT_YUV420:
0499         f->fmt.pix.bytesperline = q_data->width * 3 / 2;
0500         break;
0501     case V4L2_PIX_FMT_YUYV:
0502     default:
0503         f->fmt.pix.bytesperline = q_data->width * 2;
0504     }
0505 
0506     f->fmt.pix.sizeimage    = q_data->sizeimage;
0507     f->fmt.pix.colorspace   = ctx->colorspace;
0508 
0509     return 0;
0510 }
0511 
0512 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
0513                 struct v4l2_format *f)
0514 {
0515     return vidioc_g_fmt(priv, f);
0516 }
0517 
0518 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
0519                 struct v4l2_format *f)
0520 {
0521     return vidioc_g_fmt(priv, f);
0522 }
0523 
0524 static int vidioc_try_fmt(struct v4l2_format *f, struct deinterlace_fmt *fmt)
0525 {
0526     switch (f->fmt.pix.pixelformat) {
0527     case V4L2_PIX_FMT_YUV420:
0528         f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2;
0529         break;
0530     case V4L2_PIX_FMT_YUYV:
0531     default:
0532         f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
0533     }
0534     f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
0535 
0536     return 0;
0537 }
0538 
0539 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
0540                   struct v4l2_format *f)
0541 {
0542     struct deinterlace_fmt *fmt;
0543     struct deinterlace_ctx *ctx = priv;
0544 
0545     fmt = find_format(f);
0546     if (!fmt || !(fmt->types & MEM2MEM_CAPTURE))
0547         f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
0548 
0549     f->fmt.pix.colorspace = ctx->colorspace;
0550 
0551     if (f->fmt.pix.field != V4L2_FIELD_INTERLACED_TB &&
0552         f->fmt.pix.field != V4L2_FIELD_INTERLACED_BT &&
0553         f->fmt.pix.field != V4L2_FIELD_NONE)
0554         f->fmt.pix.field = V4L2_FIELD_INTERLACED_TB;
0555 
0556     return vidioc_try_fmt(f, fmt);
0557 }
0558 
0559 static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
0560                   struct v4l2_format *f)
0561 {
0562     struct deinterlace_fmt *fmt;
0563 
0564     fmt = find_format(f);
0565     if (!fmt || !(fmt->types & MEM2MEM_OUTPUT))
0566         f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
0567 
0568     if (!f->fmt.pix.colorspace)
0569         f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
0570 
0571     if (f->fmt.pix.field != V4L2_FIELD_SEQ_TB &&
0572         f->fmt.pix.field != V4L2_FIELD_SEQ_BT)
0573         f->fmt.pix.field = V4L2_FIELD_SEQ_TB;
0574 
0575     return vidioc_try_fmt(f, fmt);
0576 }
0577 
0578 static int vidioc_s_fmt(struct deinterlace_ctx *ctx, struct v4l2_format *f)
0579 {
0580     struct deinterlace_q_data *q_data;
0581     struct vb2_queue *vq;
0582 
0583     vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
0584     if (!vq)
0585         return -EINVAL;
0586 
0587     q_data = get_q_data(f->type);
0588     if (!q_data)
0589         return -EINVAL;
0590 
0591     if (vb2_is_busy(vq)) {
0592         v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
0593         return -EBUSY;
0594     }
0595 
0596     q_data->fmt = find_format(f);
0597     if (!q_data->fmt) {
0598         v4l2_err(&ctx->dev->v4l2_dev,
0599              "Couldn't set format type %d, wxh: %dx%d. fmt: %d, field: %d\n",
0600             f->type, f->fmt.pix.width, f->fmt.pix.height,
0601             f->fmt.pix.pixelformat, f->fmt.pix.field);
0602         return -EINVAL;
0603     }
0604 
0605     q_data->width       = f->fmt.pix.width;
0606     q_data->height      = f->fmt.pix.height;
0607     q_data->field       = f->fmt.pix.field;
0608 
0609     switch (f->fmt.pix.pixelformat) {
0610     case V4L2_PIX_FMT_YUV420:
0611         f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2;
0612         q_data->sizeimage = (q_data->width * q_data->height * 3) / 2;
0613         break;
0614     case V4L2_PIX_FMT_YUYV:
0615     default:
0616         f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
0617         q_data->sizeimage = q_data->width * q_data->height * 2;
0618     }
0619 
0620     dprintk(ctx->dev,
0621         "Setting format for type %d, wxh: %dx%d, fmt: %d, field: %d\n",
0622         f->type, q_data->width, q_data->height, q_data->fmt->fourcc,
0623         q_data->field);
0624 
0625     return 0;
0626 }
0627 
0628 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
0629                 struct v4l2_format *f)
0630 {
0631     int ret;
0632 
0633     ret = vidioc_try_fmt_vid_cap(file, priv, f);
0634     if (ret)
0635         return ret;
0636     return vidioc_s_fmt(priv, f);
0637 }
0638 
0639 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
0640                 struct v4l2_format *f)
0641 {
0642     struct deinterlace_ctx *ctx = priv;
0643     int ret;
0644 
0645     ret = vidioc_try_fmt_vid_out(file, priv, f);
0646     if (ret)
0647         return ret;
0648 
0649     ret = vidioc_s_fmt(priv, f);
0650     if (!ret)
0651         ctx->colorspace = f->fmt.pix.colorspace;
0652 
0653     return ret;
0654 }
0655 
0656 static int vidioc_streamon(struct file *file, void *priv,
0657                enum v4l2_buf_type type)
0658 {
0659     struct deinterlace_q_data *s_q_data, *d_q_data;
0660     struct deinterlace_ctx *ctx = priv;
0661 
0662     s_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_OUTPUT);
0663     d_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_CAPTURE);
0664 
0665     /* Check that src and dst queues have the same pix format */
0666     if (s_q_data->fmt->fourcc != d_q_data->fmt->fourcc) {
0667         v4l2_err(&ctx->dev->v4l2_dev,
0668              "src and dst formats don't match.\n");
0669         return -EINVAL;
0670     }
0671 
0672     /* Check that input and output deinterlacing types are compatible */
0673     switch (s_q_data->field) {
0674     case V4L2_FIELD_SEQ_BT:
0675         if (d_q_data->field != V4L2_FIELD_NONE &&
0676             d_q_data->field != V4L2_FIELD_INTERLACED_BT) {
0677             v4l2_err(&ctx->dev->v4l2_dev,
0678              "src and dst field conversion [(%d)->(%d)] not supported.\n",
0679                 s_q_data->field, d_q_data->field);
0680             return -EINVAL;
0681         }
0682         break;
0683     case V4L2_FIELD_SEQ_TB:
0684         if (d_q_data->field != V4L2_FIELD_NONE &&
0685             d_q_data->field != V4L2_FIELD_INTERLACED_TB) {
0686             v4l2_err(&ctx->dev->v4l2_dev,
0687              "src and dst field conversion [(%d)->(%d)] not supported.\n",
0688                 s_q_data->field, d_q_data->field);
0689             return -EINVAL;
0690         }
0691         break;
0692     default:
0693         return -EINVAL;
0694     }
0695 
0696     return v4l2_m2m_streamon(file, ctx->fh.m2m_ctx, type);
0697 }
0698 
0699 static const struct v4l2_ioctl_ops deinterlace_ioctl_ops = {
0700     .vidioc_querycap    = vidioc_querycap,
0701 
0702     .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
0703     .vidioc_g_fmt_vid_cap   = vidioc_g_fmt_vid_cap,
0704     .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
0705     .vidioc_s_fmt_vid_cap   = vidioc_s_fmt_vid_cap,
0706 
0707     .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
0708     .vidioc_g_fmt_vid_out   = vidioc_g_fmt_vid_out,
0709     .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,
0710     .vidioc_s_fmt_vid_out   = vidioc_s_fmt_vid_out,
0711 
0712     .vidioc_reqbufs     = v4l2_m2m_ioctl_reqbufs,
0713     .vidioc_querybuf    = v4l2_m2m_ioctl_querybuf,
0714     .vidioc_qbuf        = v4l2_m2m_ioctl_qbuf,
0715     .vidioc_dqbuf       = v4l2_m2m_ioctl_dqbuf,
0716     .vidioc_prepare_buf = v4l2_m2m_ioctl_prepare_buf,
0717     .vidioc_expbuf      = v4l2_m2m_ioctl_expbuf,
0718 
0719     .vidioc_streamon    = vidioc_streamon,
0720     .vidioc_streamoff   = v4l2_m2m_ioctl_streamoff,
0721 };
0722 
0723 
0724 /*
0725  * Queue operations
0726  */
0727 struct vb2_dc_conf {
0728     struct device           *dev;
0729 };
0730 
0731 static int deinterlace_queue_setup(struct vb2_queue *vq,
0732                 unsigned int *nbuffers, unsigned int *nplanes,
0733                 unsigned int sizes[], struct device *alloc_devs[])
0734 {
0735     struct deinterlace_ctx *ctx = vb2_get_drv_priv(vq);
0736     struct deinterlace_q_data *q_data;
0737     unsigned int size, count = *nbuffers;
0738 
0739     q_data = get_q_data(vq->type);
0740 
0741     switch (q_data->fmt->fourcc) {
0742     case V4L2_PIX_FMT_YUV420:
0743         size = q_data->width * q_data->height * 3 / 2;
0744         break;
0745     case V4L2_PIX_FMT_YUYV:
0746     default:
0747         size = q_data->width * q_data->height * 2;
0748     }
0749 
0750     *nplanes = 1;
0751     *nbuffers = count;
0752     sizes[0] = size;
0753 
0754     dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n", count, size);
0755 
0756     return 0;
0757 }
0758 
0759 static int deinterlace_buf_prepare(struct vb2_buffer *vb)
0760 {
0761     struct deinterlace_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
0762     struct deinterlace_q_data *q_data;
0763 
0764     dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
0765 
0766     q_data = get_q_data(vb->vb2_queue->type);
0767 
0768     if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
0769         dprintk(ctx->dev, "%s data will not fit into plane (%lu < %lu)\n",
0770             __func__, vb2_plane_size(vb, 0), (long)q_data->sizeimage);
0771         return -EINVAL;
0772     }
0773 
0774     vb2_set_plane_payload(vb, 0, q_data->sizeimage);
0775 
0776     return 0;
0777 }
0778 
0779 static void deinterlace_buf_queue(struct vb2_buffer *vb)
0780 {
0781     struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
0782     struct deinterlace_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
0783 
0784     v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
0785 }
0786 
0787 static const struct vb2_ops deinterlace_qops = {
0788     .queue_setup     = deinterlace_queue_setup,
0789     .buf_prepare     = deinterlace_buf_prepare,
0790     .buf_queue   = deinterlace_buf_queue,
0791     .wait_prepare    = vb2_ops_wait_prepare,
0792     .wait_finish     = vb2_ops_wait_finish,
0793 };
0794 
0795 static int queue_init(void *priv, struct vb2_queue *src_vq,
0796               struct vb2_queue *dst_vq)
0797 {
0798     struct deinterlace_ctx *ctx = priv;
0799     int ret;
0800 
0801     src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
0802     src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
0803     src_vq->drv_priv = ctx;
0804     src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
0805     src_vq->ops = &deinterlace_qops;
0806     src_vq->mem_ops = &vb2_dma_contig_memops;
0807     src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
0808     src_vq->dev = ctx->dev->v4l2_dev.dev;
0809     src_vq->lock = &ctx->dev->dev_mutex;
0810     q_data[V4L2_M2M_SRC].fmt = &formats[0];
0811     q_data[V4L2_M2M_SRC].width = 640;
0812     q_data[V4L2_M2M_SRC].height = 480;
0813     q_data[V4L2_M2M_SRC].sizeimage = (640 * 480 * 3) / 2;
0814     q_data[V4L2_M2M_SRC].field = V4L2_FIELD_SEQ_TB;
0815 
0816     ret = vb2_queue_init(src_vq);
0817     if (ret)
0818         return ret;
0819 
0820     dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
0821     dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
0822     dst_vq->drv_priv = ctx;
0823     dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
0824     dst_vq->ops = &deinterlace_qops;
0825     dst_vq->mem_ops = &vb2_dma_contig_memops;
0826     dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
0827     dst_vq->dev = ctx->dev->v4l2_dev.dev;
0828     dst_vq->lock = &ctx->dev->dev_mutex;
0829     q_data[V4L2_M2M_DST].fmt = &formats[0];
0830     q_data[V4L2_M2M_DST].width = 640;
0831     q_data[V4L2_M2M_DST].height = 480;
0832     q_data[V4L2_M2M_DST].sizeimage = (640 * 480 * 3) / 2;
0833     q_data[V4L2_M2M_SRC].field = V4L2_FIELD_INTERLACED_TB;
0834 
0835     return vb2_queue_init(dst_vq);
0836 }
0837 
0838 /*
0839  * File operations
0840  */
0841 static int deinterlace_open(struct file *file)
0842 {
0843     struct deinterlace_dev *pcdev = video_drvdata(file);
0844     struct deinterlace_ctx *ctx = NULL;
0845 
0846     ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
0847     if (!ctx)
0848         return -ENOMEM;
0849 
0850     v4l2_fh_init(&ctx->fh, video_devdata(file));
0851     file->private_data = &ctx->fh;
0852     ctx->dev = pcdev;
0853 
0854     ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(pcdev->m2m_dev, ctx, &queue_init);
0855     if (IS_ERR(ctx->fh.m2m_ctx)) {
0856         int ret = PTR_ERR(ctx->fh.m2m_ctx);
0857 
0858         kfree(ctx);
0859         return ret;
0860     }
0861 
0862     ctx->xt = kzalloc(sizeof(struct dma_interleaved_template) +
0863                 sizeof(struct data_chunk), GFP_KERNEL);
0864     if (!ctx->xt) {
0865         kfree(ctx);
0866         return -ENOMEM;
0867     }
0868 
0869     ctx->colorspace = V4L2_COLORSPACE_REC709;
0870     v4l2_fh_add(&ctx->fh);
0871 
0872     dprintk(pcdev, "Created instance %p, m2m_ctx: %p\n",
0873         ctx, ctx->fh.m2m_ctx);
0874 
0875     return 0;
0876 }
0877 
0878 static int deinterlace_release(struct file *file)
0879 {
0880     struct deinterlace_dev *pcdev = video_drvdata(file);
0881     struct deinterlace_ctx *ctx = file->private_data;
0882 
0883     dprintk(pcdev, "Releasing instance %p\n", ctx);
0884 
0885     v4l2_fh_del(&ctx->fh);
0886     v4l2_fh_exit(&ctx->fh);
0887     v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
0888     kfree(ctx->xt);
0889     kfree(ctx);
0890 
0891     return 0;
0892 }
0893 
0894 static const struct v4l2_file_operations deinterlace_fops = {
0895     .owner      = THIS_MODULE,
0896     .open       = deinterlace_open,
0897     .release    = deinterlace_release,
0898     .poll       = v4l2_m2m_fop_poll,
0899     .unlocked_ioctl = video_ioctl2,
0900     .mmap       = v4l2_m2m_fop_mmap,
0901 };
0902 
0903 static const struct video_device deinterlace_videodev = {
0904     .name       = MEM2MEM_NAME,
0905     .fops       = &deinterlace_fops,
0906     .ioctl_ops  = &deinterlace_ioctl_ops,
0907     .minor      = -1,
0908     .release    = video_device_release_empty,
0909     .vfl_dir    = VFL_DIR_M2M,
0910     .device_caps    = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING,
0911 };
0912 
0913 static const struct v4l2_m2m_ops m2m_ops = {
0914     .device_run = deinterlace_device_run,
0915     .job_ready  = deinterlace_job_ready,
0916     .job_abort  = deinterlace_job_abort,
0917 };
0918 
0919 static int deinterlace_probe(struct platform_device *pdev)
0920 {
0921     struct deinterlace_dev *pcdev;
0922     struct video_device *vfd;
0923     dma_cap_mask_t mask;
0924     int ret = 0;
0925 
0926     pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL);
0927     if (!pcdev)
0928         return -ENOMEM;
0929 
0930     spin_lock_init(&pcdev->irqlock);
0931 
0932     dma_cap_zero(mask);
0933     dma_cap_set(DMA_INTERLEAVE, mask);
0934     pcdev->dma_chan = dma_request_channel(mask, NULL, pcdev);
0935     if (!pcdev->dma_chan)
0936         return -ENODEV;
0937 
0938     if (!dma_has_cap(DMA_INTERLEAVE, pcdev->dma_chan->device->cap_mask)) {
0939         dev_err(&pdev->dev, "DMA does not support INTERLEAVE\n");
0940         ret = -ENODEV;
0941         goto rel_dma;
0942     }
0943 
0944     ret = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev);
0945     if (ret)
0946         goto rel_dma;
0947 
0948     atomic_set(&pcdev->busy, 0);
0949     mutex_init(&pcdev->dev_mutex);
0950 
0951     vfd = &pcdev->vfd;
0952     *vfd = deinterlace_videodev;
0953     vfd->lock = &pcdev->dev_mutex;
0954     vfd->v4l2_dev = &pcdev->v4l2_dev;
0955 
0956     ret = video_register_device(vfd, VFL_TYPE_VIDEO, 0);
0957     if (ret) {
0958         v4l2_err(&pcdev->v4l2_dev, "Failed to register video device\n");
0959         goto unreg_dev;
0960     }
0961 
0962     video_set_drvdata(vfd, pcdev);
0963     v4l2_info(&pcdev->v4l2_dev, MEM2MEM_TEST_MODULE_NAME
0964             " Device registered as /dev/video%d\n", vfd->num);
0965 
0966     platform_set_drvdata(pdev, pcdev);
0967 
0968     pcdev->m2m_dev = v4l2_m2m_init(&m2m_ops);
0969     if (IS_ERR(pcdev->m2m_dev)) {
0970         v4l2_err(&pcdev->v4l2_dev, "Failed to init mem2mem device\n");
0971         ret = PTR_ERR(pcdev->m2m_dev);
0972         goto err_m2m;
0973     }
0974 
0975     return 0;
0976 
0977 err_m2m:
0978     video_unregister_device(&pcdev->vfd);
0979 unreg_dev:
0980     v4l2_device_unregister(&pcdev->v4l2_dev);
0981 rel_dma:
0982     dma_release_channel(pcdev->dma_chan);
0983 
0984     return ret;
0985 }
0986 
0987 static int deinterlace_remove(struct platform_device *pdev)
0988 {
0989     struct deinterlace_dev *pcdev = platform_get_drvdata(pdev);
0990 
0991     v4l2_info(&pcdev->v4l2_dev, "Removing " MEM2MEM_TEST_MODULE_NAME);
0992     v4l2_m2m_release(pcdev->m2m_dev);
0993     video_unregister_device(&pcdev->vfd);
0994     v4l2_device_unregister(&pcdev->v4l2_dev);
0995     dma_release_channel(pcdev->dma_chan);
0996 
0997     return 0;
0998 }
0999 
1000 static struct platform_driver deinterlace_pdrv = {
1001     .probe      = deinterlace_probe,
1002     .remove     = deinterlace_remove,
1003     .driver     = {
1004         .name   = MEM2MEM_NAME,
1005     },
1006 };
1007 module_platform_driver(deinterlace_pdrv);
1008
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!