drivers/media/video/mxc/output/mxc_vout.c

   1 /*
   2  * Copyright (C) 2011 Freescale Semiconductor, Inc. All Rights Reserved.
   3  */
   4
   5 /*
   6  * The code contained herein is licensed under the GNU General Public
   7  * License. You may obtain a copy of the GNU General Public License
   8  * Version 2 or later at the following locations:
   9  *
  10  * http://www.opensource.org/licenses/gpl-license.html
  11  * http://www.gnu.org/copyleft/gpl.html
  12  */
  13
  14 #include <linux/init.h>
  15 #include <linux/module.h>
  16 #include <linux/vmalloc.h>
  17 #include <linux/sched.h>
  18 #include <linux/types.h>
  19 #include <linux/platform_device.h>
  20 #include <linux/dma-mapping.h>
  21 #include <linux/videodev2.h>
  22 #include <linux/mxcfb.h>
  23 #include <linux/console.h>
  24 #include <linux/mxc_v4l2.h>
  25 #include <mach/ipu-v3.h>
  26
  27 #include <media/videobuf-dma-contig.h>
  28 #include <media/v4l2-device.h>
  29 #include <media/v4l2-ioctl.h>
  30
  31 #define MAX_FB_NUM      6
  32 #define FB_BUFS         3
  33
  34 struct mxc_vout_fb {
  35         char *name;
  36         int ipu_id;
  37         struct v4l2_rect crop_bounds;
  38         unsigned int disp_fmt;
  39         bool disp_support_csc;
  40         bool disp_support_windows;
  41 };
  42
  43 struct mxc_vout_output {
  44         int open_cnt;
  45         struct fb_info *fbi;
  46         struct video_device *vfd;
  47         struct mutex mutex;
  48         struct mutex task_lock;
  49         enum v4l2_buf_type type;
  50
  51         struct videobuf_queue vbq;
  52         spinlock_t vbq_lock;
  53
  54         struct list_head queue_list;
  55         struct list_head active_list;
  56
  57         struct v4l2_rect crop_bounds;
  58         unsigned int disp_fmt;
  59         struct mxcfb_pos win_pos;
  60         bool disp_support_windows;
  61         bool disp_support_csc;
  62
  63         struct ipu_task task;
  64
  65         bool timer_stop;
  66         struct timer_list timer;
  67         struct workqueue_struct *v4l_wq;
  68         struct work_struct disp_work;
  69         unsigned long frame_count;
  70         unsigned long start_jiffies;
  71
  72         int ctrl_rotate;
  73         int ctrl_vflip;
  74         int ctrl_hflip;
  75
  76         dma_addr_t disp_bufs[FB_BUFS];
  77
  78         struct videobuf_buffer *pre_vb;
  79 };
  80
  81 struct mxc_vout_dev {
  82         struct device   *dev;
  83         struct v4l2_device v4l2_dev;
  84         struct mxc_vout_output *out[MAX_FB_NUM];
  85         int out_num;
  86 };
  87
  88 /* Driver Configuration macros */
  89 #define VOUT_NAME               "mxc_vout"
  90
  91 /* Variables configurable through module params*/
  92 static int debug;
  93 static int video_nr = 16;
  94
  95 /* Module parameters */
  96 module_param(video_nr, int, S_IRUGO);
  97 MODULE_PARM_DESC(video_nr, "video device numbers");
  98 module_param(debug, bool, S_IRUGO);
  99 MODULE_PARM_DESC(debug, "Debug level (0-1)");
 100
 101 const static struct v4l2_fmtdesc mxc_formats[] = {
 102         {
 103                 .description = "RGB565",
 104                 .pixelformat = V4L2_PIX_FMT_RGB565,
 105         },
 106         {
 107                 .description = "BGR24",
 108                 .pixelformat = V4L2_PIX_FMT_BGR24,
 109         },
 110         {
 111                 .description = "RGB24",
 112                 .pixelformat = V4L2_PIX_FMT_RGB24,
 113         },
 114         {
 115                 .description = "RGB32",
 116                 .pixelformat = V4L2_PIX_FMT_RGB32,
 117         },
 118         {
 119                 .description = "BGR32",
 120                 .pixelformat = V4L2_PIX_FMT_BGR32,
 121         },
 122         {
 123                 .description = "NV12",
 124                 .pixelformat = V4L2_PIX_FMT_NV12,
 125         },
 126         {
 127                 .description = "UYVY",
 128                 .pixelformat = V4L2_PIX_FMT_UYVY,
 129         },
 130         {
 131                 .description = "YUYV",
 132                 .pixelformat = V4L2_PIX_FMT_YUYV,
 133         },
 134         {
 135                 .description = "YUV422 planar",
 136                 .pixelformat = V4L2_PIX_FMT_YUV422P,
 137         },
 138         {
 139                 .description = "YUV444",
 140                 .pixelformat = V4L2_PIX_FMT_YUV444,
 141         },
 142         {
 143                 .description = "YUV420",
 144                 .pixelformat = V4L2_PIX_FMT_YUV420,
 145         },
 146 };
 147
 148 #define NUM_MXC_VOUT_FORMATS (ARRAY_SIZE(mxc_formats))
 149
 150 #define DEF_INPUT_WIDTH         320
 151 #define DEF_INPUT_HEIGHT        240
 152
 153 static int mxc_vidioc_streamoff(struct file *file, void *fh, enum v4l2_buf_type i);
 154
 155 static struct mxc_vout_fb g_fb_setting[MAX_FB_NUM];
 156
 157 static ipu_channel_t get_ipu_channel(struct fb_info *fbi)
 158 {
 159         ipu_channel_t ipu_ch = CHAN_NONE;
 160         mm_segment_t old_fs;
 161
 162         if (fbi->fbops->fb_ioctl) {
 163                 old_fs = get_fs();
 164                 set_fs(KERNEL_DS);
 165                 fbi->fbops->fb_ioctl(fbi, MXCFB_GET_FB_IPU_CHAN,
 166                                 (unsigned long)&ipu_ch);
 167                 set_fs(old_fs);
 168         }
 169
 170         return ipu_ch;
 171 }
 172
 173 static unsigned int get_ipu_fmt(struct fb_info *fbi)
 174 {
 175         mm_segment_t old_fs;
 176         unsigned int fb_fmt;
 177
 178         if (fbi->fbops->fb_ioctl) {
 179                 old_fs = get_fs();
 180                 set_fs(KERNEL_DS);
 181                 fbi->fbops->fb_ioctl(fbi, MXCFB_GET_DIFMT,
 182                                 (unsigned long)&fb_fmt);
 183                 set_fs(old_fs);
 184         }
 185
 186         return fb_fmt;
 187 }
 188
 189 static void update_display_setting(void)
 190 {
 191         int i;
 192         struct fb_info *fbi;
 193         struct v4l2_rect bg_crop_bounds[2];
 194
 195         for (i = 0; i < num_registered_fb; i++) {
 196                 fbi = registered_fb[i];
 197
 198                 memset(&g_fb_setting[i], 0, sizeof(struct mxc_vout_fb));
 199
 200                 if (!strncmp(fbi->fix.id, "DISP3", 5))
 201                         g_fb_setting[i].ipu_id = 0;
 202                 else
 203                         g_fb_setting[i].ipu_id = 1;
 204
 205                 g_fb_setting[i].name = fbi->fix.id;
 206                 g_fb_setting[i].crop_bounds.left = 0;
 207                 g_fb_setting[i].crop_bounds.top = 0;
 208                 g_fb_setting[i].crop_bounds.width = fbi->var.xres;
 209                 g_fb_setting[i].crop_bounds.height = fbi->var.yres;
 210                 g_fb_setting[i].disp_fmt = get_ipu_fmt(fbi);
 211
 212                 if (get_ipu_channel(fbi) == MEM_BG_SYNC) {
 213                         bg_crop_bounds[g_fb_setting[i].ipu_id] =
 214                                 g_fb_setting[i].crop_bounds;
 215                         g_fb_setting[i].disp_support_csc = true;
 216                 } else if (get_ipu_channel(fbi) == MEM_FG_SYNC) {
 217                         g_fb_setting[i].disp_support_csc = true;
 218                         g_fb_setting[i].disp_support_windows = true;
 219                 }
 220         }
 221
 222         for (i = 0; i < num_registered_fb; i++) {
 223                 fbi = registered_fb[i];
 224
 225                 if (get_ipu_channel(fbi) == MEM_FG_SYNC)
 226                         g_fb_setting[i].crop_bounds =
 227                                 bg_crop_bounds[g_fb_setting[i].ipu_id];
 228         }
 229 }
 230
 231 /* called after g_fb_setting filled by update_display_setting */
 232 static int update_setting_from_fbi(struct mxc_vout_output *vout,
 233                         struct fb_info *fbi)
 234 {
 235         int i;
 236         bool found = false;
 237
 238         for (i = 0; i < MAX_FB_NUM; i++) {
 239                 if (g_fb_setting[i].name) {
 240                         if (!strcmp(fbi->fix.id, g_fb_setting[i].name)) {
 241                                 vout->crop_bounds = g_fb_setting[i].crop_bounds;
 242                                 vout->disp_fmt = g_fb_setting[i].disp_fmt;
 243                                 vout->disp_support_csc = g_fb_setting[i].disp_support_csc;
 244                                 vout->disp_support_windows =
 245                                         g_fb_setting[i].disp_support_windows;
 246                                 found = true;
 247                                 break;
 248                         }
 249                 }
 250         }
 251
 252         if (!found) {
 253                 v4l2_err(vout->vfd->v4l2_dev, "can not find output\n");
 254                 return -EINVAL;
 255         }
 256         strlcpy(vout->vfd->name, fbi->fix.id, sizeof(vout->vfd->name));
 257
 258         memset(&vout->task, 0, sizeof(struct ipu_task));
 259
 260         vout->task.input.width = DEF_INPUT_WIDTH;
 261         vout->task.input.height = DEF_INPUT_HEIGHT;
 262         vout->task.input.crop.pos.x = 0;
 263         vout->task.input.crop.pos.y = 0;
 264         vout->task.input.crop.w = DEF_INPUT_WIDTH;
 265         vout->task.input.crop.h = DEF_INPUT_HEIGHT;
 266
 267         vout->task.output.width = vout->crop_bounds.width;
 268         vout->task.output.height = vout->crop_bounds.height;
 269         vout->task.output.crop.pos.x = 0;
 270         vout->task.output.crop.pos.y = 0;
 271         vout->task.output.crop.w = vout->crop_bounds.width;
 272         vout->task.output.crop.h = vout->crop_bounds.height;
 273         if (colorspaceofpixel(vout->disp_fmt) == YUV_CS)
 274                 vout->task.output.format = IPU_PIX_FMT_UYVY;
 275         else
 276                 vout->task.output.format = IPU_PIX_FMT_RGB565;
 277
 278         return 0;
 279 }
 280
 281 static inline unsigned long get_jiffies(struct timeval *t)
 282 {
 283         struct timeval cur;
 284
 285         if (t->tv_usec >= 1000000) {
 286                 t->tv_sec += t->tv_usec / 1000000;
 287                 t->tv_usec = t->tv_usec % 1000000;
 288         }
 289
 290         do_gettimeofday(&cur);
 291         if ((t->tv_sec < cur.tv_sec)
 292             || ((t->tv_sec == cur.tv_sec) && (t->tv_usec < cur.tv_usec)))
 293                 return jiffies;
 294
 295         if (t->tv_usec < cur.tv_usec) {
 296                 cur.tv_sec = t->tv_sec - cur.tv_sec - 1;
 297                 cur.tv_usec = t->tv_usec + 1000000 - cur.tv_usec;
 298         } else {
 299                 cur.tv_sec = t->tv_sec - cur.tv_sec;
 300                 cur.tv_usec = t->tv_usec - cur.tv_usec;
 301         }
 302
 303         return jiffies + timeval_to_jiffies(&cur);
 304 }
 305
 306 static bool deinterlace_3_field(struct mxc_vout_output *vout)
 307 {
 308         return (vout->task.input.deinterlace.enable &&
 309                 (vout->task.input.deinterlace.motion != HIGH_MOTION));
 310 }
 311
 312 static bool is_pp_bypass(struct mxc_vout_output *vout)
 313 {
 314         if ((vout->task.input.width == vout->task.output.width) &&
 315                 (vout->task.input.height == vout->task.output.height) &&
 316                 (vout->task.input.crop.w == vout->task.output.crop.w) &&
 317                 (vout->task.input.crop.h == vout->task.output.crop.h) &&
 318                 (vout->task.output.rotate < IPU_ROTATE_90_RIGHT) &&
 319                 !vout->task.input.deinterlace.enable) {
 320                 if (vout->disp_support_csc)
 321                         return true;
 322                 else if (!need_csc(vout->task.input.format, vout->disp_fmt))
 323                         return true;
 324         /* input crop show to full output which can show based on xres_virtual/yres_virtual */
 325         } else if ((vout->task.input.crop.w == vout->task.output.crop.w) &&
 326                         (vout->task.output.crop.w == vout->task.output.width) &&
 327                         (vout->task.input.crop.h == vout->task.output.crop.h) &&
 328                         (vout->task.output.crop.h == vout->task.output.height) &&
 329                         (vout->task.output.rotate < IPU_ROTATE_90_RIGHT) &&
 330                         !vout->task.input.deinterlace.enable) {
 331                 if (vout->disp_support_csc)
 332                         return true;
 333                 else if (!need_csc(vout->task.input.format, vout->disp_fmt))
 334                         return true;
 335         }
 336         return false;
 337 }
 338
 339 static void setup_buf_timer(struct mxc_vout_output *vout,
 340                         struct videobuf_buffer *vb)
 341 {
 342         unsigned long timeout;
 343
 344         /* if timestamp is 0, then default to 30fps */
 345         if ((vb->ts.tv_sec == 0)
 346                         && (vb->ts.tv_usec == 0)
 347                         && vout->start_jiffies)
 348                 timeout =
 349                         vout->start_jiffies + vout->frame_count * HZ / 30;
 350         else
 351                 timeout = get_jiffies(&vb->ts);
 352
 353         if (jiffies >= timeout) {
 354                 v4l2_dbg(1, debug, vout->vfd->v4l2_dev,
 355                                 "warning: timer timeout already expired.\n");
 356         }
 357
 358         if (mod_timer(&vout->timer, timeout)) {
 359                 v4l2_warn(vout->vfd->v4l2_dev,
 360                                 "warning: timer was already set\n");
 361         }
 362
 363         v4l2_dbg(1, debug, vout->vfd->v4l2_dev,
 364                         "timer handler next schedule: %lu\n", timeout);
 365 }
 366
 367 static int show_buf(struct mxc_vout_output *vout, int idx)
 368 {
 369         struct fb_info *fbi = vout->fbi;
 370         struct fb_var_screeninfo var;
 371         int ret;
 372
 373         memcpy(&var, &fbi->var, sizeof(var));
 374
 375         if (is_pp_bypass(vout)) {
 376                 /*
 377                  * crack fb base
 378                  * NOTE: should not do other fb operation during v4l2
 379                  */
 380                 console_lock();
 381                 fbi->fix.smem_start = vout->task.output.paddr;
 382                 fbi->var.yoffset = vout->task.input.crop.pos.y + 1;
 383                 var.xoffset = vout->task.input.crop.pos.x;
 384                 var.yoffset = vout->task.input.crop.pos.y;
 385                 ret = fb_pan_display(fbi, &var);
 386                 console_unlock();
 387         } else {
 388                 var.yoffset = idx * fbi->var.yres;
 389                 console_lock();
 390                 ret = fb_pan_display(fbi, &var);
 391                 console_unlock();
 392         }
 393
 394         return ret;
 395 }
 396
 397 static void disp_work_func(struct work_struct *work)
 398 {
 399         struct mxc_vout_output *vout =
 400                 container_of(work, struct mxc_vout_output, disp_work);
 401         struct videobuf_queue *q = &vout->vbq;
 402         struct videobuf_buffer *vb, *vb_next = NULL;
 403         unsigned long flags = 0;
 404         int ret;
 405
 406         spin_lock_irqsave(q->irqlock, flags);
 407
 408         if (deinterlace_3_field(vout)) {
 409                 if (list_is_singular(&vout->active_list)) {
 410                         v4l2_warn(vout->vfd->v4l2_dev,
 411                                         "deinterlacing: no enough entry in active_list\n");
 412                         spin_unlock_irqrestore(q->irqlock, flags);
 413                         return;
 414                 }
 415         } else {
 416                 if (list_empty(&vout->active_list)) {
 417                         v4l2_warn(vout->vfd->v4l2_dev,
 418                                         "no entry in active_list, should not be here\n");
 419                         spin_unlock_irqrestore(q->irqlock, flags);
 420                         return;
 421                 }
 422         }
 423         vb = list_first_entry(&vout->active_list,
 424                         struct videobuf_buffer, queue);
 425
 426         if (deinterlace_3_field(vout))
 427                 vb_next = list_first_entry(vout->active_list.next,
 428                                 struct videobuf_buffer, queue);
 429
 430         spin_unlock_irqrestore(q->irqlock, flags);
 431
 432         if (vb->memory == V4L2_MEMORY_USERPTR)
 433                 vout->task.input.paddr = vb->baddr;
 434         else
 435                 vout->task.input.paddr = videobuf_to_dma_contig(vb);
 436
 437         if (is_pp_bypass(vout))
 438                 vout->task.output.paddr = vout->task.input.paddr;
 439         else {
 440                 if (deinterlace_3_field(vout)) {
 441                         if (vb->memory == V4L2_MEMORY_USERPTR)
 442                                 vout->task.input.paddr_n = vb_next->baddr;
 443                         else
 444                                 vout->task.input.paddr_n =
 445                                         videobuf_to_dma_contig(vb_next);
 446                 }
 447                 vout->task.output.paddr =
 448                         vout->disp_bufs[vout->frame_count % FB_BUFS];
 449                 mutex_lock(&vout->task_lock);
 450                 ret = ipu_queue_task(&vout->task);
 451                 mutex_unlock(&vout->task_lock);
 452                 if (ret < 0)
 453                         goto err;
 454         }
 455
 456         if (show_buf(vout, vout->frame_count % FB_BUFS) < 0)
 457                 goto err;
 458
 459         spin_lock_irqsave(q->irqlock, flags);
 460
 461         list_del(&vb->queue);
 462
 463         /*
 464          * previous videobuf finish show, set VIDEOBUF_DONE state here
 465          * to avoid tearing issue, which make sure showing buffer will
 466          * not be dequeue to write new data. It also bring side-effect
 467          * that the last buffer can not be dequeue correctly, app need
 468          * take care about it.
 469          */
 470         if (vout->pre_vb) {
 471                 vout->pre_vb->state = VIDEOBUF_DONE;
 472                 wake_up_interruptible(&vout->pre_vb->done);
 473         }
 474
 475         vout->pre_vb = vb;
 476
 477         vout->frame_count++;
 478
 479         /* pick next queue buf to setup timer */
 480         if (list_empty(&vout->queue_list))
 481                 vout->timer_stop = true;
 482         else {
 483                 vb = list_first_entry(&vout->queue_list,
 484                                 struct videobuf_buffer, queue);
 485                 setup_buf_timer(vout, vb);
 486         }
 487
 488         spin_unlock_irqrestore(q->irqlock, flags);
 489
 490         v4l2_dbg(1, debug, vout->vfd->v4l2_dev, "disp work finish one frame\n");
 491
 492         return;
 493 err:
 494         v4l2_err(vout->vfd->v4l2_dev, "display work fail\n");
 495         vout->timer_stop = true;
 496         vb->state = VIDEOBUF_ERROR;
 497         return;
 498 }
 499
 500 static void mxc_vout_timer_handler(unsigned long arg)
 501 {
 502         struct mxc_vout_output *vout =
 503                         (struct mxc_vout_output *) arg;
 504         struct videobuf_queue *q = &vout->vbq;
 505         struct videobuf_buffer *vb;
 506         unsigned long flags = 0;
 507
 508         spin_lock_irqsave(q->irqlock, flags);
 509
 510         /*
 511          * put first queued entry into active, if previous entry did not
 512          * finish, setup current entry's timer again.
 513          */
 514         if (list_empty(&vout->queue_list)) {
 515                 spin_unlock_irqrestore(q->irqlock, flags);
 516                 return;
 517         }
 518
 519         /* move videobuf from queued list to active list */
 520         vb = list_first_entry(&vout->queue_list,
 521                         struct videobuf_buffer, queue);
 522         list_del(&vb->queue);
 523         list_add_tail(&vb->queue, &vout->active_list);
 524
 525         if (queue_work(vout->v4l_wq, &vout->disp_work) == 0) {
 526                 v4l2_warn(vout->vfd->v4l2_dev,
 527                         "disp work was in queue already, queue buf again next time\n");
 528                 list_del(&vb->queue);
 529                 list_add(&vb->queue, &vout->queue_list);
 530                 spin_unlock_irqrestore(q->irqlock, flags);
 531                 return;
 532         }
 533
 534         vb->state = VIDEOBUF_ACTIVE;
 535
 536         spin_unlock_irqrestore(q->irqlock, flags);
 537 }
 538
 539 /* Video buffer call backs */
 540
 541 /*
 542  * Buffer setup function is called by videobuf layer when REQBUF ioctl is
 543  * called. This is used to setup buffers and return size and count of
 544  * buffers allocated. After the call to this buffer, videobuf layer will
 545  * setup buffer queue depending on the size and count of buffers
 546  */
 547 static int mxc_vout_buffer_setup(struct videobuf_queue *q, unsigned int *count,
 548                           unsigned int *size)
 549 {
 550         struct mxc_vout_output *vout = q->priv_data;
 551
 552         if (!vout)
 553                 return -EINVAL;
 554
 555         if (V4L2_BUF_TYPE_VIDEO_OUTPUT != q->type)
 556                 return -EINVAL;
 557
 558         *size = PAGE_ALIGN(vout->task.input.width * vout->task.input.height *
 559                         fmt_to_bpp(vout->task.input.format)/8);
 560
 561         return 0;
 562 }
 563
 564 /*
 565  * This function will be called when VIDIOC_QBUF ioctl is called.
 566  * It prepare buffers before give out for the display. This function
 567  * converts user space virtual address into physical address if userptr memory
 568  * exchange mechanism is used.
 569  */
 570 static int mxc_vout_buffer_prepare(struct videobuf_queue *q,
 571                             struct videobuf_buffer *vb,
 572                             enum v4l2_field field)
 573 {
 574         vb->state = VIDEOBUF_PREPARED;
 575         return 0;
 576 }
 577
 578 /*
 579  * Buffer queue funtion will be called from the videobuf layer when _QBUF
 580  * ioctl is called. It is used to enqueue buffer, which is ready to be
 581  * displayed.
 582  * This function is protected by q->irqlock.
 583  */
 584 static void mxc_vout_buffer_queue(struct videobuf_queue *q,
 585                           struct videobuf_buffer *vb)
 586 {
 587         struct mxc_vout_output *vout = q->priv_data;
 588
 589         list_add_tail(&vb->queue, &vout->queue_list);
 590         vb->state = VIDEOBUF_QUEUED;
 591
 592         if (vout->timer_stop) {
 593                 if (deinterlace_3_field(vout) &&
 594                         list_empty(&vout->active_list)) {
 595                         vb = list_first_entry(&vout->queue_list,
 596                                         struct videobuf_buffer, queue);
 597                         list_del(&vb->queue);
 598                         list_add_tail(&vb->queue, &vout->active_list);
 599                 } else {
 600                         setup_buf_timer(vout, vb);
 601                         vout->timer_stop = false;
 602                 }
 603         }
 604 }
 605
 606 /*
 607  * Buffer release function is called from videobuf layer to release buffer
 608  * which are already allocated
 609  */
 610 static void mxc_vout_buffer_release(struct videobuf_queue *q,
 611                             struct videobuf_buffer *vb)
 612 {
 613         vb->state = VIDEOBUF_NEEDS_INIT;
 614 }
 615
 616 static int mxc_vout_mmap(struct file *file, struct vm_area_struct *vma)
 617 {
 618         int ret;
 619         struct mxc_vout_output *vout = file->private_data;
 620
 621         if (!vout)
 622                 return -ENODEV;
 623
 624         ret = videobuf_mmap_mapper(&vout->vbq, vma);
 625         if (ret < 0)
 626                 v4l2_err(vout->vfd->v4l2_dev,
 627                                 "offset invalid [offset=0x%lx]\n",
 628                                 (vma->vm_pgoff << PAGE_SHIFT));
 629
 630         return ret;
 631 }
 632
 633 static int mxc_vout_release(struct file *file)
 634 {
 635         unsigned int ret = 0;
 636         struct videobuf_queue *q;
 637         struct mxc_vout_output *vout = file->private_data;
 638
 639         if (!vout)
 640                 return 0;
 641
 642         if (--vout->open_cnt == 0) {
 643                 q = &vout->vbq;
 644                 if (q->streaming)
 645                         ret = mxc_vidioc_streamoff(file, vout, vout->type);
 646                 destroy_workqueue(vout->v4l_wq);
 647         }
 648
 649         return ret;
 650 }
 651
 652 static int mxc_vout_open(struct file *file)
 653 {
 654         struct mxc_vout_output *vout = NULL;
 655         int ret;
 656
 657         vout = video_drvdata(file);
 658
 659         if (vout == NULL)
 660                 return -ENODEV;
 661
 662         if (vout->open_cnt++ == 0) {
 663                 vout->ctrl_rotate = 0;
 664                 vout->ctrl_vflip = 0;
 665                 vout->ctrl_hflip = 0;
 666                 update_display_setting();
 667                 ret = update_setting_from_fbi(vout, vout->fbi);
 668                 if (ret < 0)
 669                         goto err;
 670
 671                 vout->v4l_wq = create_singlethread_workqueue("v4l2q");
 672                 if (!vout->v4l_wq) {
 673                         v4l2_err(vout->vfd->v4l2_dev,
 674                                         "Could not create work queue\n");
 675                         ret = -ENOMEM;
 676                         goto err;
 677                 }
 678
 679                 INIT_WORK(&vout->disp_work, disp_work_func);
 680
 681                 INIT_LIST_HEAD(&vout->queue_list);
 682                 INIT_LIST_HEAD(&vout->active_list);
 683
 684                 vout->frame_count = 0;
 685
 686                 vout->win_pos.x = 0;
 687                 vout->win_pos.y = 0;
 688         }
 689
 690         file->private_data = vout;
 691
 692 err:
 693         return ret;
 694 }
 695
 696 /*
 697  * V4L2 ioctls
 698  */
 699 static int mxc_vidioc_querycap(struct file *file, void *fh,
 700                 struct v4l2_capability *cap)
 701 {
 702         struct mxc_vout_output *vout = fh;
 703
 704         strlcpy(cap->driver, VOUT_NAME, sizeof(cap->driver));
 705         strlcpy(cap->card, vout->vfd->name, sizeof(cap->card));
 706         cap->bus_info[0] = '\0';
 707         cap->capabilities = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_OUTPUT;
 708
 709         return 0;
 710 }
 711
 712 static int mxc_vidioc_enum_fmt_vid_out(struct file *file, void *fh,
 713                         struct v4l2_fmtdesc *fmt)
 714 {
 715         if (fmt->index >= NUM_MXC_VOUT_FORMATS)
 716                 return -EINVAL;
 717
 718         strlcpy(fmt->description, mxc_formats[fmt->index].description,
 719                         sizeof(fmt->description));
 720         fmt->pixelformat = mxc_formats[fmt->index].pixelformat;
 721
 722         return 0;
 723 }
 724
 725 static int mxc_vidioc_g_fmt_vid_out(struct file *file, void *fh,
 726                         struct v4l2_format *f)
 727 {
 728         struct mxc_vout_output *vout = fh;
 729         struct v4l2_rect *rect = NULL;
 730
 731         f->fmt.pix.width = vout->task.input.width;
 732         f->fmt.pix.height = vout->task.input.height;
 733         f->fmt.pix.pixelformat = vout->task.input.format;
 734         f->fmt.pix.sizeimage = vout->task.input.width * vout->task.input.height *
 735                         fmt_to_bpp(vout->task.input.format)/8;
 736
 737         if (f->fmt.pix.priv) {
 738                 rect = (struct v4l2_rect *)f->fmt.pix.priv;
 739                 rect->left = vout->task.input.crop.pos.x;
 740                 rect->top = vout->task.input.crop.pos.y;
 741                 rect->width = vout->task.input.crop.w;
 742                 rect->height = vout->task.input.crop.h;
 743         }
 744
 745         return 0;
 746 }
 747
 748 static inline int ipu_try_task(struct ipu_task *task)
 749 {
 750         int ret;
 751
 752 again:
 753         ret = ipu_check_task(task);
 754         if (ret != IPU_CHECK_OK) {
 755                 if (ret > IPU_CHECK_ERR_MIN) {
 756                         if (ret == IPU_CHECK_ERR_SPLIT_INPUTW_OVER) {
 757                                 task->input.crop.w -= 8;
 758                                 goto again;
 759                         }
 760                         if (ret == IPU_CHECK_ERR_SPLIT_INPUTH_OVER) {
 761                                 task->input.crop.h -= 8;
 762                                 goto again;
 763                         }
 764                         if (ret == IPU_CHECK_ERR_SPLIT_OUTPUTW_OVER) {
 765                                 task->output.crop.w -= 8;
 766                                 goto again;
 767                         }
 768                         if (ret == IPU_CHECK_ERR_SPLIT_OUTPUTH_OVER) {
 769                                 task->output.crop.h -= 8;
 770                                 goto again;
 771                         }
 772                         ret = -EINVAL;
 773                 }
 774         } else
 775                 ret = 0;
 776
 777         return ret;
 778 }
 779
 780 static int mxc_vout_try_format(struct mxc_vout_output *vout, struct v4l2_format *f)
 781 {
 782         int ret = 0;
 783         struct v4l2_rect *rect = NULL;
 784
 785         vout->task.input.width = f->fmt.pix.width;
 786         vout->task.input.height = f->fmt.pix.height;
 787         vout->task.input.format = f->fmt.pix.pixelformat;
 788
 789         switch (f->fmt.pix.field) {
 790         /* Images are in progressive format, not interlaced */
 791         case V4L2_FIELD_NONE:
 792                 break;
 793         /* The two fields of a frame are passed in separate buffers,
 794            in temporal order, i. e. the older one first. */
 795         case V4L2_FIELD_ALTERNATE:
 796                 v4l2_err(vout->vfd->v4l2_dev,
 797                         "V4L2_FIELD_ALTERNATE field format not supported yet!\n");
 798                 break;
 799         case V4L2_FIELD_INTERLACED_TB:
 800                 v4l2_info(vout->vfd->v4l2_dev, "Enable deinterlace.\n");
 801                 vout->task.input.deinterlace.enable = true;
 802                 vout->task.input.deinterlace.field_fmt =
 803                                 IPU_DEINTERLACE_FIELD_TOP;
 804                 break;
 805         case V4L2_FIELD_INTERLACED_BT:
 806                 v4l2_info(vout->vfd->v4l2_dev, "Enable deinterlace.\n");
 807                 vout->task.input.deinterlace.enable = true;
 808                 vout->task.input.deinterlace.field_fmt =
 809                                 IPU_DEINTERLACE_FIELD_BOTTOM;
 810                 break;
 811         default:
 812                 break;
 813         }
 814
 815         if (f->fmt.pix.priv) {
 816                 rect = (struct v4l2_rect *)f->fmt.pix.priv;
 817                 vout->task.input.crop.pos.x = rect->left;
 818                 vout->task.input.crop.pos.y = rect->top;
 819                 vout->task.input.crop.w = rect->width - rect->width%8;
 820                 vout->task.input.crop.h = rect->height - rect->height%8;
 821         } else {
 822                 vout->task.input.crop.pos.x = 0;
 823                 vout->task.input.crop.pos.y = 0;
 824                 vout->task.input.crop.w = f->fmt.pix.width - f->fmt.pix.width%8;
 825                 vout->task.input.crop.h = f->fmt.pix.height - f->fmt.pix.height%8;
 826         }
 827
 828         /* assume task.output already set by S_CROP */
 829         if (is_pp_bypass(vout)) {
 830                 v4l2_info(vout->vfd->v4l2_dev, "Bypass IC.\n");
 831                 vout->task.output.format = vout->task.input.format;
 832         } else {
 833                 /* if need CSC, choose IPU-DP or IPU_IC do it */
 834                 if (vout->disp_support_csc) {
 835                         if (colorspaceofpixel(vout->task.input.format) == YUV_CS)
 836                                 vout->task.output.format = IPU_PIX_FMT_UYVY;
 837                         else
 838                                 vout->task.output.format = IPU_PIX_FMT_RGB565;
 839                 } else {
 840                         if (colorspaceofpixel(vout->disp_fmt) == YUV_CS)
 841                                 vout->task.output.format = IPU_PIX_FMT_UYVY;
 842                         else
 843                                 vout->task.output.format = IPU_PIX_FMT_RGB565;
 844                 }
 845                 ret = ipu_try_task(&vout->task);
 846                 if (!ret) {
 847                         if (rect) {
 848                                 rect->width = vout->task.input.crop.w;
 849                                 rect->height = vout->task.input.crop.h;
 850                         } else {
 851                                 f->fmt.pix.width = vout->task.input.crop.w;
 852                                 f->fmt.pix.height = vout->task.input.crop.h;
 853                         }
 854                 }
 855         }
 856
 857         return ret;
 858 }
 859
 860 static int mxc_vidioc_s_fmt_vid_out(struct file *file, void *fh,
 861                         struct v4l2_format *f)
 862 {
 863         struct mxc_vout_output *vout = fh;
 864         int ret = 0;
 865
 866         if (vout->vbq.streaming)
 867                 return -EBUSY;
 868
 869         mutex_lock(&vout->task_lock);
 870         ret = mxc_vout_try_format(vout, f);
 871         mutex_unlock(&vout->task_lock);
 872
 873         return ret;
 874 }
 875
 876 static int mxc_vidioc_cropcap(struct file *file, void *fh,
 877                 struct v4l2_cropcap *cropcap)
 878 {
 879         struct mxc_vout_output *vout = fh;
 880
 881         if (cropcap->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
 882                 return -EINVAL;
 883
 884         cropcap->bounds = vout->crop_bounds;
 885         cropcap->defrect = vout->crop_bounds;
 886
 887         return 0;
 888 }
 889
 890 static int mxc_vidioc_g_crop(struct file *file, void *fh, struct v4l2_crop *crop)
 891 {
 892         struct mxc_vout_output *vout = fh;
 893
 894         if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
 895                 return -EINVAL;
 896
 897         if (vout->task.output.crop.w && vout->task.output.crop.h) {
 898                 crop->c.left = vout->task.output.crop.pos.x;
 899                 crop->c.top = vout->task.output.crop.pos.y;
 900                 crop->c.width = vout->task.output.crop.w;
 901                 crop->c.height = vout->task.output.crop.h;
 902         } else {
 903                 crop->c.left = 0;
 904                 crop->c.top = 0;
 905                 crop->c.width = vout->task.output.width;
 906                 crop->c.height = vout->task.output.height;
 907         }
 908
 909         return 0;
 910 }
 911
 912 static int mxc_vidioc_s_crop(struct file *file, void *fh, struct v4l2_crop *crop)
 913 {
 914         struct mxc_vout_output *vout = fh;
 915         struct v4l2_rect *b = &vout->crop_bounds;
 916
 917         if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
 918                 return -EINVAL;
 919
 920         if (crop->c.width < 0 || crop->c.height < 0)
 921                 return -EINVAL;
 922
 923         if (vout->vbq.streaming)
 924                 return -EBUSY;
 925
 926         if (crop->c.width == 0)
 927                 crop->c.width = b->width - b->left;
 928         if (crop->c.height == 0)
 929                 crop->c.height = b->height - b->top;
 930
 931         if (crop->c.top < b->top)
 932                 crop->c.top = b->top;
 933         if (crop->c.top >= b->top + b->height)
 934                 crop->c.top = b->top + b->height - 1;
 935         if (crop->c.height > b->top - crop->c.top + b->height)
 936                 crop->c.height =
 937                         b->top - crop->c.top + b->height;
 938
 939         if (crop->c.left < b->left)
 940                 crop->c.left = b->left;
 941         if (crop->c.left >= b->left + b->width)
 942                 crop->c.left = b->left + b->width - 1;
 943         if (crop->c.width > b->left - crop->c.left + b->width)
 944                 crop->c.width =
 945                         b->left - crop->c.left + b->width;
 946
 947         /* stride line limitation */
 948         crop->c.height -= crop->c.height % 8;
 949         crop->c.width -= crop->c.width % 8;
 950
 951         mutex_lock(&vout->task_lock);
 952
 953         if (vout->disp_support_windows) {
 954                 vout->task.output.crop.pos.x = 0;
 955                 vout->task.output.crop.pos.y = 0;
 956                 vout->win_pos.x = crop->c.left;
 957                 vout->win_pos.y = crop->c.top;
 958                 vout->task.output.width = crop->c.width;
 959                 vout->task.output.height = crop->c.height;
 960         } else {
 961                 vout->task.output.crop.pos.x = crop->c.left;
 962                 vout->task.output.crop.pos.y = crop->c.top;
 963         }
 964
 965         vout->task.output.crop.w = crop->c.width;
 966         vout->task.output.crop.h = crop->c.height;
 967
 968         mutex_unlock(&vout->task_lock);
 969
 970         return 0;
 971 }
 972
 973 static int mxc_vidioc_queryctrl(struct file *file, void *fh,
 974                 struct v4l2_queryctrl *ctrl)
 975 {
 976         int ret = 0;
 977
 978         switch (ctrl->id) {
 979         case V4L2_CID_ROTATE:
 980                 ret = v4l2_ctrl_query_fill(ctrl, 0, 270, 90, 0);
 981                 break;
 982         case V4L2_CID_VFLIP:
 983                 ret = v4l2_ctrl_query_fill(ctrl, 0, 1, 1, 0);
 984                 break;
 985         case V4L2_CID_HFLIP:
 986                 ret = v4l2_ctrl_query_fill(ctrl, 0, 1, 1, 0);
 987                 break;
 988         case V4L2_CID_MXC_MOTION:
 989                 ret = v4l2_ctrl_query_fill(ctrl, 0, 2, 1, 0);
 990                 break;
 991         default:
 992                 ctrl->name[0] = '\0';
 993                 ret = -EINVAL;
 994         }
 995         return ret;
 996 }
 997
 998 static int mxc_vidioc_g_ctrl(struct file *file, void *fh, struct v4l2_control *ctrl)
 999 {
1000         int ret = 0;
1001         struct mxc_vout_output *vout = fh;
1002
1003         switch (ctrl->id) {
1004         case V4L2_CID_ROTATE:
1005                 ctrl->value = vout->ctrl_rotate;
1006                 break;
1007         case V4L2_CID_VFLIP:
1008                 ctrl->value = vout->ctrl_vflip;
1009                 break;
1010         case V4L2_CID_HFLIP:
1011                 ctrl->value = vout->ctrl_hflip;
1012                 break;
1013         case V4L2_CID_MXC_MOTION:
1014                 if (vout->task.input.deinterlace.enable)
1015                         ctrl->value = vout->task.input.deinterlace.motion;
1016                 else
1017                         ctrl->value = 0;
1018                 break;
1019         default:
1020                 ret = -EINVAL;
1021         }
1022         return ret;
1023 }
1024
1025 static void setup_task_rotation(struct mxc_vout_output *vout)
1026 {
1027         if (vout->ctrl_rotate == 0) {
1028                 if (vout->ctrl_vflip && vout->ctrl_hflip)
1029                         vout->task.output.rotate = IPU_ROTATE_180;
1030                 else if (vout->ctrl_vflip)
1031                         vout->task.output.rotate = IPU_ROTATE_VERT_FLIP;
1032                 else if (vout->ctrl_hflip)
1033                         vout->task.output.rotate = IPU_ROTATE_HORIZ_FLIP;
1034                 else
1035                         vout->task.output.rotate = IPU_ROTATE_NONE;
1036         } else if (vout->ctrl_rotate == 90) {
1037                 if (vout->ctrl_vflip && vout->ctrl_hflip)
1038                         vout->task.output.rotate = IPU_ROTATE_90_LEFT;
1039                 else if (vout->ctrl_vflip)
1040                         vout->task.output.rotate = IPU_ROTATE_90_RIGHT_VFLIP;
1041                 else if (vout->ctrl_hflip)
1042                         vout->task.output.rotate = IPU_ROTATE_90_RIGHT_HFLIP;
1043                 else
1044                         vout->task.output.rotate = IPU_ROTATE_90_RIGHT;
1045         } else if (vout->ctrl_rotate == 180) {
1046                 if (vout->ctrl_vflip && vout->ctrl_hflip)
1047                         vout->task.output.rotate = IPU_ROTATE_NONE;
1048                 else if (vout->ctrl_vflip)
1049                         vout->task.output.rotate = IPU_ROTATE_HORIZ_FLIP;
1050                 else if (vout->ctrl_hflip)
1051                         vout->task.output.rotate = IPU_ROTATE_VERT_FLIP;
1052                 else
1053                         vout->task.output.rotate = IPU_ROTATE_180;
1054         } else if (vout->ctrl_rotate == 270) {
1055                 if (vout->ctrl_vflip && vout->ctrl_hflip)
1056                         vout->task.output.rotate = IPU_ROTATE_90_RIGHT;
1057                 else if (vout->ctrl_vflip)
1058                         vout->task.output.rotate = IPU_ROTATE_90_RIGHT_HFLIP;
1059                 else if (vout->ctrl_hflip)
1060                         vout->task.output.rotate = IPU_ROTATE_90_RIGHT_VFLIP;
1061                 else
1062                         vout->task.output.rotate = IPU_ROTATE_90_LEFT;
1063         }
1064 }
1065
1066 static int mxc_vidioc_s_ctrl(struct file *file, void *fh, struct v4l2_control *ctrl)
1067 {
1068         int ret = 0;
1069         struct mxc_vout_output *vout = fh;
1070
1071         mutex_lock(&vout->task_lock);
1072         switch (ctrl->id) {
1073         case V4L2_CID_ROTATE:
1074         {
1075                 vout->ctrl_rotate = (ctrl->value/90) * 90;
1076                 if (vout->ctrl_rotate > 270)
1077                         vout->ctrl_rotate = 270;
1078                 setup_task_rotation(vout);
1079                 break;
1080         }
1081         case V4L2_CID_VFLIP:
1082         {
1083                 vout->ctrl_vflip = ctrl->value;
1084                 setup_task_rotation(vout);
1085                 break;
1086         }
1087         case V4L2_CID_HFLIP:
1088         {
1089                 vout->ctrl_hflip = ctrl->value;
1090                 setup_task_rotation(vout);
1091                 break;
1092         }
1093         case V4L2_CID_MXC_MOTION:
1094         {
1095                 vout->task.input.deinterlace.motion = ctrl->value;
1096                 break;
1097         }
1098         default:
1099                 ret = -EINVAL;
1100         }
1101         mutex_unlock(&vout->task_lock);
1102         return ret;
1103 }
1104
1105 static int mxc_vidioc_reqbufs(struct file *file, void *fh,
1106                         struct v4l2_requestbuffers *req)
1107 {
1108         int ret = 0;
1109         struct mxc_vout_output *vout = fh;
1110         struct videobuf_queue *q = &vout->vbq;
1111
1112         if (req->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
1113                 return -EINVAL;
1114
1115         /* should not be here after streaming, videobuf_reqbufs will control */
1116         mutex_lock(&vout->task_lock);
1117
1118         ret = videobuf_reqbufs(q, req);
1119
1120         mutex_unlock(&vout->task_lock);
1121         return ret;
1122 }
1123
1124 static int mxc_vidioc_querybuf(struct file *file, void *fh,
1125                         struct v4l2_buffer *b)
1126 {
1127         int ret;
1128         struct mxc_vout_output *vout = fh;
1129
1130         ret = videobuf_querybuf(&vout->vbq, b);
1131         if (!ret) {
1132                 /* return physical address */
1133                 struct videobuf_buffer *vb = vout->vbq.bufs[b->index];
1134                 if (b->flags & V4L2_BUF_FLAG_MAPPED)
1135                         b->m.offset = videobuf_to_dma_contig(vb);
1136         }
1137
1138         return ret;
1139 }
1140
1141 static int mxc_vidioc_qbuf(struct file *file, void *fh,
1142                         struct v4l2_buffer *buffer)
1143 {
1144         struct mxc_vout_output *vout = fh;
1145
1146         return videobuf_qbuf(&vout->vbq, buffer);
1147 }
1148
1149 static int mxc_vidioc_dqbuf(struct file *file, void *fh, struct v4l2_buffer *b)
1150 {
1151         struct mxc_vout_output *vout = fh;
1152
1153         if (!vout->vbq.streaming)
1154                 return -EINVAL;
1155
1156         if (file->f_flags & O_NONBLOCK)
1157                 return videobuf_dqbuf(&vout->vbq, (struct v4l2_buffer *)b, 1);
1158         else
1159                 return videobuf_dqbuf(&vout->vbq, (struct v4l2_buffer *)b, 0);
1160 }
1161
1162 static int set_window_position(struct mxc_vout_output *vout)
1163 {
1164         struct fb_info *fbi = vout->fbi;
1165         mm_segment_t old_fs;
1166         int ret;
1167
1168         if (vout->disp_support_windows) {
1169                 old_fs = get_fs();
1170                 set_fs(KERNEL_DS);
1171                 ret = fbi->fbops->fb_ioctl(fbi, MXCFB_SET_OVERLAY_POS,
1172                                 (unsigned long)&vout->win_pos);
1173                 set_fs(old_fs);
1174         }
1175
1176         return ret;
1177 }
1178
1179 static int config_disp_output(struct mxc_vout_output *vout)
1180 {
1181         struct fb_info *fbi = vout->fbi;
1182         struct fb_var_screeninfo var;
1183         int i, display_buf_size, fb_num, ret;
1184
1185         memcpy(&var, &fbi->var, sizeof(var));
1186
1187         var.xres = vout->task.output.width;
1188         var.yres = vout->task.output.height;
1189         if (is_pp_bypass(vout)) {
1190                 fb_num = 1;
1191                 /* input crop */
1192                 if (vout->task.input.width > vout->task.output.width)
1193                         var.xres_virtual = vout->task.input.width;
1194                 else
1195                         var.xres_virtual = var.xres;
1196                 if (vout->task.input.height > vout->task.output.height)
1197                         var.yres_virtual = vout->task.input.height;
1198                 else
1199                         var.yres_virtual = var.yres;
1200         } else {
1201                 fb_num = FB_BUFS;
1202                 var.xres_virtual = var.xres;
1203                 var.yres_virtual = fb_num * var.yres;
1204         }
1205         var.bits_per_pixel = fmt_to_bpp(vout->task.output.format);
1206         var.nonstd = vout->task.output.format;
1207
1208         v4l2_dbg(1, debug, vout->vfd->v4l2_dev,
1209                         "set display fb to %d %d\n",
1210                         var.xres, var.yres);
1211
1212         /* Init display channel through fb API */
1213         var.yoffset = 0;
1214         var.activate |= FB_ACTIVATE_FORCE;
1215         console_lock();
1216         fbi->flags |= FBINFO_MISC_USEREVENT;
1217         ret = fb_set_var(fbi, &var);
1218         fbi->flags &= ~FBINFO_MISC_USEREVENT;
1219         console_unlock();
1220         if (ret < 0)
1221                 return ret;
1222
1223         display_buf_size = fbi->fix.line_length * fbi->var.yres;
1224         for (i = 0; i < fb_num; i++)
1225                 vout->disp_bufs[i] = fbi->fix.smem_start + i * display_buf_size;
1226
1227         ret = set_window_position(vout);
1228         if (ret < 0)
1229                 return ret;
1230
1231         console_lock();
1232         ret = fb_blank(fbi, FB_BLANK_UNBLANK);
1233         console_unlock();
1234
1235         return ret;
1236 }
1237
1238 static void release_disp_output(struct mxc_vout_output *vout)
1239 {
1240         struct fb_info *fbi = vout->fbi;
1241
1242         console_lock();
1243         fb_blank(fbi, FB_BLANK_POWERDOWN);
1244         console_unlock();
1245
1246         /* fix if ic bypass crack smem_start */
1247         if (is_pp_bypass(vout)) {
1248                 console_lock();
1249                 fbi->fix.smem_start = vout->disp_bufs[0];
1250                 console_unlock();
1251         }
1252
1253         if (get_ipu_channel(fbi) == MEM_BG_SYNC) {
1254                 console_lock();
1255                 fb_blank(fbi, FB_BLANK_UNBLANK);
1256                 console_unlock();
1257         }
1258 }
1259
1260 static int mxc_vidioc_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
1261 {
1262         struct mxc_vout_output *vout = fh;
1263         struct videobuf_queue *q = &vout->vbq;
1264         int ret;
1265
1266         if (q->streaming) {
1267                 v4l2_err(vout->vfd->v4l2_dev,
1268                                 "video output already run\n");
1269                 ret = -EBUSY;
1270                 goto done;
1271         }
1272
1273         if (deinterlace_3_field(vout) && list_is_singular(&q->stream)) {
1274                 v4l2_err(vout->vfd->v4l2_dev,
1275                                 "deinterlacing: need queue 2 frame before streamon\n");
1276                 ret = -EINVAL;
1277                 goto done;
1278         }
1279
1280         ret = config_disp_output(vout);
1281         if (ret < 0) {
1282                 v4l2_err(vout->vfd->v4l2_dev,
1283                                 "Config display output failed\n");
1284                 goto done;
1285         }
1286
1287         init_timer(&vout->timer);
1288         vout->timer.function = mxc_vout_timer_handler;
1289         vout->timer.data = (unsigned long)vout;
1290         vout->timer_stop = true;
1291
1292         vout->start_jiffies = jiffies;
1293
1294         vout->pre_vb = NULL;
1295
1296         ret = videobuf_streamon(q);
1297 done:
1298         return ret;
1299 }
1300
1301 static int mxc_vidioc_streamoff(struct file *file, void *fh, enum v4l2_buf_type i)
1302 {
1303         struct mxc_vout_output *vout = fh;
1304         struct videobuf_queue *q = &vout->vbq;
1305         int ret;
1306
1307         del_timer(&vout->timer);
1308
1309         cancel_work_sync(&vout->disp_work);
1310         flush_workqueue(vout->v4l_wq);
1311
1312         release_disp_output(vout);
1313
1314         ret = videobuf_streamoff(&vout->vbq);
1315         if (ret < 0)
1316                 goto err;
1317
1318         ret = videobuf_mmap_free(q);
1319
1320 err:
1321         return ret;
1322 }
1323
1324 static const struct v4l2_ioctl_ops mxc_vout_ioctl_ops = {
1325         .vidioc_querycap                        = mxc_vidioc_querycap,
1326         .vidioc_enum_fmt_vid_out                = mxc_vidioc_enum_fmt_vid_out,
1327         .vidioc_g_fmt_vid_out                   = mxc_vidioc_g_fmt_vid_out,
1328         .vidioc_s_fmt_vid_out                   = mxc_vidioc_s_fmt_vid_out,
1329         .vidioc_cropcap                         = mxc_vidioc_cropcap,
1330         .vidioc_g_crop                          = mxc_vidioc_g_crop,
1331         .vidioc_s_crop                          = mxc_vidioc_s_crop,
1332         .vidioc_queryctrl                       = mxc_vidioc_queryctrl,
1333         .vidioc_g_ctrl                          = mxc_vidioc_g_ctrl,
1334         .vidioc_s_ctrl                          = mxc_vidioc_s_ctrl,
1335         .vidioc_reqbufs                         = mxc_vidioc_reqbufs,
1336         .vidioc_querybuf                        = mxc_vidioc_querybuf,
1337         .vidioc_qbuf                            = mxc_vidioc_qbuf,
1338         .vidioc_dqbuf                           = mxc_vidioc_dqbuf,
1339         .vidioc_streamon                        = mxc_vidioc_streamon,
1340         .vidioc_streamoff                       = mxc_vidioc_streamoff,
1341 };
1342
1343 static const struct v4l2_file_operations mxc_vout_fops = {
1344         .owner          = THIS_MODULE,
1345         .unlocked_ioctl = video_ioctl2,
1346         .mmap           = mxc_vout_mmap,
1347         .open           = mxc_vout_open,
1348         .release        = mxc_vout_release,
1349 };
1350
1351 static struct video_device mxc_vout_template = {
1352         .name           = "MXC Video Output",
1353         .fops           = &mxc_vout_fops,
1354         .ioctl_ops      = &mxc_vout_ioctl_ops,
1355         .release        = video_device_release,
1356 };
1357
1358 static struct videobuf_queue_ops mxc_vout_vbq_ops = {
1359         .buf_setup = mxc_vout_buffer_setup,
1360         .buf_prepare = mxc_vout_buffer_prepare,
1361         .buf_release = mxc_vout_buffer_release,
1362         .buf_queue = mxc_vout_buffer_queue,
1363 };
1364
1365 static void mxc_vout_free_output(struct mxc_vout_dev *dev)
1366 {
1367         int i;
1368         struct mxc_vout_output *vout;
1369         struct video_device *vfd;
1370
1371         for (i = 0; i < dev->out_num; i++) {
1372                 vout = dev->out[i];
1373                 vfd = vout->vfd;
1374                 if (vfd) {
1375                         if (!video_is_registered(vfd))
1376                                 video_device_release(vfd);
1377                         else
1378                                 video_unregister_device(vfd);
1379                 }
1380                 kfree(vout);
1381         }
1382 }
1383
1384 static int __init mxc_vout_setup_output(struct mxc_vout_dev *dev)
1385 {
1386         struct videobuf_queue *q;
1387         struct fb_info *fbi;
1388         struct mxc_vout_output *vout;
1389         int i, ret = 0;
1390
1391         update_display_setting();
1392
1393         /* all output/overlay based on fb */
1394         for (i = 0; i < num_registered_fb; i++) {
1395                 fbi = registered_fb[i];
1396
1397                 vout = kzalloc(sizeof(struct mxc_vout_output), GFP_KERNEL);
1398                 if (!vout) {
1399                         ret = -ENOMEM;
1400                         break;
1401                 }
1402
1403                 dev->out[dev->out_num] = vout;
1404                 dev->out_num++;
1405
1406                 vout->fbi = fbi;
1407                 vout->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
1408                 vout->vfd = video_device_alloc();
1409                 if (!vout->vfd) {
1410                         ret = -ENOMEM;
1411                         break;
1412                 }
1413
1414                 *vout->vfd = mxc_vout_template;
1415                 vout->vfd->debug = debug;
1416                 vout->vfd->v4l2_dev = &dev->v4l2_dev;
1417                 vout->vfd->lock = &vout->mutex;
1418
1419                 mutex_init(&vout->mutex);
1420                 mutex_init(&vout->task_lock);
1421
1422                 strlcpy(vout->vfd->name, fbi->fix.id, sizeof(vout->vfd->name));
1423
1424                 video_set_drvdata(vout->vfd, vout);
1425
1426                 if (video_register_device(vout->vfd,
1427                         VFL_TYPE_GRABBER, video_nr + i) < 0) {
1428                         ret = -ENODEV;
1429                         break;
1430                 }
1431
1432                 q = &vout->vbq;
1433                 q->dev = dev->dev;
1434                 spin_lock_init(&vout->vbq_lock);
1435                 videobuf_queue_dma_contig_init(q, &mxc_vout_vbq_ops, q->dev,
1436                                 &vout->vbq_lock, vout->type, V4L2_FIELD_NONE,
1437                                 sizeof(struct videobuf_buffer), vout, NULL);
1438
1439                 v4l2_info(vout->vfd->v4l2_dev, "V4L2 device registered as %s\n",
1440                                 video_device_node_name(vout->vfd));
1441
1442         }
1443
1444         return ret;
1445 }
1446
1447 static int mxc_vout_probe(struct platform_device *pdev)
1448 {
1449         int ret;
1450         struct mxc_vout_dev *dev;
1451
1452         dev = kzalloc(sizeof(*dev), GFP_KERNEL);
1453         if (!dev)
1454                 return -ENOMEM;
1455
1456         dev->dev = &pdev->dev;
1457         dev->dev->dma_mask = kmalloc(sizeof(*dev->dev->dma_mask), GFP_KERNEL);
1458         *dev->dev->dma_mask = DMA_BIT_MASK(32);
1459         dev->dev->coherent_dma_mask = DMA_BIT_MASK(32);
1460
1461         ret = v4l2_device_register(dev->dev, &dev->v4l2_dev);
1462         if (ret) {
1463                 dev_err(dev->dev, "v4l2_device_register failed\n");
1464                 goto free_dev;
1465         }
1466
1467         ret = mxc_vout_setup_output(dev);
1468         if (ret < 0)
1469                 goto rel_vdev;
1470
1471         return 0;
1472
1473 rel_vdev:
1474         mxc_vout_free_output(dev);
1475         v4l2_device_unregister(&dev->v4l2_dev);
1476 free_dev:
1477         kfree(dev);
1478         return ret;
1479 }
1480
1481 static int mxc_vout_remove(struct platform_device *pdev)
1482 {
1483         struct v4l2_device *v4l2_dev = platform_get_drvdata(pdev);
1484         struct mxc_vout_dev *dev = container_of(v4l2_dev, struct
1485                         mxc_vout_dev, v4l2_dev);
1486
1487         mxc_vout_free_output(dev);
1488         v4l2_device_unregister(v4l2_dev);
1489         kfree(dev);
1490         return 0;
1491 }
1492
1493 static struct platform_driver mxc_vout_driver = {
1494         .driver = {
1495                 .name = "mxc_v4l2_output",
1496         },
1497         .probe = mxc_vout_probe,
1498         .remove = mxc_vout_remove,
1499 };
1500
1501 static int __init mxc_vout_init(void)
1502 {
1503         if (platform_driver_register(&mxc_vout_driver) != 0) {
1504                 printk(KERN_ERR VOUT_NAME ":Could not register Video driver\n");
1505                 return -EINVAL;
1506         }
1507         return 0;
1508 }
1509
1510 static void mxc_vout_cleanup(void)
1511 {
1512         platform_driver_unregister(&mxc_vout_driver);
1513 }
1514
1515 module_init(mxc_vout_init);
1516 module_exit(mxc_vout_cleanup);
1517
1518 MODULE_AUTHOR("Freescale Semiconductor, Inc.");
1519 MODULE_DESCRIPTION("V4L2-driver for MXC video output");
1520 MODULE_LICENSE("GPL");