drivers/media/platform/mxc/output/mxc_pxp_v4l2.c

   1 /*
   2  * Copyright (C) 2010-2014 Freescale Semiconductor, Inc.
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program; if not, write to the Free Software
  16  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  17  *
  18  */
  19 /*
  20  * Based on STMP378X PxP driver
  21  * Copyright 2008-2009 Embedded Alley Solutions, Inc All Rights Reserved.
  22  */
  23
  24 #include <linux/dma-mapping.h>
  25 #include <linux/fb.h>
  26 #include <linux/init.h>
  27 #include <linux/interrupt.h>
  28 #include <linux/io.h>
  29 #include <linux/kernel.h>
  30 #include <linux/module.h>
  31 #include <linux/mutex.h>
  32 #include <linux/platform_device.h>
  33 #include <linux/vmalloc.h>
  34 #include <linux/videodev2.h>
  35 #include <linux/dmaengine.h>
  36 #include <linux/pxp_dma.h>
  37 #include <linux/delay.h>
  38 #include <linux/console.h>
  39 #include <linux/mxcfb.h>
  40
  41 #include <media/videobuf-dma-contig.h>
  42 #include <media/v4l2-common.h>
  43 #include <media/v4l2-dev.h>
  44 #include <media/v4l2-ioctl.h>
  45
  46 #include "mxc_pxp_v4l2.h"
  47
  48 #define PXP_DRIVER_NAME                 "pxp-v4l2"
  49 #define PXP_DRIVER_MAJOR                2
  50 #define PXP_DRIVER_MINOR                0
  51
  52 #define PXP_DEF_BUFS                    2
  53 #define PXP_MIN_PIX                     8
  54
  55 #define V4L2_OUTPUT_TYPE_INTERNAL       4
  56
  57 static int video_nr = -1;       /* -1 ==> auto assign */
  58 static struct pxp_data_format pxp_s0_formats[] = {
  59         {
  60                 .name = "24-bit RGB",
  61                 .bpp = 4,
  62                 .fourcc = V4L2_PIX_FMT_RGB24,
  63                 .colorspace = V4L2_COLORSPACE_SRGB,
  64         }, {
  65                 .name = "16-bit RGB 5:6:5",
  66                 .bpp = 2,
  67                 .fourcc = V4L2_PIX_FMT_RGB565,
  68                 .colorspace = V4L2_COLORSPACE_SRGB,
  69         }, {
  70                 .name = "16-bit RGB 5:5:5",
  71                 .bpp = 2,
  72                 .fourcc = V4L2_PIX_FMT_RGB555,
  73                 .colorspace = V4L2_COLORSPACE_SRGB,
  74         }, {
  75                 .name = "YUV 4:2:0 Planar",
  76                 .bpp = 2,
  77                 .fourcc = V4L2_PIX_FMT_YUV420,
  78                 .colorspace = V4L2_COLORSPACE_JPEG,
  79         }, {
  80                 .name = "YUV 4:2:2 Planar",
  81                 .bpp = 2,
  82                 .fourcc = V4L2_PIX_FMT_YUV422P,
  83                 .colorspace = V4L2_COLORSPACE_JPEG,
  84         }, {
  85                 .name = "UYVY",
  86                 .bpp = 2,
  87                 .fourcc = V4L2_PIX_FMT_UYVY,
  88                 .colorspace = V4L2_COLORSPACE_JPEG,
  89         },
  90 };
  91
  92 static unsigned int v4l2_fmt_to_pxp_fmt(u32 v4l2_pix_fmt)
  93 {
  94         u32 pxp_fmt = 0;
  95
  96         if (v4l2_pix_fmt == V4L2_PIX_FMT_RGB24)
  97                 pxp_fmt = PXP_PIX_FMT_RGB24;
  98         else if (v4l2_pix_fmt == V4L2_PIX_FMT_RGB565)
  99                 pxp_fmt = PXP_PIX_FMT_RGB565;
 100         else if (v4l2_pix_fmt == V4L2_PIX_FMT_RGB555)
 101                 pxp_fmt = PXP_PIX_FMT_RGB555;
 102         else if (v4l2_pix_fmt == V4L2_PIX_FMT_RGB555)
 103                 pxp_fmt = PXP_PIX_FMT_RGB555;
 104         else if (v4l2_pix_fmt == V4L2_PIX_FMT_YUV420)
 105                 pxp_fmt = PXP_PIX_FMT_YUV420P;
 106         else if (v4l2_pix_fmt == V4L2_PIX_FMT_YUV422P)
 107                 pxp_fmt = PXP_PIX_FMT_YUV422P;
 108         else if (v4l2_pix_fmt == V4L2_PIX_FMT_UYVY)
 109                 pxp_fmt = PXP_PIX_FMT_UYVY;
 110
 111         return pxp_fmt;
 112 }
 113 struct v4l2_queryctrl pxp_controls[] = {
 114         {
 115                 .id             = V4L2_CID_HFLIP,
 116                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
 117                 .name           = "Horizontal Flip",
 118                 .minimum        = 0,
 119                 .maximum        = 1,
 120                 .step           = 1,
 121                 .default_value  = 0,
 122                 .flags          = 0,
 123         }, {
 124                 .id             = V4L2_CID_VFLIP,
 125                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
 126                 .name           = "Vertical Flip",
 127                 .minimum        = 0,
 128                 .maximum        = 1,
 129                 .step           = 1,
 130                 .default_value  = 0,
 131                 .flags          = 0,
 132         }, {
 133                 .id             = V4L2_CID_PRIVATE_BASE,
 134                 .type           = V4L2_CTRL_TYPE_INTEGER,
 135                 .name           = "Rotation",
 136                 .minimum        = 0,
 137                 .maximum        = 270,
 138                 .step           = 90,
 139                 .default_value  = 0,
 140                 .flags          = 0,
 141         }, {
 142                 .id             = V4L2_CID_PRIVATE_BASE + 1,
 143                 .name           = "Background Color",
 144                 .minimum        = 0,
 145                 .maximum        = 0xFFFFFF,
 146                 .step           = 1,
 147                 .default_value  = 0,
 148                 .flags          = 0,
 149                 .type           = V4L2_CTRL_TYPE_INTEGER,
 150         }, {
 151                 .id             = V4L2_CID_PRIVATE_BASE + 2,
 152                 .name           = "Set S0 Chromakey",
 153                 .minimum        = -1,
 154                 .maximum        = 0xFFFFFF,
 155                 .step           = 1,
 156                 .default_value  = -1,
 157                 .flags          = 0,
 158                 .type           = V4L2_CTRL_TYPE_INTEGER,
 159         }, {
 160                 .id             = V4L2_CID_PRIVATE_BASE + 3,
 161                 .name           = "YUV Colorspace",
 162                 .minimum        = 0,
 163                 .maximum        = 1,
 164                 .step           = 1,
 165                 .default_value  = 0,
 166                 .flags          = 0,
 167                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
 168         },
 169 };
 170
 171 static void free_dma_buf(struct pxps *pxp, struct dma_mem *buf)
 172 {
 173         dma_free_coherent(&pxp->pdev->dev, buf->size, buf->vaddr, buf->paddr);
 174         dev_dbg(&pxp->pdev->dev,
 175                         "free dma size:0x%x, paddr:0x%x\n",
 176                         buf->size, buf->paddr);
 177         memset(buf, 0, sizeof(*buf));
 178 }
 179
 180 static int alloc_dma_buf(struct pxps *pxp, struct dma_mem *buf)
 181 {
 182
 183         buf->vaddr = dma_alloc_coherent(&pxp->pdev->dev, buf->size, &buf->paddr,
 184                                                 GFP_DMA | GFP_KERNEL);
 185         if (!buf->vaddr) {
 186                 dev_err(&pxp->pdev->dev,
 187                         "cannot get dma buf size:0x%x\n", buf->size);
 188                 return -ENOMEM;
 189         }
 190         dev_dbg(&pxp->pdev->dev,
 191                 "alloc dma buf size:0x%x, paddr:0x%x\n", buf->size, buf->paddr);
 192         return 0;
 193 }
 194
 195 /* callback function */
 196 static void video_dma_done(void *arg)
 197 {
 198         struct pxp_tx_desc *tx_desc = to_tx_desc(arg);
 199         struct dma_chan *chan = tx_desc->txd.chan;
 200         struct pxp_channel *pxp_chan = to_pxp_channel(chan);
 201         struct pxps *pxp = pxp_chan->client;
 202         struct videobuf_buffer *vb;
 203
 204         dev_dbg(chan->device->dev, "callback cookie %d, active DMA 0x%08x\n",
 205                         tx_desc->txd.cookie,
 206                         pxp->active ? sg_dma_address(&pxp->active->sg[0]) : 0);
 207
 208         spin_lock(&pxp->lock);
 209         if (pxp->active) {
 210                 vb = &pxp->active->vb;
 211
 212                 list_del_init(&vb->queue);
 213                 vb->state = VIDEOBUF_DONE;
 214                 do_gettimeofday(&vb->ts);
 215                 vb->field_count++;
 216                 wake_up(&vb->done);
 217         }
 218
 219         if (list_empty(&pxp->outq)) {
 220                 pxp->active = NULL;
 221                 spin_unlock(&pxp->lock);
 222
 223                 return;
 224         }
 225
 226         pxp->active = list_entry(pxp->outq.next,
 227                                      struct pxp_buffer, vb.queue);
 228         pxp->active->vb.state = VIDEOBUF_ACTIVE;
 229         spin_unlock(&pxp->lock);
 230 }
 231
 232 static int acquire_dma_channel(struct pxps *pxp)
 233 {
 234         dma_cap_mask_t mask;
 235         struct dma_chan *chan;
 236         struct pxp_channel **pchan = &pxp->pxp_channel[0];
 237
 238         if (*pchan) {
 239                 struct videobuf_buffer *vb, *_vb;
 240                 dma_release_channel(&(*pchan)->dma_chan);
 241                 *pchan = NULL;
 242                 pxp->active = NULL;
 243                 list_for_each_entry_safe(vb, _vb, &pxp->outq, queue) {
 244                         list_del_init(&vb->queue);
 245                         vb->state = VIDEOBUF_ERROR;
 246                         wake_up(&vb->done);
 247                 }
 248         }
 249
 250         dma_cap_zero(mask);
 251         dma_cap_set(DMA_SLAVE, mask);
 252         dma_cap_set(DMA_PRIVATE, mask);
 253         chan = dma_request_channel(mask, NULL, NULL);
 254         if (!chan)
 255                 return -EBUSY;
 256
 257         *pchan = to_pxp_channel(chan);
 258         (*pchan)->client = pxp;
 259
 260         return 0;
 261 }
 262
 263 static int _get_fbinfo(struct fb_info **fbi)
 264 {
 265         int i;
 266         for (i = 0; i < num_registered_fb; i++) {
 267                 char *idstr = registered_fb[i]->fix.id;
 268                 if (strcmp(idstr, "mxs") == 0) {
 269                         *fbi = registered_fb[i];
 270                         return 0;
 271                 }
 272         }
 273
 274         return -ENODEV;
 275 }
 276
 277 static int pxp_set_fbinfo(struct pxps *pxp)
 278 {
 279         struct v4l2_framebuffer *fb = &pxp->fb;
 280         int err;
 281
 282         err = _get_fbinfo(&pxp->fbi);
 283         if (err)
 284                 return err;
 285
 286         fb->fmt.width = pxp->fbi->var.xres;
 287         fb->fmt.height = pxp->fbi->var.yres;
 288         pxp->pxp_conf.out_param.stride = pxp->fbi->var.xres;
 289         if (pxp->fbi->var.bits_per_pixel == 16)
 290                 fb->fmt.pixelformat = V4L2_PIX_FMT_RGB565;
 291         else
 292                 fb->fmt.pixelformat = V4L2_PIX_FMT_RGB24;
 293
 294         fb->base = (void *)pxp->fbi->fix.smem_start;
 295
 296         return 0;
 297 }
 298
 299 static int _get_cur_fb_blank(struct pxps *pxp)
 300 {
 301         struct fb_info *fbi;
 302         mm_segment_t old_fs;
 303         int err = 0;
 304
 305         err = _get_fbinfo(&fbi);
 306         if (err)
 307                 return err;
 308
 309         if (fbi->fbops->fb_ioctl) {
 310                 old_fs = get_fs();
 311                 set_fs(KERNEL_DS);
 312                 err = fbi->fbops->fb_ioctl(fbi, MXCFB_GET_FB_BLANK,
 313                                 (unsigned int)(&pxp->fb_blank));
 314                 set_fs(old_fs);
 315         }
 316
 317         return err;
 318 }
 319
 320 static int pxp_show_buf(struct pxps *pxp, unsigned long paddr)
 321 {
 322         struct fb_info *fbi = pxp->fbi;
 323         int ret = -EINVAL;
 324
 325         if (paddr == 0) {
 326                 dev_err(&pxp->pdev->dev, "Invalid paddr\n");
 327                 return ret;
 328         }
 329
 330         console_lock();
 331         fbi->fix.smem_start = paddr;
 332         ret = fb_pan_display(fbi, &fbi->var);
 333         console_unlock();
 334
 335         return ret;
 336 }
 337
 338 static int set_fb_blank(int blank)
 339 {
 340         struct fb_info *fbi;
 341         int err = 0;
 342
 343         err = _get_fbinfo(&fbi);
 344         if (err)
 345                 return err;
 346
 347         console_lock();
 348         fb_blank(fbi, blank);
 349         console_unlock();
 350
 351         return err;
 352 }
 353
 354 static int pxp_set_cstate(struct pxps *pxp, struct v4l2_control *vc)
 355 {
 356
 357         if (vc->id == V4L2_CID_HFLIP) {
 358                 pxp->pxp_conf.proc_data.hflip = vc->value;
 359         } else if (vc->id == V4L2_CID_VFLIP) {
 360                 pxp->pxp_conf.proc_data.vflip = vc->value;
 361         } else if (vc->id == V4L2_CID_PRIVATE_BASE) {
 362                 if (vc->value % 90)
 363                         return -ERANGE;
 364                 pxp->pxp_conf.proc_data.rotate = vc->value;
 365         } else if (vc->id == V4L2_CID_PRIVATE_BASE + 1) {
 366                 pxp->pxp_conf.proc_data.bgcolor = vc->value;
 367         } else if (vc->id == V4L2_CID_PRIVATE_BASE + 2) {
 368                 pxp->pxp_conf.s0_param.color_key = vc->value;
 369         } else if (vc->id == V4L2_CID_PRIVATE_BASE + 3) {
 370                 pxp->pxp_conf.proc_data.yuv = vc->value;
 371         }
 372
 373         return 0;
 374 }
 375
 376 static int pxp_get_cstate(struct pxps *pxp, struct v4l2_control *vc)
 377 {
 378         if (vc->id == V4L2_CID_HFLIP)
 379                 vc->value = pxp->pxp_conf.proc_data.hflip;
 380         else if (vc->id == V4L2_CID_VFLIP)
 381                 vc->value = pxp->pxp_conf.proc_data.vflip;
 382         else if (vc->id == V4L2_CID_PRIVATE_BASE)
 383                 vc->value = pxp->pxp_conf.proc_data.rotate;
 384         else if (vc->id == V4L2_CID_PRIVATE_BASE + 1)
 385                 vc->value = pxp->pxp_conf.proc_data.bgcolor;
 386         else if (vc->id == V4L2_CID_PRIVATE_BASE + 2)
 387                 vc->value = pxp->pxp_conf.s0_param.color_key;
 388         else if (vc->id == V4L2_CID_PRIVATE_BASE + 3)
 389                 vc->value = pxp->pxp_conf.proc_data.yuv;
 390
 391         return 0;
 392 }
 393
 394 static int pxp_enumoutput(struct file *file, void *fh,
 395                         struct v4l2_output *o)
 396 {
 397         struct pxps *pxp = video_get_drvdata(video_devdata(file));
 398
 399         if ((o->index < 0) || (o->index > 1))
 400                 return -EINVAL;
 401
 402         memset(o, 0, sizeof(struct v4l2_output));
 403         if (o->index == 0) {
 404                 strcpy(o->name, "PxP Display Output");
 405                 pxp->output = 0;
 406         } else {
 407                 strcpy(o->name, "PxP Virtual Output");
 408                 pxp->output = 1;
 409         }
 410         o->type = V4L2_OUTPUT_TYPE_INTERNAL;
 411         o->std = 0;
 412         o->reserved[0] = pxp->outbuf.paddr;
 413
 414         return 0;
 415 }
 416
 417 static int pxp_g_output(struct file *file, void *fh,
 418                         unsigned int *i)
 419 {
 420         struct pxps *pxp = video_get_drvdata(video_devdata(file));
 421
 422         *i = pxp->output;
 423
 424         return 0;
 425 }
 426
 427 static int pxp_s_output(struct file *file, void *fh,
 428                         unsigned int i)
 429 {
 430         struct pxps *pxp = video_get_drvdata(video_devdata(file));
 431         struct v4l2_pix_format *fmt = &pxp->fb.fmt;
 432         u32 size;
 433         int ret, bpp;
 434
 435         if ((i < 0) || (i > 1))
 436                 return -EINVAL;
 437
 438         /* Output buffer is same format as fbdev */
 439         if (fmt->pixelformat == V4L2_PIX_FMT_RGB24)
 440                 bpp = 4;
 441         else
 442                 bpp = 2;
 443
 444         size = fmt->width * fmt->height * bpp;
 445         if (size > pxp->outbuf.size) {
 446                 if (pxp->outbuf.vaddr)
 447                         free_dma_buf(pxp, &pxp->outbuf);
 448                 pxp->outbuf.size = size;
 449                 ret = alloc_dma_buf(pxp, &pxp->outbuf);
 450                 if (ret < 0)
 451                         return ret;
 452         }
 453
 454         pxp->pxp_conf.out_param.width = fmt->width;
 455         pxp->pxp_conf.out_param.height = fmt->height;
 456         if (fmt->pixelformat == V4L2_PIX_FMT_RGB24)
 457                 pxp->pxp_conf.out_param.pixel_fmt = PXP_PIX_FMT_RGB24;
 458         else
 459                 pxp->pxp_conf.out_param.pixel_fmt = PXP_PIX_FMT_RGB565;
 460
 461         return 0;
 462 }
 463
 464 static int pxp_enum_fmt_video_output(struct file *file, void *fh,
 465                                 struct v4l2_fmtdesc *fmt)
 466 {
 467         enum v4l2_buf_type type = fmt->type;
 468         int index = fmt->index;
 469
 470         if ((fmt->index < 0) || (fmt->index >= ARRAY_SIZE(pxp_s0_formats)))
 471                 return -EINVAL;
 472
 473         memset(fmt, 0, sizeof(struct v4l2_fmtdesc));
 474         fmt->index = index;
 475         fmt->type = type;
 476         fmt->pixelformat = pxp_s0_formats[index].fourcc;
 477         strcpy(fmt->description, pxp_s0_formats[index].name);
 478
 479         return 0;
 480 }
 481
 482 static int pxp_g_fmt_video_output(struct file *file, void *fh,
 483                                 struct v4l2_format *f)
 484 {
 485         struct v4l2_pix_format *pf = &f->fmt.pix;
 486         struct pxps *pxp = video_get_drvdata(video_devdata(file));
 487         struct pxp_data_format *fmt = pxp->s0_fmt;
 488
 489         pf->width = pxp->pxp_conf.s0_param.width;
 490         pf->height = pxp->pxp_conf.s0_param.height;
 491         pf->pixelformat = fmt->fourcc;
 492         pf->field = V4L2_FIELD_NONE;
 493         pf->bytesperline = fmt->bpp * pf->width;
 494         pf->sizeimage = pf->bytesperline * pf->height;
 495         pf->colorspace = fmt->colorspace;
 496         pf->priv = 0;
 497
 498         return 0;
 499 }
 500
 501 static struct pxp_data_format *pxp_get_format(struct v4l2_format *f)
 502 {
 503         struct pxp_data_format *fmt;
 504         int i;
 505
 506         for (i = 0; i < ARRAY_SIZE(pxp_s0_formats); i++) {
 507                 fmt = &pxp_s0_formats[i];
 508                 if (fmt->fourcc == f->fmt.pix.pixelformat)
 509                         break;
 510         }
 511
 512         if (i == ARRAY_SIZE(pxp_s0_formats))
 513                 return NULL;
 514
 515         return &pxp_s0_formats[i];
 516 }
 517
 518 static int pxp_try_fmt_video_output(struct file *file, void *fh,
 519                                 struct v4l2_format *f)
 520 {
 521         int w = f->fmt.pix.width;
 522         int h = f->fmt.pix.height;
 523         struct pxp_data_format *fmt = pxp_get_format(f);
 524
 525         if (!fmt)
 526                 return -EINVAL;
 527
 528         w = min(w, 2040);
 529         w = max(w, 8);
 530         h = min(h, 2040);
 531         h = max(h, 8);
 532         f->fmt.pix.field = V4L2_FIELD_NONE;
 533         f->fmt.pix.width = w;
 534         f->fmt.pix.height = h;
 535         f->fmt.pix.pixelformat = fmt->fourcc;
 536
 537         return 0;
 538 }
 539
 540 static int pxp_s_fmt_video_output(struct file *file, void *fh,
 541                                 struct v4l2_format *f)
 542 {
 543         struct pxps *pxp = video_get_drvdata(video_devdata(file));
 544         struct v4l2_pix_format *pf = &f->fmt.pix;
 545         int ret;
 546
 547         ret = acquire_dma_channel(pxp);
 548         if (ret < 0)
 549                 return ret;
 550
 551         ret = pxp_try_fmt_video_output(file, fh, f);
 552         if (ret == 0) {
 553                 pxp->s0_fmt = pxp_get_format(f);
 554                 pxp->pxp_conf.s0_param.pixel_fmt =
 555                         v4l2_fmt_to_pxp_fmt(pxp->s0_fmt->fourcc);
 556                 pxp->pxp_conf.s0_param.width = pf->width;
 557                 pxp->pxp_conf.s0_param.height = pf->height;
 558         }
 559
 560
 561         return ret;
 562 }
 563
 564 static int pxp_g_fmt_output_overlay(struct file *file, void *fh,
 565                                 struct v4l2_format *f)
 566 {
 567         struct pxps *pxp = video_get_drvdata(video_devdata(file));
 568         struct v4l2_window *wf = &f->fmt.win;
 569
 570         memset(wf, 0, sizeof(struct v4l2_window));
 571         wf->chromakey = pxp->s1_chromakey;
 572         wf->global_alpha = pxp->global_alpha;
 573         wf->field = V4L2_FIELD_NONE;
 574         wf->clips = NULL;
 575         wf->clipcount = 0;
 576         wf->bitmap = NULL;
 577         wf->w.left = pxp->pxp_conf.proc_data.srect.left;
 578         wf->w.top = pxp->pxp_conf.proc_data.srect.top;
 579         wf->w.width = pxp->pxp_conf.proc_data.srect.width;
 580         wf->w.height = pxp->pxp_conf.proc_data.srect.height;
 581
 582         return 0;
 583 }
 584
 585 static int pxp_try_fmt_output_overlay(struct file *file, void *fh,
 586                                 struct v4l2_format *f)
 587 {
 588         struct pxps *pxp = video_get_drvdata(video_devdata(file));
 589         struct v4l2_window *wf = &f->fmt.win;
 590         struct v4l2_rect srect;
 591         u32 s1_chromakey = wf->chromakey;
 592         u8 global_alpha = wf->global_alpha;
 593
 594         memcpy(&srect, &(wf->w), sizeof(struct v4l2_rect));
 595
 596         pxp_g_fmt_output_overlay(file, fh, f);
 597
 598         wf->chromakey = s1_chromakey;
 599         wf->global_alpha = global_alpha;
 600
 601         /* Constrain parameters to the input buffer */
 602         wf->w.left = srect.left;
 603         wf->w.top = srect.top;
 604         wf->w.width = min(srect.width,
 605                         ((__u32)pxp->pxp_conf.s0_param.width - wf->w.left));
 606         wf->w.height = min(srect.height,
 607                         ((__u32)pxp->pxp_conf.s0_param.height - wf->w.top));
 608
 609         return 0;
 610 }
 611
 612 static int pxp_s_fmt_output_overlay(struct file *file, void *fh,
 613                                         struct v4l2_format *f)
 614 {
 615         struct pxps *pxp = video_get_drvdata(video_devdata(file));
 616         struct v4l2_window *wf = &f->fmt.win;
 617         int ret = pxp_try_fmt_output_overlay(file, fh, f);
 618
 619         if (ret == 0) {
 620                 pxp->global_alpha = wf->global_alpha;
 621                 pxp->s1_chromakey = wf->chromakey;
 622                 pxp->pxp_conf.proc_data.srect.left = wf->w.left;
 623                 pxp->pxp_conf.proc_data.srect.top = wf->w.top;
 624                 pxp->pxp_conf.proc_data.srect.width = wf->w.width;
 625                 pxp->pxp_conf.proc_data.srect.height = wf->w.height;
 626                 pxp->pxp_conf.ol_param[0].global_alpha = pxp->global_alpha;
 627                 pxp->pxp_conf.ol_param[0].color_key = pxp->s1_chromakey;
 628                 pxp->pxp_conf.ol_param[0].color_key_enable =
 629                                         pxp->s1_chromakey_state;
 630         }
 631
 632         return ret;
 633 }
 634
 635 static int pxp_reqbufs(struct file *file, void *priv,
 636                         struct v4l2_requestbuffers *r)
 637 {
 638         struct pxps *pxp = video_get_drvdata(video_devdata(file));
 639
 640         return videobuf_reqbufs(&pxp->s0_vbq, r);
 641 }
 642
 643 static int pxp_querybuf(struct file *file, void *priv,
 644                         struct v4l2_buffer *b)
 645 {
 646         struct pxps *pxp = video_get_drvdata(video_devdata(file));
 647
 648         return videobuf_querybuf(&pxp->s0_vbq, b);
 649 }
 650
 651 static int pxp_qbuf(struct file *file, void *priv,
 652                         struct v4l2_buffer *b)
 653 {
 654         struct pxps *pxp = video_get_drvdata(video_devdata(file));
 655
 656         return videobuf_qbuf(&pxp->s0_vbq, b);
 657 }
 658
 659 static int pxp_dqbuf(struct file *file, void *priv,
 660                         struct v4l2_buffer *b)
 661 {
 662         struct pxps *pxp = video_get_drvdata(video_devdata(file));
 663
 664         return videobuf_dqbuf(&pxp->s0_vbq, b, file->f_flags & O_NONBLOCK);
 665 }
 666
 667 static int pxp_streamon(struct file *file, void *priv,
 668                         enum v4l2_buf_type t)
 669 {
 670         struct pxps *pxp = video_get_drvdata(video_devdata(file));
 671         int ret = 0;
 672
 673         if (t != V4L2_BUF_TYPE_VIDEO_OUTPUT)
 674                 return -EINVAL;
 675
 676         _get_cur_fb_blank(pxp);
 677         set_fb_blank(FB_BLANK_UNBLANK);
 678
 679         ret = videobuf_streamon(&pxp->s0_vbq);
 680
 681         if (!ret && (pxp->output == 0))
 682                 pxp_show_buf(pxp, pxp->outbuf.paddr);
 683
 684         return ret;
 685 }
 686
 687 static int pxp_streamoff(struct file *file, void *priv,
 688                         enum v4l2_buf_type t)
 689 {
 690         struct pxps *pxp = video_get_drvdata(video_devdata(file));
 691         int ret = 0;
 692
 693         if ((t != V4L2_BUF_TYPE_VIDEO_OUTPUT))
 694                 return -EINVAL;
 695
 696         ret = videobuf_streamoff(&pxp->s0_vbq);
 697
 698         pxp_show_buf(pxp, (unsigned long)pxp->fb.base);
 699
 700         if (pxp->fb_blank)
 701                 set_fb_blank(FB_BLANK_POWERDOWN);
 702
 703         return ret;
 704 }
 705
 706 static int pxp_buf_setup(struct videobuf_queue *q,
 707                         unsigned int *count, unsigned *size)
 708 {
 709         struct pxps *pxp = q->priv_data;
 710
 711         *size = pxp->pxp_conf.s0_param.width *
 712                 pxp->pxp_conf.s0_param.height * pxp->s0_fmt->bpp;
 713
 714         if (0 == *count)
 715                 *count = PXP_DEF_BUFS;
 716
 717         return 0;
 718 }
 719
 720 static void pxp_buf_free(struct videobuf_queue *q, struct pxp_buffer *buf)
 721 {
 722         struct videobuf_buffer *vb = &buf->vb;
 723
 724         BUG_ON(in_interrupt());
 725
 726         pr_debug("%s (vb=0x%p) 0x%08lx %d\n", __func__,
 727                 vb, vb->baddr, vb->bsize);
 728
 729         /*
 730          * This waits until this buffer is out of danger, i.e., until it is no
 731          * longer in STATE_QUEUED or STATE_ACTIVE
 732          */
 733         videobuf_waiton(q, vb, 0, 0);
 734
 735         videobuf_dma_contig_free(q, vb);
 736         buf->txd = NULL;
 737
 738         vb->state = VIDEOBUF_NEEDS_INIT;
 739 }
 740
 741 static int pxp_buf_prepare(struct videobuf_queue *q,
 742                         struct videobuf_buffer *vb,
 743                         enum v4l2_field field)
 744 {
 745         struct pxps *pxp = q->priv_data;
 746         struct pxp_config_data *pxp_conf = &pxp->pxp_conf;
 747         struct pxp_proc_data *proc_data = &pxp_conf->proc_data;
 748         struct pxp_buffer *buf = container_of(vb, struct pxp_buffer, vb);
 749         struct pxp_tx_desc *desc;
 750         int ret = 0;
 751         int i, length;
 752
 753         if (!pxp->outbuf.paddr)  {
 754                 dev_err(&pxp->pdev->dev, "Not allocate memory for "
 755                         "PxP Out buffer?\n");
 756                 return -ENOMEM;
 757         }
 758
 759         vb->width = pxp->pxp_conf.s0_param.width;
 760         vb->height = pxp->pxp_conf.s0_param.height;
 761         vb->size = vb->width * vb->height * pxp->s0_fmt->bpp;
 762         vb->field = V4L2_FIELD_NONE;
 763         if (vb->state != VIDEOBUF_NEEDS_INIT)
 764                 pxp_buf_free(q, buf);
 765
 766         if (vb->state == VIDEOBUF_NEEDS_INIT) {
 767                 struct pxp_channel *pchan = pxp->pxp_channel[0];
 768                 struct scatterlist *sg = &buf->sg[0];
 769
 770                 /* This actually (allocates and) maps buffers */
 771                 ret = videobuf_iolock(q, vb, NULL);
 772                 if (ret) {
 773                         pr_err("fail to call videobuf_iolock, ret = %d\n", ret);
 774                         goto fail;
 775                 }
 776
 777                 /*
 778                  * sg[0] for input(S0)
 779                  * Sg[1] for output
 780                  */
 781                 sg_init_table(sg, 3);
 782
 783                 buf->txd = pchan->dma_chan.device->device_prep_slave_sg(
 784                         &pchan->dma_chan, sg, 3, DMA_FROM_DEVICE,
 785                         DMA_PREP_INTERRUPT, NULL);
 786                 if (!buf->txd) {
 787                         ret = -EIO;
 788                         goto fail;
 789                 }
 790
 791                 buf->txd->callback_param        = buf->txd;
 792                 buf->txd->callback              = video_dma_done;
 793
 794                 desc = to_tx_desc(buf->txd);
 795                 length = desc->len;
 796                 for (i = 0; i < length; i++) {
 797                         if (i == 0) {/* S0 */
 798                                 memcpy(&desc->proc_data, proc_data,
 799                                         sizeof(struct pxp_proc_data));
 800                                 pxp_conf->s0_param.paddr =
 801                                                 videobuf_to_dma_contig(vb);
 802                                 memcpy(&desc->layer_param.s0_param,
 803                                         &pxp_conf->s0_param,
 804                                         sizeof(struct pxp_layer_param));
 805                         } else if (i == 1) { /* Output */
 806                                 if (proc_data->rotate % 180) {
 807                                         pxp_conf->out_param.width =
 808                                                 pxp->fb.fmt.height;
 809                                         pxp_conf->out_param.height =
 810                                                 pxp->fb.fmt.width;
 811                                 } else {
 812                                         pxp_conf->out_param.width =
 813                                                 pxp->fb.fmt.width;
 814                                         pxp_conf->out_param.height =
 815                                                 pxp->fb.fmt.height;
 816                                 }
 817
 818                                 pxp_conf->out_param.paddr = pxp->outbuf.paddr;
 819                                 memcpy(&desc->layer_param.out_param,
 820                                         &pxp_conf->out_param,
 821                                         sizeof(struct pxp_layer_param));
 822                         } else if (pxp_conf->ol_param[0].combine_enable) {
 823                                 /* Overlay */
 824                                 pxp_conf->ol_param[0].paddr =
 825                                                 (dma_addr_t)pxp->fb.base;
 826                                 pxp_conf->ol_param[0].width = pxp->fb.fmt.width;
 827                                 pxp_conf->ol_param[0].height =
 828                                                 pxp->fb.fmt.height;
 829                                 pxp_conf->ol_param[0].pixel_fmt =
 830                                                 pxp_conf->out_param.pixel_fmt;
 831                                 memcpy(&desc->layer_param.ol_param,
 832                                        &pxp_conf->ol_param[0],
 833                                        sizeof(struct pxp_layer_param));
 834                         }
 835
 836                         desc = desc->next;
 837                 }
 838
 839                 vb->state = VIDEOBUF_PREPARED;
 840         }
 841
 842         return 0;
 843
 844 fail:
 845         pxp_buf_free(q, buf);
 846         return ret;
 847 }
 848
 849
 850 static void pxp_buf_queue(struct videobuf_queue *q,
 851                         struct videobuf_buffer *vb)
 852 {
 853         struct pxps *pxp = q->priv_data;
 854         struct pxp_buffer *buf = container_of(vb, struct pxp_buffer, vb);
 855         struct dma_async_tx_descriptor *txd = buf->txd;
 856         struct pxp_channel *pchan = pxp->pxp_channel[0];
 857         dma_cookie_t cookie;
 858
 859         BUG_ON(!irqs_disabled());
 860
 861         list_add_tail(&vb->queue, &pxp->outq);
 862
 863         if (!pxp->active) {
 864                 pxp->active = buf;
 865                 vb->state = VIDEOBUF_ACTIVE;
 866         } else {
 867                 vb->state = VIDEOBUF_QUEUED;
 868         }
 869
 870         spin_unlock_irq(&pxp->lock);
 871
 872         cookie = txd->tx_submit(txd);
 873         dev_dbg(&pxp->pdev->dev, "Submitted cookie %d DMA 0x%08x\n",
 874                                 cookie, sg_dma_address(&buf->sg[0]));
 875         mdelay(5);
 876         /* trigger ePxP */
 877         dma_async_issue_pending(&pchan->dma_chan);
 878
 879         spin_lock_irq(&pxp->lock);
 880
 881         if (cookie >= 0)
 882                 return;
 883
 884         /* Submit error */
 885         pr_err("%s: Submit error\n", __func__);
 886         vb->state = VIDEOBUF_PREPARED;
 887
 888         list_del_init(&vb->queue);
 889
 890         if (pxp->active == buf)
 891                 pxp->active = NULL;
 892 }
 893
 894 static void pxp_buf_release(struct videobuf_queue *q,
 895                         struct videobuf_buffer *vb)
 896 {
 897         struct pxps *pxp = q->priv_data;
 898         struct pxp_buffer *buf = container_of(vb, struct pxp_buffer, vb);
 899         unsigned long flags;
 900
 901         spin_lock_irqsave(&pxp->lock, flags);
 902         if ((vb->state == VIDEOBUF_ACTIVE || vb->state == VIDEOBUF_QUEUED) &&
 903             !list_empty(&vb->queue)) {
 904                 vb->state = VIDEOBUF_ERROR;
 905
 906                 list_del_init(&vb->queue);
 907                 if (pxp->active == buf)
 908                         pxp->active = NULL;
 909         }
 910         spin_unlock_irqrestore(&pxp->lock, flags);
 911
 912         pxp_buf_free(q, buf);
 913 }
 914
 915 static struct videobuf_queue_ops pxp_vbq_ops = {
 916         .buf_setup      = pxp_buf_setup,
 917         .buf_prepare    = pxp_buf_prepare,
 918         .buf_queue      = pxp_buf_queue,
 919         .buf_release    = pxp_buf_release,
 920 };
 921
 922 static int pxp_querycap(struct file *file, void *fh,
 923                         struct v4l2_capability *cap)
 924 {
 925         struct pxps *pxp = video_get_drvdata(video_devdata(file));
 926
 927         memset(cap, 0, sizeof(*cap));
 928         strcpy(cap->driver, "pxp");
 929         strcpy(cap->card, "pxp");
 930         strlcpy(cap->bus_info, dev_name(&pxp->pdev->dev),
 931                 sizeof(cap->bus_info));
 932
 933         cap->version = (PXP_DRIVER_MAJOR << 8) + PXP_DRIVER_MINOR;
 934
 935         cap->capabilities = V4L2_CAP_VIDEO_OUTPUT |
 936                                 V4L2_CAP_VIDEO_OUTPUT_OVERLAY |
 937                                 V4L2_CAP_STREAMING;
 938
 939         return 0;
 940 }
 941
 942 static int pxp_g_fbuf(struct file *file, void *priv,
 943                         struct v4l2_framebuffer *fb)
 944 {
 945         struct pxps *pxp = video_get_drvdata(video_devdata(file));
 946
 947         memset(fb, 0, sizeof(*fb));
 948
 949         fb->capability = V4L2_FBUF_CAP_EXTERNOVERLAY |
 950                          V4L2_FBUF_CAP_CHROMAKEY |
 951                          V4L2_FBUF_CAP_LOCAL_ALPHA |
 952                          V4L2_FBUF_CAP_GLOBAL_ALPHA;
 953
 954         if (pxp->global_alpha_state)
 955                 fb->flags |= V4L2_FBUF_FLAG_GLOBAL_ALPHA;
 956         if (pxp->s1_chromakey_state)
 957                 fb->flags |= V4L2_FBUF_FLAG_CHROMAKEY;
 958
 959         return 0;
 960 }
 961
 962 static int pxp_s_fbuf(struct file *file, void *priv,
 963                         const struct v4l2_framebuffer *fb)
 964 {
 965         struct pxps *pxp = video_get_drvdata(video_devdata(file));
 966
 967         pxp->overlay_state =
 968                 (fb->flags & V4L2_FBUF_FLAG_OVERLAY) != 0;
 969         pxp->global_alpha_state =
 970                 (fb->flags & V4L2_FBUF_FLAG_GLOBAL_ALPHA) != 0;
 971         pxp->s1_chromakey_state =
 972                 (fb->flags & V4L2_FBUF_FLAG_CHROMAKEY) != 0;
 973
 974         pxp->pxp_conf.ol_param[0].combine_enable = pxp->overlay_state;
 975         pxp->pxp_conf.ol_param[0].global_alpha_enable = pxp->global_alpha_state;
 976
 977         return 0;
 978 }
 979
 980 static int pxp_g_crop(struct file *file, void *fh,
 981                         struct v4l2_crop *c)
 982 {
 983         struct pxps *pxp = video_get_drvdata(video_devdata(file));
 984
 985         if (c->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY)
 986                 return -EINVAL;
 987
 988         c->c.left = pxp->pxp_conf.proc_data.drect.left;
 989         c->c.top = pxp->pxp_conf.proc_data.drect.top;
 990         c->c.width = pxp->pxp_conf.proc_data.drect.width;
 991         c->c.height = pxp->pxp_conf.proc_data.drect.height;
 992
 993         return 0;
 994 }
 995
 996 static int pxp_s_crop(struct file *file, void *fh,
 997                         const struct v4l2_crop *c)
 998 {
 999         struct pxps *pxp = video_get_drvdata(video_devdata(file));
1000         int l = c->c.left;
1001         int t = c->c.top;
1002         int w = c->c.width;
1003         int h = c->c.height;
1004         int fbw = pxp->fb.fmt.width;
1005         int fbh = pxp->fb.fmt.height;
1006
1007         if (c->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY)
1008                 return -EINVAL;
1009
1010         /* Constrain parameters to FB limits */
1011         w = min(w, fbw);
1012         w = max(w, PXP_MIN_PIX);
1013         h = min(h, fbh);
1014         h = max(h, PXP_MIN_PIX);
1015         if ((l + w) > fbw)
1016                 l = 0;
1017         if ((t + h) > fbh)
1018                 t = 0;
1019
1020         /* Round up values to PxP pixel block */
1021         l = roundup(l, PXP_MIN_PIX);
1022         t = roundup(t, PXP_MIN_PIX);
1023         w = roundup(w, PXP_MIN_PIX);
1024         h = roundup(h, PXP_MIN_PIX);
1025
1026         pxp->pxp_conf.proc_data.drect.left = l;
1027         pxp->pxp_conf.proc_data.drect.top = t;
1028         pxp->pxp_conf.proc_data.drect.width = w;
1029         pxp->pxp_conf.proc_data.drect.height = h;
1030
1031         return 0;
1032 }
1033
1034 static int pxp_queryctrl(struct file *file, void *priv,
1035                          struct v4l2_queryctrl *qc)
1036 {
1037         int i;
1038
1039         for (i = 0; i < ARRAY_SIZE(pxp_controls); i++)
1040                 if (qc->id && qc->id == pxp_controls[i].id) {
1041                         memcpy(qc, &(pxp_controls[i]), sizeof(*qc));
1042                         return 0;
1043                 }
1044
1045         return -EINVAL;
1046 }
1047
1048 static int pxp_g_ctrl(struct file *file, void *priv,
1049                          struct v4l2_control *vc)
1050 {
1051         int i;
1052
1053         struct pxps *pxp = video_get_drvdata(video_devdata(file));
1054
1055         for (i = 0; i < ARRAY_SIZE(pxp_controls); i++)
1056                 if (vc->id == pxp_controls[i].id)
1057                         return pxp_get_cstate(pxp, vc);
1058
1059         return -EINVAL;
1060 }
1061
1062 static int pxp_s_ctrl(struct file *file, void *priv,
1063                          struct v4l2_control *vc)
1064 {
1065         int i;
1066         struct pxps *pxp = video_get_drvdata(video_devdata(file));
1067
1068         for (i = 0; i < ARRAY_SIZE(pxp_controls); i++)
1069                 if (vc->id == pxp_controls[i].id) {
1070                         if (vc->value < pxp_controls[i].minimum ||
1071                             vc->value > pxp_controls[i].maximum)
1072                                 return -ERANGE;
1073                         return pxp_set_cstate(pxp, vc);
1074                 }
1075
1076         return -EINVAL;
1077 }
1078
1079 void pxp_release(struct video_device *vfd)
1080 {
1081         struct pxps *pxp = video_get_drvdata(vfd);
1082
1083         spin_lock(&pxp->lock);
1084         video_device_release(vfd);
1085         spin_unlock(&pxp->lock);
1086 }
1087
1088 static int pxp_open(struct file *file)
1089 {
1090         struct pxps *pxp = video_get_drvdata(video_devdata(file));
1091         int ret = 0;
1092
1093         mutex_lock(&pxp->mutex);
1094         pxp->users++;
1095
1096         if (pxp->users > 1) {
1097                 pxp->users--;
1098                 ret = -EBUSY;
1099                 goto out;
1100         }
1101 out:
1102         mutex_unlock(&pxp->mutex);
1103         if (ret)
1104                 return ret;
1105
1106         ret = pxp_set_fbinfo(pxp);
1107         if (ret) {
1108                 dev_err(&pxp->pdev->dev, "failed to call pxp_set_fbinfo\n");
1109                 return ret;
1110         }
1111
1112         videobuf_queue_dma_contig_init(&pxp->s0_vbq,
1113                                 &pxp_vbq_ops,
1114                                 &pxp->pdev->dev,
1115                                 &pxp->lock,
1116                                 V4L2_BUF_TYPE_VIDEO_OUTPUT,
1117                                 V4L2_FIELD_NONE,
1118                                 sizeof(struct pxp_buffer),
1119                                 pxp,
1120                                 NULL);
1121         dev_dbg(&pxp->pdev->dev, "call pxp_open\n");
1122
1123         return 0;
1124 }
1125
1126 static int pxp_close(struct file *file)
1127 {
1128         struct pxps *pxp = video_get_drvdata(video_devdata(file));
1129
1130         pxp_streamoff(file, NULL, V4L2_BUF_TYPE_VIDEO_OUTPUT);
1131         videobuf_stop(&pxp->s0_vbq);
1132         videobuf_mmap_free(&pxp->s0_vbq);
1133         pxp->active = NULL;
1134
1135         mutex_lock(&pxp->mutex);
1136         pxp->users--;
1137         mutex_unlock(&pxp->mutex);
1138
1139         return 0;
1140 }
1141
1142 static int pxp_mmap(struct file *file, struct vm_area_struct *vma)
1143 {
1144         struct pxps *pxp = video_get_drvdata(video_devdata(file));
1145         int ret;
1146
1147         ret = videobuf_mmap_mapper(&pxp->s0_vbq, vma);
1148
1149         return ret;
1150 }
1151
1152 static const struct v4l2_file_operations pxp_fops = {
1153         .owner          = THIS_MODULE,
1154         .open           = pxp_open,
1155         .release        = pxp_close,
1156         .unlocked_ioctl = video_ioctl2,
1157         .mmap           = pxp_mmap,
1158 };
1159
1160 static const struct v4l2_ioctl_ops pxp_ioctl_ops = {
1161         .vidioc_querycap                = pxp_querycap,
1162
1163         .vidioc_reqbufs                 = pxp_reqbufs,
1164         .vidioc_querybuf                = pxp_querybuf,
1165         .vidioc_qbuf                    = pxp_qbuf,
1166         .vidioc_dqbuf                   = pxp_dqbuf,
1167
1168         .vidioc_streamon                = pxp_streamon,
1169         .vidioc_streamoff               = pxp_streamoff,
1170
1171         .vidioc_enum_output             = pxp_enumoutput,
1172         .vidioc_g_output                = pxp_g_output,
1173         .vidioc_s_output                = pxp_s_output,
1174
1175         .vidioc_enum_fmt_vid_out        = pxp_enum_fmt_video_output,
1176         .vidioc_try_fmt_vid_out         = pxp_try_fmt_video_output,
1177         .vidioc_g_fmt_vid_out           = pxp_g_fmt_video_output,
1178         .vidioc_s_fmt_vid_out           = pxp_s_fmt_video_output,
1179
1180         .vidioc_try_fmt_vid_out_overlay = pxp_try_fmt_output_overlay,
1181         .vidioc_g_fmt_vid_out_overlay   = pxp_g_fmt_output_overlay,
1182         .vidioc_s_fmt_vid_out_overlay   = pxp_s_fmt_output_overlay,
1183
1184         .vidioc_g_fbuf                  = pxp_g_fbuf,
1185         .vidioc_s_fbuf                  = pxp_s_fbuf,
1186
1187         .vidioc_g_crop                  = pxp_g_crop,
1188         .vidioc_s_crop                  = pxp_s_crop,
1189
1190         .vidioc_queryctrl               = pxp_queryctrl,
1191         .vidioc_g_ctrl                  = pxp_g_ctrl,
1192         .vidioc_s_ctrl                  = pxp_s_ctrl,
1193 };
1194
1195 static const struct video_device pxp_template = {
1196         .name                           = "PxP",
1197         .vfl_type                       = V4L2_CAP_VIDEO_OUTPUT |
1198                                                 V4L2_CAP_VIDEO_OVERLAY |
1199                                                 V4L2_CAP_STREAMING,
1200         .vfl_dir                        = VFL_DIR_TX,
1201         .fops                           = &pxp_fops,
1202         .release                        = pxp_release,
1203         .minor                          = -1,
1204         .ioctl_ops                      = &pxp_ioctl_ops,
1205 };
1206
1207 static const struct of_device_id imx_pxpv4l2_dt_ids[] = {
1208         { .compatible = "fsl,imx6sl-pxp-v4l2", },
1209         { /* sentinel */ }
1210 };
1211 MODULE_DEVICE_TABLE(of, imx_pxpv4l2_dt_ids);
1212
1213 static int pxp_probe(struct platform_device *pdev)
1214 {
1215         struct pxps *pxp;
1216         int err = 0;
1217
1218         pxp = kzalloc(sizeof(*pxp), GFP_KERNEL);
1219         if (!pxp) {
1220                 dev_err(&pdev->dev, "failed to allocate control object\n");
1221                 err = -ENOMEM;
1222                 goto exit;
1223         }
1224
1225         dev_set_drvdata(&pdev->dev, pxp);
1226
1227         INIT_LIST_HEAD(&pxp->outq);
1228         spin_lock_init(&pxp->lock);
1229         mutex_init(&pxp->mutex);
1230
1231         pxp->pdev = pdev;
1232
1233         pxp->vdev = video_device_alloc();
1234         if (!pxp->vdev) {
1235                 dev_err(&pdev->dev, "video_device_alloc() failed\n");
1236                 err = -ENOMEM;
1237                 goto freeirq;
1238         }
1239
1240         memcpy(pxp->vdev, &pxp_template, sizeof(pxp_template));
1241         video_set_drvdata(pxp->vdev, pxp);
1242
1243         err = video_register_device(pxp->vdev, VFL_TYPE_GRABBER, video_nr);
1244         if (err) {
1245                 dev_err(&pdev->dev, "failed to register video device\n");
1246                 goto freevdev;
1247         }
1248
1249         dev_info(&pdev->dev, "initialized\n");
1250
1251 exit:
1252         return err;
1253
1254 freevdev:
1255         video_device_release(pxp->vdev);
1256
1257 freeirq:
1258         kfree(pxp);
1259
1260         return err;
1261 }
1262
1263 static int pxp_remove(struct platform_device *pdev)
1264 {
1265         struct pxps *pxp = platform_get_drvdata(pdev);
1266
1267         video_unregister_device(pxp->vdev);
1268         video_device_release(pxp->vdev);
1269
1270         free_dma_buf(pxp, &pxp->outbuf);
1271
1272         kfree(pxp);
1273
1274         return 0;
1275 }
1276
1277 static struct platform_driver pxp_driver = {
1278         .driver         = {
1279                 .name   = PXP_DRIVER_NAME,
1280                 .of_match_table = of_match_ptr(imx_pxpv4l2_dt_ids),
1281         },
1282         .probe          = pxp_probe,
1283         .remove         = pxp_remove,
1284 };
1285
1286 module_platform_driver(pxp_driver);
1287
1288 module_param(video_nr, int, 0444);
1289 MODULE_DESCRIPTION("MXC PxP V4L2 driver");
1290 MODULE_AUTHOR("Freescale Semiconductor, Inc.");
1291 MODULE_LICENSE("GPL");