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

   1 /*
   2  * Copyright (C) 2011-2012 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 #define VALID_HEIGHT_1080P      (1080)
  34 #define FRAME_HEIGHT_1080P      (1088)
  35 #define FRAME_WIDTH_1080P       (1920)
  36 #define MAX_INTERLACED_WIDTH    (1024)
  37 #define CHECK_TILED_1080P_DISPLAY(vout) \
  38         (((vout)->task.input.format == IPU_PIX_FMT_TILED_NV12) &&       \
  39                ((vout)->task.input.width == FRAME_WIDTH_1080P) &&       \
  40                ((vout)->task.output.crop.w == FRAME_WIDTH_1080P) &&     \
  41                ((vout)->task.input.height == FRAME_HEIGHT_1080P) &&     \
  42                ((vout)->task.output.crop.h == VALID_HEIGHT_1080P))
  43 #define CHECK_TILED_1080P_STREAM(vout)  \
  44         (((vout)->task.input.format == IPU_PIX_FMT_TILED_NV12) &&       \
  45                ((vout)->task.input.width == FRAME_WIDTH_1080P) &&       \
  46                ((vout)->task.input.crop.w == FRAME_WIDTH_1080P) &&      \
  47                ((vout)->task.input.height == FRAME_HEIGHT_1080P) &&     \
  48                ((vout)->task.input.crop.h == FRAME_HEIGHT_1080P))
  49
  50 struct mxc_vout_fb {
  51         char *name;
  52         int ipu_id;
  53         struct v4l2_rect crop_bounds;
  54         unsigned int disp_fmt;
  55         bool disp_support_csc;
  56         bool disp_support_windows;
  57 };
  58
  59 struct mxc_vout_output {
  60         int open_cnt;
  61         struct fb_info *fbi;
  62         struct video_device *vfd;
  63         struct mutex mutex;
  64         struct mutex task_lock;
  65         enum v4l2_buf_type type;
  66
  67         struct videobuf_queue vbq;
  68         spinlock_t vbq_lock;
  69
  70         struct list_head queue_list;
  71         struct list_head active_list;
  72
  73         struct v4l2_rect crop_bounds;
  74         unsigned int disp_fmt;
  75         struct mxcfb_pos win_pos;
  76         bool disp_support_windows;
  77         bool disp_support_csc;
  78
  79         bool fmt_init;
  80         bool bypass_pp;
  81         bool is_vdoaipu_task;
  82         struct ipu_task task;
  83         struct ipu_task vdoa_task;
  84         struct vdoa_mem {
  85                 void *vaddr;
  86                 dma_addr_t paddr;
  87                 size_t size;
  88         } vdoa_dma;
  89
  90         bool timer_stop;
  91         struct timer_list timer;
  92         struct workqueue_struct *v4l_wq;
  93         struct work_struct disp_work;
  94         unsigned long frame_count;
  95         unsigned long start_jiffies;
  96
  97         int ctrl_rotate;
  98         int ctrl_vflip;
  99         int ctrl_hflip;
 100
 101         dma_addr_t disp_bufs[FB_BUFS];
 102
 103         struct videobuf_buffer *pre_vb;
 104 };
 105
 106 struct mxc_vout_dev {
 107         struct device   *dev;
 108         struct v4l2_device v4l2_dev;
 109         struct mxc_vout_output *out[MAX_FB_NUM];
 110         int out_num;
 111 };
 112
 113 /* Driver Configuration macros */
 114 #define VOUT_NAME               "mxc_vout"
 115
 116 /* Variables configurable through module params*/
 117 static int debug;
 118 static int video_nr = 16;
 119
 120 /* Module parameters */
 121 module_param(video_nr, int, S_IRUGO);
 122 MODULE_PARM_DESC(video_nr, "video device numbers");
 123 module_param(debug, int, 0600);
 124 MODULE_PARM_DESC(debug, "Debug level (0-1)");
 125
 126 const static struct v4l2_fmtdesc mxc_formats[] = {
 127         {
 128                 .description = "RGB565",
 129                 .pixelformat = V4L2_PIX_FMT_RGB565,
 130         },
 131         {
 132                 .description = "BGR24",
 133                 .pixelformat = V4L2_PIX_FMT_BGR24,
 134         },
 135         {
 136                 .description = "RGB24",
 137                 .pixelformat = V4L2_PIX_FMT_RGB24,
 138         },
 139         {
 140                 .description = "RGB32",
 141                 .pixelformat = V4L2_PIX_FMT_RGB32,
 142         },
 143         {
 144                 .description = "BGR32",
 145                 .pixelformat = V4L2_PIX_FMT_BGR32,
 146         },
 147         {
 148                 .description = "NV12",
 149                 .pixelformat = V4L2_PIX_FMT_NV12,
 150         },
 151         {
 152                 .description = "UYVY",
 153                 .pixelformat = V4L2_PIX_FMT_UYVY,
 154         },
 155         {
 156                 .description = "YUYV",
 157                 .pixelformat = V4L2_PIX_FMT_YUYV,
 158         },
 159         {
 160                 .description = "YUV422 planar",
 161                 .pixelformat = V4L2_PIX_FMT_YUV422P,
 162         },
 163         {
 164                 .description = "YUV444",
 165                 .pixelformat = V4L2_PIX_FMT_YUV444,
 166         },
 167         {
 168                 .description = "YUV420",
 169                 .pixelformat = V4L2_PIX_FMT_YUV420,
 170         },
 171         {
 172                 .description = "TILED NV12P",
 173                 .pixelformat = IPU_PIX_FMT_TILED_NV12,
 174         },
 175         {
 176                 .description = "TILED NV12F",
 177                 .pixelformat = IPU_PIX_FMT_TILED_NV12F,
 178         },
 179 };
 180
 181 #define NUM_MXC_VOUT_FORMATS (ARRAY_SIZE(mxc_formats))
 182
 183 #define DEF_INPUT_WIDTH         320
 184 #define DEF_INPUT_HEIGHT        240
 185
 186 static int mxc_vidioc_streamoff(struct file *file, void *fh, enum v4l2_buf_type i);
 187
 188 static struct mxc_vout_fb g_fb_setting[MAX_FB_NUM];
 189 static int config_disp_output(struct mxc_vout_output *vout);
 190 static void release_disp_output(struct mxc_vout_output *vout);
 191
 192 static unsigned int get_frame_size(struct mxc_vout_output *vout)
 193 {
 194         unsigned int size;
 195
 196         if (IPU_PIX_FMT_TILED_NV12 == vout->task.input.format)
 197                 size = TILED_NV12_FRAME_SIZE(vout->task.input.width,
 198                                         vout->task.input.height);
 199         else if (IPU_PIX_FMT_TILED_NV12F == vout->task.input.format) {
 200                 size = TILED_NV12_FRAME_SIZE(vout->task.input.width,
 201                                         vout->task.input.height/2);
 202                 size *= 2;
 203         } else
 204                 size = vout->task.input.width * vout->task.input.height *
 205                                 fmt_to_bpp(vout->task.input.format)/8;
 206
 207         return size;
 208 }
 209
 210 static ipu_channel_t get_ipu_channel(struct fb_info *fbi)
 211 {
 212         ipu_channel_t ipu_ch = CHAN_NONE;
 213         mm_segment_t old_fs;
 214
 215         if (fbi->fbops->fb_ioctl) {
 216                 old_fs = get_fs();
 217                 set_fs(KERNEL_DS);
 218                 fbi->fbops->fb_ioctl(fbi, MXCFB_GET_FB_IPU_CHAN,
 219                                 (unsigned long)&ipu_ch);
 220                 set_fs(old_fs);
 221         }
 222
 223         return ipu_ch;
 224 }
 225
 226 static unsigned int get_ipu_fmt(struct fb_info *fbi)
 227 {
 228         mm_segment_t old_fs;
 229         unsigned int fb_fmt;
 230
 231         if (fbi->fbops->fb_ioctl) {
 232                 old_fs = get_fs();
 233                 set_fs(KERNEL_DS);
 234                 fbi->fbops->fb_ioctl(fbi, MXCFB_GET_DIFMT,
 235                                 (unsigned long)&fb_fmt);
 236                 set_fs(old_fs);
 237         }
 238
 239         return fb_fmt;
 240 }
 241
 242 static void update_display_setting(void)
 243 {
 244         int i;
 245         struct fb_info *fbi;
 246         struct v4l2_rect bg_crop_bounds[2];
 247
 248         for (i = 0; i < num_registered_fb; i++) {
 249                 fbi = registered_fb[i];
 250
 251                 memset(&g_fb_setting[i], 0, sizeof(struct mxc_vout_fb));
 252
 253                 if (!strncmp(fbi->fix.id, "DISP3", 5))
 254                         g_fb_setting[i].ipu_id = 0;
 255                 else
 256                         g_fb_setting[i].ipu_id = 1;
 257
 258                 g_fb_setting[i].name = fbi->fix.id;
 259                 g_fb_setting[i].crop_bounds.left = 0;
 260                 g_fb_setting[i].crop_bounds.top = 0;
 261                 g_fb_setting[i].crop_bounds.width = fbi->var.xres;
 262                 g_fb_setting[i].crop_bounds.height = fbi->var.yres;
 263                 g_fb_setting[i].disp_fmt = get_ipu_fmt(fbi);
 264
 265                 if (get_ipu_channel(fbi) == MEM_BG_SYNC) {
 266                         bg_crop_bounds[g_fb_setting[i].ipu_id] =
 267                                 g_fb_setting[i].crop_bounds;
 268                         g_fb_setting[i].disp_support_csc = true;
 269                 } else if (get_ipu_channel(fbi) == MEM_FG_SYNC) {
 270                         g_fb_setting[i].disp_support_csc = true;
 271                         g_fb_setting[i].disp_support_windows = true;
 272                 }
 273         }
 274
 275         for (i = 0; i < num_registered_fb; i++) {
 276                 fbi = registered_fb[i];
 277
 278                 if (get_ipu_channel(fbi) == MEM_FG_SYNC)
 279                         g_fb_setting[i].crop_bounds =
 280                                 bg_crop_bounds[g_fb_setting[i].ipu_id];
 281         }
 282 }
 283
 284 /* called after g_fb_setting filled by update_display_setting */
 285 static int update_setting_from_fbi(struct mxc_vout_output *vout,
 286                         struct fb_info *fbi)
 287 {
 288         int i;
 289         bool found = false;
 290
 291         for (i = 0; i < MAX_FB_NUM; i++) {
 292                 if (g_fb_setting[i].name) {
 293                         if (!strcmp(fbi->fix.id, g_fb_setting[i].name)) {
 294                                 vout->crop_bounds = g_fb_setting[i].crop_bounds;
 295                                 vout->disp_fmt = g_fb_setting[i].disp_fmt;
 296                                 vout->disp_support_csc = g_fb_setting[i].disp_support_csc;
 297                                 vout->disp_support_windows =
 298                                         g_fb_setting[i].disp_support_windows;
 299                                 found = true;
 300                                 break;
 301                         }
 302                 }
 303         }
 304
 305         if (!found) {
 306                 v4l2_err(vout->vfd->v4l2_dev, "can not find output\n");
 307                 return -EINVAL;
 308         }
 309         strlcpy(vout->vfd->name, fbi->fix.id, sizeof(vout->vfd->name));
 310
 311         memset(&vout->task, 0, sizeof(struct ipu_task));
 312
 313         vout->task.input.width = DEF_INPUT_WIDTH;
 314         vout->task.input.height = DEF_INPUT_HEIGHT;
 315         vout->task.input.crop.pos.x = 0;
 316         vout->task.input.crop.pos.y = 0;
 317         vout->task.input.crop.w = DEF_INPUT_WIDTH;
 318         vout->task.input.crop.h = DEF_INPUT_HEIGHT;
 319
 320         vout->task.output.width = vout->crop_bounds.width;
 321         vout->task.output.height = vout->crop_bounds.height;
 322         vout->task.output.crop.pos.x = 0;
 323         vout->task.output.crop.pos.y = 0;
 324         vout->task.output.crop.w = vout->crop_bounds.width;
 325         vout->task.output.crop.h = vout->crop_bounds.height;
 326         if (colorspaceofpixel(vout->disp_fmt) == YUV_CS)
 327                 vout->task.output.format = IPU_PIX_FMT_UYVY;
 328         else
 329                 vout->task.output.format = IPU_PIX_FMT_RGB565;
 330
 331         return 0;
 332 }
 333
 334 static inline unsigned long get_jiffies(struct timeval *t)
 335 {
 336         struct timeval cur;
 337
 338         if (t->tv_usec >= 1000000) {
 339                 t->tv_sec += t->tv_usec / 1000000;
 340                 t->tv_usec = t->tv_usec % 1000000;
 341         }
 342
 343         do_gettimeofday(&cur);
 344         if ((t->tv_sec < cur.tv_sec)
 345             || ((t->tv_sec == cur.tv_sec) && (t->tv_usec < cur.tv_usec)))
 346                 return jiffies;
 347
 348         if (t->tv_usec < cur.tv_usec) {
 349                 cur.tv_sec = t->tv_sec - cur.tv_sec - 1;
 350                 cur.tv_usec = t->tv_usec + 1000000 - cur.tv_usec;
 351         } else {
 352                 cur.tv_sec = t->tv_sec - cur.tv_sec;
 353                 cur.tv_usec = t->tv_usec - cur.tv_usec;
 354         }
 355
 356         return jiffies + timeval_to_jiffies(&cur);
 357 }
 358
 359 static bool deinterlace_3_field(struct mxc_vout_output *vout)
 360 {
 361         return (vout->task.input.deinterlace.enable &&
 362                 (vout->task.input.deinterlace.motion != HIGH_MOTION));
 363 }
 364
 365 static bool is_pp_bypass(struct mxc_vout_output *vout)
 366 {
 367         if ((IPU_PIX_FMT_TILED_NV12 == vout->task.input.format) ||
 368                 (IPU_PIX_FMT_TILED_NV12F == vout->task.input.format))
 369                 return false;
 370         if ((vout->task.input.width == vout->task.output.width) &&
 371                 (vout->task.input.height == vout->task.output.height) &&
 372                 (vout->task.input.crop.w == vout->task.output.crop.w) &&
 373                 (vout->task.input.crop.h == vout->task.output.crop.h) &&
 374                 (vout->task.output.rotate < IPU_ROTATE_HORIZ_FLIP) &&
 375                 !vout->task.input.deinterlace.enable) {
 376                 if (vout->disp_support_csc)
 377                         return true;
 378                 else if (!need_csc(vout->task.input.format, vout->disp_fmt))
 379                         return true;
 380         /* input crop show to full output which can show based on xres_virtual/yres_virtual */
 381         } else if ((vout->task.input.crop.w == vout->task.output.crop.w) &&
 382                         (vout->task.output.crop.w == vout->task.output.width) &&
 383                         (vout->task.input.crop.h == vout->task.output.crop.h) &&
 384                         (vout->task.output.crop.h == vout->task.output.height) &&
 385                         (vout->task.output.rotate < IPU_ROTATE_HORIZ_FLIP) &&
 386                         !vout->task.input.deinterlace.enable) {
 387                 if (vout->disp_support_csc)
 388                         return true;
 389                 else if (!need_csc(vout->task.input.format, vout->disp_fmt))
 390                         return true;
 391         }
 392         return false;
 393 }
 394
 395 static void setup_buf_timer(struct mxc_vout_output *vout,
 396                         struct videobuf_buffer *vb)
 397 {
 398         unsigned long timeout;
 399
 400         /* if timestamp is 0, then default to 30fps */
 401         if ((vb->ts.tv_sec == 0)
 402                         && (vb->ts.tv_usec == 0)
 403                         && vout->start_jiffies)
 404                 timeout =
 405                         vout->start_jiffies + vout->frame_count * HZ / 30;
 406         else
 407                 timeout = get_jiffies(&vb->ts);
 408
 409         if (jiffies >= timeout) {
 410                 v4l2_dbg(1, debug, vout->vfd->v4l2_dev,
 411                                 "warning: timer timeout already expired.\n");
 412         }
 413
 414         if (mod_timer(&vout->timer, timeout)) {
 415                 v4l2_warn(vout->vfd->v4l2_dev,
 416                                 "warning: timer was already set\n");
 417         }
 418
 419         v4l2_dbg(1, debug, vout->vfd->v4l2_dev,
 420                         "timer handler next schedule: %lu\n", timeout);
 421 }
 422
 423 static int show_buf(struct mxc_vout_output *vout, int idx,
 424         struct ipu_pos *ipos)
 425 {
 426         struct fb_info *fbi = vout->fbi;
 427         struct fb_var_screeninfo var;
 428         int ret;
 429         u32 is_1080p;
 430         u32 yres = 0;
 431
 432         memcpy(&var, &fbi->var, sizeof(var));
 433
 434         if (vout->bypass_pp) {
 435                 /*
 436                  * crack fb base
 437                  * NOTE: should not do other fb operation during v4l2
 438                  */
 439                 console_lock();
 440                 fbi->fix.smem_start = vout->task.output.paddr;
 441                 fbi->var.yoffset = ipos->y + 1;
 442                 var.xoffset = ipos->x;
 443                 var.yoffset = ipos->y;
 444                 ret = fb_pan_display(fbi, &var);
 445                 console_unlock();
 446         } else {
 447                 console_lock();
 448                 is_1080p = CHECK_TILED_1080P_DISPLAY(vout);
 449                 if (is_1080p) {
 450                         yres = fbi->var.yres;
 451                         fbi->var.yres = FRAME_HEIGHT_1080P;
 452                 }
 453
 454                 var.yoffset = idx * fbi->var.yres;
 455                 ret = fb_pan_display(fbi, &var);
 456                 if (is_1080p)
 457                         fbi->var.yres = yres;
 458                 console_unlock();
 459         }
 460
 461         return ret;
 462 }
 463
 464 static void disp_work_func(struct work_struct *work)
 465 {
 466         struct mxc_vout_output *vout =
 467                 container_of(work, struct mxc_vout_output, disp_work);
 468         struct videobuf_queue *q = &vout->vbq;
 469         struct videobuf_buffer *vb, *vb_next = NULL;
 470         unsigned long flags = 0;
 471         struct ipu_pos ipos;
 472         int ret = 0;
 473         u32 is_1080p;
 474         u32 ocrop_h = 0;
 475         u32 tiled_interlaced = 0;
 476
 477         v4l2_dbg(1, debug, vout->vfd->v4l2_dev, "disp work begin one frame\n");
 478
 479         spin_lock_irqsave(q->irqlock, flags);
 480
 481         if (deinterlace_3_field(vout)) {
 482                 if (list_is_singular(&vout->active_list)) {
 483                         v4l2_warn(vout->vfd->v4l2_dev,
 484                                         "deinterlacing: no enough entry in active_list\n");
 485                         spin_unlock_irqrestore(q->irqlock, flags);
 486                         return;
 487                 }
 488         } else {
 489                 if (list_empty(&vout->active_list)) {
 490                         v4l2_warn(vout->vfd->v4l2_dev,
 491                                         "no entry in active_list, should not be here\n");
 492                         spin_unlock_irqrestore(q->irqlock, flags);
 493                         return;
 494                 }
 495         }
 496         vb = list_first_entry(&vout->active_list,
 497                         struct videobuf_buffer, queue);
 498
 499         if (deinterlace_3_field(vout))
 500                 vb_next = list_first_entry(vout->active_list.next,
 501                                 struct videobuf_buffer, queue);
 502
 503         spin_unlock_irqrestore(q->irqlock, flags);
 504
 505         mutex_lock(&vout->task_lock);
 506
 507         if (vb->memory == V4L2_MEMORY_USERPTR)
 508                 vout->task.input.paddr = vb->baddr;
 509         else
 510                 vout->task.input.paddr = videobuf_to_dma_contig(vb);
 511
 512         if (vout->bypass_pp) {
 513                 vout->task.output.paddr = vout->task.input.paddr;
 514                 ipos.x = vout->task.input.crop.pos.x;
 515                 ipos.y = vout->task.input.crop.pos.y;
 516         } else {
 517                 if (deinterlace_3_field(vout)) {
 518                         if (vb->memory == V4L2_MEMORY_USERPTR)
 519                                 vout->task.input.paddr_n = vb_next->baddr;
 520                         else
 521                                 vout->task.input.paddr_n =
 522                                         videobuf_to_dma_contig(vb_next);
 523                 }
 524                 vout->task.output.paddr =
 525                         vout->disp_bufs[vout->frame_count % FB_BUFS];
 526
 527                 is_1080p = CHECK_TILED_1080P_DISPLAY(vout);
 528                 if (is_1080p) {
 529                         vout->task.input.crop.h = FRAME_HEIGHT_1080P;
 530                         vout->task.output.height = FRAME_HEIGHT_1080P;
 531                         ocrop_h = vout->task.output.crop.h;
 532                         vout->task.output.crop.h = FRAME_HEIGHT_1080P;
 533                 }
 534                 if (vout->is_vdoaipu_task) {
 535                         vout->vdoa_task.input.paddr = vout->task.input.paddr;
 536                         if (deinterlace_3_field(vout))
 537                                 vout->vdoa_task.input.paddr_n =
 538                                                 vout->task.input.paddr_n;
 539                         vout->vdoa_task.output.paddr = vout->vdoa_dma.paddr;
 540                         ret = ipu_queue_task(&vout->vdoa_task);
 541                         if (ret < 0) {
 542                                 mutex_unlock(&vout->task_lock);
 543                                 goto err;
 544                         }
 545                         vout->task.input.paddr = vout->vdoa_task.output.paddr;
 546                         if (vout->task.input.deinterlace.enable) {
 547                                 tiled_interlaced = 1;
 548                                 vout->task.input.deinterlace.enable = 0;
 549                         }
 550                 }
 551                 ret = ipu_queue_task(&vout->task);
 552                 if (tiled_interlaced)
 553                         vout->task.input.deinterlace.enable = 1;
 554                 if (ret < 0) {
 555                         mutex_unlock(&vout->task_lock);
 556                         goto err;
 557                 }
 558                 if (is_1080p)
 559                         vout->task.output.crop.h = ocrop_h;
 560         }
 561
 562         mutex_unlock(&vout->task_lock);
 563
 564         ret = show_buf(vout, vout->frame_count % FB_BUFS, &ipos);
 565         if (ret < 0)
 566                 v4l2_dbg(1, debug, vout->vfd->v4l2_dev, "show buf with ret %d\n", ret);
 567
 568         spin_lock_irqsave(q->irqlock, flags);
 569
 570         list_del(&vb->queue);
 571
 572         /*
 573          * previous videobuf finish show, set VIDEOBUF_DONE state here
 574          * to avoid tearing issue in pp bypass case, which make sure
 575          * showing buffer will not be dequeue to write new data. It also
 576          * bring side-effect that the last buffer can not be dequeue
 577          * correctly, app need take care about it.
 578          */
 579         if (vout->pre_vb) {
 580                 vout->pre_vb->state = VIDEOBUF_DONE;
 581                 wake_up_interruptible(&vout->pre_vb->done);
 582         }
 583
 584         if (vout->bypass_pp)
 585                 vout->pre_vb = vb;
 586         else {
 587                 vout->pre_vb = NULL;
 588                 vb->state = VIDEOBUF_DONE;
 589                 wake_up_interruptible(&vb->done);
 590         }
 591
 592         vout->frame_count++;
 593
 594         /* pick next queue buf to setup timer */
 595         if (list_empty(&vout->queue_list))
 596                 vout->timer_stop = true;
 597         else {
 598                 vb = list_first_entry(&vout->queue_list,
 599                                 struct videobuf_buffer, queue);
 600                 setup_buf_timer(vout, vb);
 601         }
 602
 603         spin_unlock_irqrestore(q->irqlock, flags);
 604
 605         v4l2_dbg(1, debug, vout->vfd->v4l2_dev, "disp work finish one frame\n");
 606
 607         return;
 608 err:
 609         v4l2_err(vout->vfd->v4l2_dev, "display work fail ret = %d\n", ret);
 610         vout->timer_stop = true;
 611         vb->state = VIDEOBUF_ERROR;
 612         return;
 613 }
 614
 615 static void mxc_vout_timer_handler(unsigned long arg)
 616 {
 617         struct mxc_vout_output *vout =
 618                         (struct mxc_vout_output *) arg;
 619         struct videobuf_queue *q = &vout->vbq;
 620         struct videobuf_buffer *vb;
 621         unsigned long flags = 0;
 622
 623         spin_lock_irqsave(q->irqlock, flags);
 624
 625         /*
 626          * put first queued entry into active, if previous entry did not
 627          * finish, setup current entry's timer again.
 628          */
 629         if (list_empty(&vout->queue_list)) {
 630                 spin_unlock_irqrestore(q->irqlock, flags);
 631                 return;
 632         }
 633
 634         /* move videobuf from queued list to active list */
 635         vb = list_first_entry(&vout->queue_list,
 636                         struct videobuf_buffer, queue);
 637         list_del(&vb->queue);
 638         list_add_tail(&vb->queue, &vout->active_list);
 639
 640         if (queue_work(vout->v4l_wq, &vout->disp_work) == 0) {
 641                 v4l2_warn(vout->vfd->v4l2_dev,
 642                         "disp work was in queue already, queue buf again next time\n");
 643                 list_del(&vb->queue);
 644                 list_add(&vb->queue, &vout->queue_list);
 645                 spin_unlock_irqrestore(q->irqlock, flags);
 646                 return;
 647         }
 648
 649         vb->state = VIDEOBUF_ACTIVE;
 650
 651         spin_unlock_irqrestore(q->irqlock, flags);
 652 }
 653
 654 /* Video buffer call backs */
 655
 656 /*
 657  * Buffer setup function is called by videobuf layer when REQBUF ioctl is
 658  * called. This is used to setup buffers and return size and count of
 659  * buffers allocated. After the call to this buffer, videobuf layer will
 660  * setup buffer queue depending on the size and count of buffers
 661  */
 662 static int mxc_vout_buffer_setup(struct videobuf_queue *q, unsigned int *count,
 663                           unsigned int *size)
 664 {
 665         struct mxc_vout_output *vout = q->priv_data;
 666         unsigned int frame_size;
 667
 668         if (!vout)
 669                 return -EINVAL;
 670
 671         if (V4L2_BUF_TYPE_VIDEO_OUTPUT != q->type)
 672                 return -EINVAL;
 673
 674         frame_size = get_frame_size(vout);
 675         *size = PAGE_ALIGN(frame_size);
 676
 677         return 0;
 678 }
 679
 680 /*
 681  * This function will be called when VIDIOC_QBUF ioctl is called.
 682  * It prepare buffers before give out for the display. This function
 683  * converts user space virtual address into physical address if userptr memory
 684  * exchange mechanism is used.
 685  */
 686 static int mxc_vout_buffer_prepare(struct videobuf_queue *q,
 687                             struct videobuf_buffer *vb,
 688                             enum v4l2_field field)
 689 {
 690         vb->state = VIDEOBUF_PREPARED;
 691         return 0;
 692 }
 693
 694 /*
 695  * Buffer queue funtion will be called from the videobuf layer when _QBUF
 696  * ioctl is called. It is used to enqueue buffer, which is ready to be
 697  * displayed.
 698  * This function is protected by q->irqlock.
 699  */
 700 static void mxc_vout_buffer_queue(struct videobuf_queue *q,
 701                           struct videobuf_buffer *vb)
 702 {
 703         struct mxc_vout_output *vout = q->priv_data;
 704
 705         list_add_tail(&vb->queue, &vout->queue_list);
 706         vb->state = VIDEOBUF_QUEUED;
 707
 708         if (vout->timer_stop) {
 709                 if (deinterlace_3_field(vout) &&
 710                         list_empty(&vout->active_list)) {
 711                         vb = list_first_entry(&vout->queue_list,
 712                                         struct videobuf_buffer, queue);
 713                         list_del(&vb->queue);
 714                         list_add_tail(&vb->queue, &vout->active_list);
 715                 } else {
 716                         setup_buf_timer(vout, vb);
 717                         vout->timer_stop = false;
 718                 }
 719         }
 720 }
 721
 722 /*
 723  * Buffer release function is called from videobuf layer to release buffer
 724  * which are already allocated
 725  */
 726 static void mxc_vout_buffer_release(struct videobuf_queue *q,
 727                             struct videobuf_buffer *vb)
 728 {
 729         vb->state = VIDEOBUF_NEEDS_INIT;
 730 }
 731
 732 static int mxc_vout_mmap(struct file *file, struct vm_area_struct *vma)
 733 {
 734         int ret;
 735         struct mxc_vout_output *vout = file->private_data;
 736
 737         if (!vout)
 738                 return -ENODEV;
 739
 740         ret = videobuf_mmap_mapper(&vout->vbq, vma);
 741         if (ret < 0)
 742                 v4l2_err(vout->vfd->v4l2_dev,
 743                                 "offset invalid [offset=0x%lx]\n",
 744                                 (vma->vm_pgoff << PAGE_SHIFT));
 745
 746         return ret;
 747 }
 748
 749 static int mxc_vout_release(struct file *file)
 750 {
 751         unsigned int ret = 0;
 752         struct videobuf_queue *q;
 753         struct mxc_vout_output *vout = file->private_data;
 754
 755         if (!vout)
 756                 return 0;
 757
 758         if (--vout->open_cnt == 0) {
 759                 q = &vout->vbq;
 760                 if (q->streaming)
 761                         mxc_vidioc_streamoff(file, vout, vout->type);
 762                 else {
 763                         release_disp_output(vout);
 764                         videobuf_queue_cancel(q);
 765                 }
 766                 destroy_workqueue(vout->v4l_wq);
 767                 ret = videobuf_mmap_free(q);
 768         }
 769
 770         return ret;
 771 }
 772
 773 static int mxc_vout_open(struct file *file)
 774 {
 775         struct mxc_vout_output *vout = NULL;
 776         int ret = 0;
 777
 778         vout = video_drvdata(file);
 779
 780         if (vout == NULL)
 781                 return -ENODEV;
 782
 783         if (vout->open_cnt++ == 0) {
 784                 vout->ctrl_rotate = 0;
 785                 vout->ctrl_vflip = 0;
 786                 vout->ctrl_hflip = 0;
 787                 update_display_setting();
 788                 ret = update_setting_from_fbi(vout, vout->fbi);
 789                 if (ret < 0)
 790                         goto err;
 791
 792                 vout->v4l_wq = create_singlethread_workqueue("v4l2q");
 793                 if (!vout->v4l_wq) {
 794                         v4l2_err(vout->vfd->v4l2_dev,
 795                                         "Could not create work queue\n");
 796                         ret = -ENOMEM;
 797                         goto err;
 798                 }
 799
 800                 INIT_WORK(&vout->disp_work, disp_work_func);
 801
 802                 INIT_LIST_HEAD(&vout->queue_list);
 803                 INIT_LIST_HEAD(&vout->active_list);
 804
 805                 vout->fmt_init = false;
 806                 vout->frame_count = 0;
 807
 808                 vout->win_pos.x = 0;
 809                 vout->win_pos.y = 0;
 810         }
 811
 812         file->private_data = vout;
 813
 814 err:
 815         return ret;
 816 }
 817
 818 /*
 819  * V4L2 ioctls
 820  */
 821 static int mxc_vidioc_querycap(struct file *file, void *fh,
 822                 struct v4l2_capability *cap)
 823 {
 824         struct mxc_vout_output *vout = fh;
 825
 826         strlcpy(cap->driver, VOUT_NAME, sizeof(cap->driver));
 827         strlcpy(cap->card, vout->vfd->name, sizeof(cap->card));
 828         cap->bus_info[0] = '\0';
 829         cap->capabilities = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_OUTPUT;
 830
 831         return 0;
 832 }
 833
 834 static int mxc_vidioc_enum_fmt_vid_out(struct file *file, void *fh,
 835                         struct v4l2_fmtdesc *fmt)
 836 {
 837         if (fmt->index >= NUM_MXC_VOUT_FORMATS)
 838                 return -EINVAL;
 839
 840         strlcpy(fmt->description, mxc_formats[fmt->index].description,
 841                         sizeof(fmt->description));
 842         fmt->pixelformat = mxc_formats[fmt->index].pixelformat;
 843
 844         return 0;
 845 }
 846
 847 static int mxc_vidioc_g_fmt_vid_out(struct file *file, void *fh,
 848                         struct v4l2_format *f)
 849 {
 850         struct mxc_vout_output *vout = fh;
 851         struct v4l2_rect rect;
 852
 853         f->fmt.pix.width = vout->task.input.width;
 854         f->fmt.pix.height = vout->task.input.height;
 855         f->fmt.pix.pixelformat = vout->task.input.format;
 856         f->fmt.pix.sizeimage = get_frame_size(vout);
 857
 858         if (f->fmt.pix.priv) {
 859                 rect.left = vout->task.input.crop.pos.x;
 860                 rect.top = vout->task.input.crop.pos.y;
 861                 rect.width = vout->task.input.crop.w;
 862                 rect.height = vout->task.input.crop.h;
 863                 if (copy_to_user((void __user *)f->fmt.pix.priv,
 864                                 &rect, sizeof(rect)))
 865                         return -EFAULT;
 866         }
 867         v4l2_dbg(1, debug, vout->vfd->v4l2_dev,
 868                         "frame_size:0x%x, pix_fmt:0x%x\n",
 869                         f->fmt.pix.sizeimage,
 870                         vout->task.input.format);
 871
 872         return 0;
 873 }
 874
 875 static inline int ipu_try_task(struct mxc_vout_output *vout)
 876 {
 877         int ret;
 878         struct ipu_task *task = &vout->task;
 879
 880 again:
 881         ret = ipu_check_task(task);
 882         if (ret != IPU_CHECK_OK) {
 883                 if (ret > IPU_CHECK_ERR_MIN) {
 884                         if (ret == IPU_CHECK_ERR_SPLIT_INPUTW_OVER) {
 885                                 task->input.crop.w -= 8;
 886                                 goto again;
 887                         }
 888                         if (ret == IPU_CHECK_ERR_SPLIT_INPUTH_OVER) {
 889                                 task->input.crop.h -= 8;
 890                                 goto again;
 891                         }
 892                         if (ret == IPU_CHECK_ERR_SPLIT_OUTPUTW_OVER) {
 893                                 if (vout->disp_support_windows) {
 894                                         task->output.width -= 8;
 895                                         task->output.crop.w = task->output.width;
 896                                 } else
 897                                         task->output.crop.w -= 8;
 898                                 goto again;
 899                         }
 900                         if (ret == IPU_CHECK_ERR_SPLIT_OUTPUTH_OVER) {
 901                                 if (vout->disp_support_windows) {
 902                                         task->output.height -= 8;
 903                                         task->output.crop.h = task->output.height;
 904                                 } else
 905                                         task->output.crop.h -= 8;
 906                                 goto again;
 907                         }
 908                         ret = -EINVAL;
 909                 }
 910         } else
 911                 ret = 0;
 912
 913         return ret;
 914 }
 915
 916 static inline int vdoaipu_try_task(struct mxc_vout_output *vout)
 917 {
 918         int ret;
 919         u32 icrop_h = 0, icrop_w = 0;
 920         int is_1080p_stream;
 921         size_t size;
 922         struct ipu_task *ipu_task = &vout->task;
 923         struct ipu_task *vdoa_task = &vout->vdoa_task;
 924         u32 deinterlace = 0;
 925
 926         if (vout->task.input.deinterlace.enable)
 927                 deinterlace = 1;
 928         is_1080p_stream = CHECK_TILED_1080P_STREAM(vout);
 929         if (is_1080p_stream)
 930                 ipu_task->input.crop.h = VALID_HEIGHT_1080P;
 931
 932         if (ipu_task->input.crop.h % IPU_PIX_FMT_TILED_NV12_MBALIGN) {
 933                 icrop_h = ipu_task->input.crop.h;
 934                 ipu_task->input.crop.h = ALIGN(ipu_task->input.crop.h,
 935                                                 IPU_PIX_FMT_TILED_NV12_MBALIGN);
 936         }
 937         if (ipu_task->input.crop.w % IPU_PIX_FMT_TILED_NV12_MBALIGN) {
 938                 icrop_w = ipu_task->input.crop.w;
 939                 ipu_task->input.crop.w = ALIGN(ipu_task->input.crop.w,
 940                                                 IPU_PIX_FMT_TILED_NV12_MBALIGN);
 941         }
 942
 943         memset(vdoa_task, 0, sizeof(*vdoa_task));
 944         memcpy(&vdoa_task->input, &ipu_task->input, sizeof(ipu_task->input));
 945         vdoa_task->output.format = IPU_PIX_FMT_NV12;
 946         vdoa_task->output.width = ipu_task->input.crop.w;
 947         vdoa_task->output.height = ipu_task->input.crop.h;
 948         vdoa_task->output.crop.w = ipu_task->input.crop.w;
 949         vdoa_task->output.crop.h = ipu_task->input.crop.h;
 950
 951         size = PAGE_ALIGN(ipu_task->input.crop.w *
 952                                         ipu_task->input.crop.h *
 953                                         fmt_to_bpp(vdoa_task->output.format)/8);
 954         if (size > vout->vdoa_dma.size) {
 955                 if (vout->vdoa_dma.vaddr) {
 956                         dma_free_coherent(vout->vbq.dev, vout->vdoa_dma.size,
 957                                 vout->vdoa_dma.vaddr, vout->vdoa_dma.paddr);
 958                         v4l2_dbg(1, debug, vout->vfd->v4l2_dev,
 959                                         "free vdoa_dma.size:0x%x, paddr:0x%x\n",
 960                                         vout->vdoa_dma.size,
 961                                         vout->vdoa_dma.paddr);
 962                         memset(&vout->vdoa_dma, 0, sizeof(vout->vdoa_dma));
 963                 }
 964                 vout->vdoa_dma.size = size;
 965                 vout->vdoa_dma.vaddr = dma_alloc_coherent(vout->vbq.dev,
 966                                                         vout->vdoa_dma.size,
 967                                                         &vout->vdoa_dma.paddr,
 968                                                         GFP_KERNEL);
 969                 if (!vout->vdoa_dma.vaddr) {
 970                         v4l2_err(vout->vfd->v4l2_dev,
 971                                 "cannot get vdoa dma buf size:0x%x\n", size);
 972                         return -ENOMEM;
 973                 }
 974                 v4l2_dbg(1, debug, vout->vfd->v4l2_dev,
 975                                 "alloc vdoa_dma.size:0x%x, paddr:0x%x\n",
 976                                 vout->vdoa_dma.size,
 977                                 vout->vdoa_dma.paddr);
 978         }
 979         ret = ipu_check_task(vdoa_task);
 980         if (ret != IPU_CHECK_OK)
 981                 return -EINVAL;
 982
 983         ipu_task->input.format = vdoa_task->output.format;
 984         if (icrop_h) {
 985                 ipu_task->input.height = vdoa_task->output.height;
 986                 ipu_task->input.crop.h = icrop_h;
 987         }
 988         if (icrop_w) {
 989                 ipu_task->input.width = vdoa_task->output.width;
 990                 ipu_task->input.crop.w = icrop_w;
 991         }
 992         if (deinterlace)
 993                 ipu_task->input.deinterlace.enable = 0;
 994         ret = ipu_try_task(vout);
 995         if (deinterlace)
 996                 ipu_task->input.deinterlace.enable = 1;
 997
 998         return ret;
 999 }
1000
1001 static int mxc_vout_try_task(struct mxc_vout_output *vout)
1002 {
1003         int ret = 0;
1004         struct ipu_output *output = &vout->task.output;
1005         struct ipu_input *input = &vout->task.input;
1006
1007         input->crop.w -= input->crop.w%8;
1008         input->crop.h -= input->crop.h%8;
1009         /* assume task.output already set by S_CROP */
1010         if (is_pp_bypass(vout)) {
1011                 v4l2_info(vout->vfd->v4l2_dev, "Bypass IC.\n");
1012                 vout->bypass_pp = true;
1013                 output->format = input->format;
1014         } else {
1015                 /* if need CSC, choose IPU-DP or IPU_IC do it */
1016                 vout->bypass_pp = false;
1017                 if (vout->disp_support_csc) {
1018                         if (colorspaceofpixel(input->format) == YUV_CS)
1019                                 output->format = IPU_PIX_FMT_UYVY;
1020                         else
1021                                 output->format = IPU_PIX_FMT_RGB565;
1022                 } else {
1023                         if (colorspaceofpixel(vout->disp_fmt) == YUV_CS)
1024                                 output->format = IPU_PIX_FMT_UYVY;
1025                         else
1026                                 output->format = IPU_PIX_FMT_RGB565;
1027                 }
1028
1029                 vout->is_vdoaipu_task = 0;
1030                 if ((IPU_PIX_FMT_TILED_NV12 == input->format) ||
1031                         (IPU_PIX_FMT_TILED_NV12F == input->format)) {
1032                         /* check resize/rotate/flip, or csc task */
1033                         if ((IPU_ROTATE_NONE != output->rotate) ||
1034                                 (input->crop.w != output->crop.w) ||
1035                                 (input->crop.h != output->crop.h) ||
1036                                 (!vout->disp_support_csc &&
1037                                 (colorspaceofpixel(vout->disp_fmt) == RGB_CS)))
1038                                 vout->is_vdoaipu_task = 1;
1039                         else
1040                                 /* IC bypass */
1041                                 output->format = IPU_PIX_FMT_NV12;
1042                 }
1043
1044                 if (vout->is_vdoaipu_task)
1045                         ret = vdoaipu_try_task(vout);
1046                 else
1047                         ret = ipu_try_task(vout);
1048         }
1049
1050         return ret;
1051 }
1052
1053 static int mxc_vout_try_format(struct mxc_vout_output *vout, struct v4l2_format *f)
1054 {
1055         int ret = 0;
1056         struct v4l2_rect rect;
1057         u32 o_height = 0;
1058         u32 ocrop_h = 0;
1059         u32 is_1080p;
1060
1061         if (f->fmt.pix.priv && copy_from_user(&rect,
1062                 (void __user *)f->fmt.pix.priv, sizeof(rect)))
1063                 return -EFAULT;
1064
1065         vout->task.input.width = f->fmt.pix.width;
1066         vout->task.input.height = f->fmt.pix.height;
1067         vout->task.input.format = f->fmt.pix.pixelformat;
1068
1069         if (IPU_PIX_FMT_TILED_NV12F == vout->task.input.format) {
1070                 if ((vout->task.input.width > MAX_INTERLACED_WIDTH) ||
1071                         (vout->task.input.deinterlace.motion == HIGH_MOTION))
1072                         return -EINVAL;
1073                 v4l2_info(vout->vfd->v4l2_dev,
1074                                 "tiled fmt enable deinterlace.\n");
1075                 vout->task.input.deinterlace.enable = true;
1076                 vout->task.input.deinterlace.field_fmt =
1077                                 IPU_DEINTERLACE_FIELD_TOP;
1078         }
1079         switch (f->fmt.pix.field) {
1080         /* Images are in progressive format, not interlaced */
1081         case V4L2_FIELD_NONE:
1082                 break;
1083         /* The two fields of a frame are passed in separate buffers,
1084            in temporal order, i. e. the older one first. */
1085         case V4L2_FIELD_ALTERNATE:
1086                 v4l2_err(vout->vfd->v4l2_dev,
1087                         "V4L2_FIELD_ALTERNATE field format not supported yet!\n");
1088                 break;
1089         case V4L2_FIELD_INTERLACED_TB:
1090                 v4l2_info(vout->vfd->v4l2_dev, "Enable deinterlace TB.\n");
1091                 vout->task.input.deinterlace.enable = true;
1092                 vout->task.input.deinterlace.field_fmt =
1093                                 IPU_DEINTERLACE_FIELD_TOP;
1094                 break;
1095         case V4L2_FIELD_INTERLACED_BT:
1096                 v4l2_info(vout->vfd->v4l2_dev, "Enable deinterlace BT.\n");
1097                 vout->task.input.deinterlace.enable = true;
1098                 vout->task.input.deinterlace.field_fmt =
1099                                 IPU_DEINTERLACE_FIELD_BOTTOM;
1100                 break;
1101         default:
1102                 break;
1103         }
1104
1105         if (f->fmt.pix.priv) {
1106                 vout->task.input.crop.pos.x = rect.left;
1107                 vout->task.input.crop.pos.y = rect.top;
1108                 vout->task.input.crop.w = rect.width;
1109                 vout->task.input.crop.h = rect.height;
1110         } else {
1111                 vout->task.input.crop.pos.x = 0;
1112                 vout->task.input.crop.pos.y = 0;
1113                 vout->task.input.crop.w = f->fmt.pix.width;
1114                 vout->task.input.crop.h = f->fmt.pix.height;
1115         }
1116
1117         is_1080p = CHECK_TILED_1080P_DISPLAY(vout);
1118         if (is_1080p) {
1119                 vout->task.input.crop.h = FRAME_HEIGHT_1080P;
1120                 o_height = vout->task.output.height;
1121                 ocrop_h = vout->task.output.crop.h;
1122                 vout->task.output.height = FRAME_HEIGHT_1080P;
1123                 vout->task.output.crop.h = FRAME_HEIGHT_1080P;
1124         }
1125
1126         ret = mxc_vout_try_task(vout);
1127         if (!ret) {
1128                 if (f->fmt.pix.priv) {
1129                         rect.width = vout->task.input.crop.w;
1130                         rect.height = vout->task.input.crop.h;
1131                         if (copy_to_user((void __user *)f->fmt.pix.priv,
1132                                 &rect, sizeof(rect)))
1133                                 ret = -EFAULT;
1134                 } else {
1135                         f->fmt.pix.width = vout->task.input.crop.w;
1136                         f->fmt.pix.height = vout->task.input.crop.h;
1137                 }
1138         }
1139
1140         if (is_1080p) {
1141                 vout->task.output.height = o_height;
1142                 vout->task.output.crop.h = ocrop_h;
1143         }
1144
1145         return ret;
1146 }
1147
1148 static int mxc_vidioc_s_fmt_vid_out(struct file *file, void *fh,
1149                         struct v4l2_format *f)
1150 {
1151         struct mxc_vout_output *vout = fh;
1152         int ret = 0;
1153
1154         if (vout->vbq.streaming)
1155                 return -EBUSY;
1156
1157         mutex_lock(&vout->task_lock);
1158         ret = mxc_vout_try_format(vout, f);
1159         if (ret >= 0)
1160                 vout->fmt_init = true;
1161         mutex_unlock(&vout->task_lock);
1162
1163         return ret;
1164 }
1165
1166 static int mxc_vidioc_cropcap(struct file *file, void *fh,
1167                 struct v4l2_cropcap *cropcap)
1168 {
1169         struct mxc_vout_output *vout = fh;
1170
1171         if (cropcap->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
1172                 return -EINVAL;
1173
1174         cropcap->bounds = vout->crop_bounds;
1175         cropcap->defrect = vout->crop_bounds;
1176
1177         return 0;
1178 }
1179
1180 static int mxc_vidioc_g_crop(struct file *file, void *fh, struct v4l2_crop *crop)
1181 {
1182         struct mxc_vout_output *vout = fh;
1183
1184         if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
1185                 return -EINVAL;
1186
1187         if (vout->disp_support_windows) {
1188                 crop->c.left = vout->win_pos.x;
1189                 crop->c.top = vout->win_pos.y;
1190                 crop->c.width = vout->task.output.width;
1191                 crop->c.height = vout->task.output.height;
1192         } else {
1193                 if (vout->task.output.crop.w && vout->task.output.crop.h) {
1194                         crop->c.left = vout->task.output.crop.pos.x;
1195                         crop->c.top = vout->task.output.crop.pos.y;
1196                         crop->c.width = vout->task.output.crop.w;
1197                         crop->c.height = vout->task.output.crop.h;
1198                 } else {
1199                         crop->c.left = 0;
1200                         crop->c.top = 0;
1201                         crop->c.width = vout->task.output.width;
1202                         crop->c.height = vout->task.output.height;
1203                 }
1204         }
1205
1206         return 0;
1207 }
1208
1209 static int mxc_vidioc_s_crop(struct file *file, void *fh, struct v4l2_crop *crop)
1210 {
1211         struct mxc_vout_output *vout = fh;
1212         struct v4l2_rect *b = &vout->crop_bounds;
1213         int ret = 0;
1214
1215         if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
1216                 return -EINVAL;
1217
1218         if (crop->c.width < 0 || crop->c.height < 0)
1219                 return -EINVAL;
1220
1221         if (crop->c.width == 0)
1222                 crop->c.width = b->width - b->left;
1223         if (crop->c.height == 0)
1224                 crop->c.height = b->height - b->top;
1225
1226         if (crop->c.top < b->top)
1227                 crop->c.top = b->top;
1228         if (crop->c.top >= b->top + b->height)
1229                 crop->c.top = b->top + b->height - 1;
1230         if (crop->c.height > b->top - crop->c.top + b->height)
1231                 crop->c.height =
1232                         b->top - crop->c.top + b->height;
1233
1234         if (crop->c.left < b->left)
1235                 crop->c.left = b->left;
1236         if (crop->c.left >= b->left + b->width)
1237                 crop->c.left = b->left + b->width - 1;
1238         if (crop->c.width > b->left - crop->c.left + b->width)
1239                 crop->c.width =
1240                         b->left - crop->c.left + b->width;
1241
1242         /* stride line limitation */
1243         crop->c.height -= crop->c.height % 8;
1244         crop->c.width -= crop->c.width % 8;
1245
1246         /* the same setting, return */
1247         if (vout->disp_support_windows) {
1248                 if ((vout->win_pos.x == crop->c.left) &&
1249                         (vout->win_pos.y == crop->c.top) &&
1250                         (vout->task.output.crop.w == crop->c.width) &&
1251                         (vout->task.output.crop.h == crop->c.height))
1252                         return 0;
1253         } else {
1254                 if ((vout->task.output.crop.pos.x == crop->c.left) &&
1255                         (vout->task.output.crop.pos.y == crop->c.top) &&
1256                         (vout->task.output.crop.w == crop->c.width) &&
1257                         (vout->task.output.crop.h == crop->c.height))
1258                         return 0;
1259         }
1260
1261         /* wait current work finish */
1262         if (vout->vbq.streaming)
1263                 cancel_work_sync(&vout->disp_work);
1264
1265         mutex_lock(&vout->task_lock);
1266
1267         if (vout->disp_support_windows) {
1268                 vout->task.output.crop.pos.x = 0;
1269                 vout->task.output.crop.pos.y = 0;
1270                 vout->win_pos.x = crop->c.left;
1271                 vout->win_pos.y = crop->c.top;
1272                 vout->task.output.width = crop->c.width;
1273                 vout->task.output.height = crop->c.height;
1274         } else {
1275                 vout->task.output.crop.pos.x = crop->c.left;
1276                 vout->task.output.crop.pos.y = crop->c.top;
1277         }
1278
1279         vout->task.output.crop.w = crop->c.width;
1280         vout->task.output.crop.h = crop->c.height;
1281
1282         /*
1283          * must S_CROP before S_FMT, for fist time S_CROP, will not check
1284          * ipu task, it will check in S_FMT, after S_FMT, S_CROP should
1285          * check ipu task too.
1286          */
1287         if (vout->fmt_init) {
1288                 if (vout->vbq.streaming)
1289                         release_disp_output(vout);
1290
1291                 ret = mxc_vout_try_task(vout);
1292                 if (ret < 0) {
1293                         v4l2_err(vout->vfd->v4l2_dev,
1294                                         "vout check task failed\n");
1295                         goto done;
1296                 }
1297                 if (vout->vbq.streaming) {
1298                         ret = config_disp_output(vout);
1299                         if (ret < 0) {
1300                                 v4l2_err(vout->vfd->v4l2_dev,
1301                                                 "Config display output failed\n");
1302                                 goto done;
1303                         }
1304                 }
1305         }
1306
1307 done:
1308         mutex_unlock(&vout->task_lock);
1309
1310         return ret;
1311 }
1312
1313 static int mxc_vidioc_queryctrl(struct file *file, void *fh,
1314                 struct v4l2_queryctrl *ctrl)
1315 {
1316         int ret = 0;
1317
1318         switch (ctrl->id) {
1319         case V4L2_CID_ROTATE:
1320                 ret = v4l2_ctrl_query_fill(ctrl, 0, 270, 90, 0);
1321                 break;
1322         case V4L2_CID_VFLIP:
1323                 ret = v4l2_ctrl_query_fill(ctrl, 0, 1, 1, 0);
1324                 break;
1325         case V4L2_CID_HFLIP:
1326                 ret = v4l2_ctrl_query_fill(ctrl, 0, 1, 1, 0);
1327                 break;
1328         case V4L2_CID_MXC_MOTION:
1329                 ret = v4l2_ctrl_query_fill(ctrl, 0, 2, 1, 0);
1330                 break;
1331         default:
1332                 ctrl->name[0] = '\0';
1333                 ret = -EINVAL;
1334         }
1335         return ret;
1336 }
1337
1338 static int mxc_vidioc_g_ctrl(struct file *file, void *fh, struct v4l2_control *ctrl)
1339 {
1340         int ret = 0;
1341         struct mxc_vout_output *vout = fh;
1342
1343         switch (ctrl->id) {
1344         case V4L2_CID_ROTATE:
1345                 ctrl->value = vout->ctrl_rotate;
1346                 break;
1347         case V4L2_CID_VFLIP:
1348                 ctrl->value = vout->ctrl_vflip;
1349                 break;
1350         case V4L2_CID_HFLIP:
1351                 ctrl->value = vout->ctrl_hflip;
1352                 break;
1353         case V4L2_CID_MXC_MOTION:
1354                 if (vout->task.input.deinterlace.enable)
1355                         ctrl->value = vout->task.input.deinterlace.motion;
1356                 else
1357                         ctrl->value = 0;
1358                 break;
1359         default:
1360                 ret = -EINVAL;
1361         }
1362         return ret;
1363 }
1364
1365 static void setup_task_rotation(struct mxc_vout_output *vout)
1366 {
1367         if (vout->ctrl_rotate == 0) {
1368                 if (vout->ctrl_vflip && vout->ctrl_hflip)
1369                         vout->task.output.rotate = IPU_ROTATE_180;
1370                 else if (vout->ctrl_vflip)
1371                         vout->task.output.rotate = IPU_ROTATE_VERT_FLIP;
1372                 else if (vout->ctrl_hflip)
1373                         vout->task.output.rotate = IPU_ROTATE_HORIZ_FLIP;
1374                 else
1375                         vout->task.output.rotate = IPU_ROTATE_NONE;
1376         } else if (vout->ctrl_rotate == 90) {
1377                 if (vout->ctrl_vflip && vout->ctrl_hflip)
1378                         vout->task.output.rotate = IPU_ROTATE_90_LEFT;
1379                 else if (vout->ctrl_vflip)
1380                         vout->task.output.rotate = IPU_ROTATE_90_RIGHT_VFLIP;
1381                 else if (vout->ctrl_hflip)
1382                         vout->task.output.rotate = IPU_ROTATE_90_RIGHT_HFLIP;
1383                 else
1384                         vout->task.output.rotate = IPU_ROTATE_90_RIGHT;
1385         } else if (vout->ctrl_rotate == 180) {
1386                 if (vout->ctrl_vflip && vout->ctrl_hflip)
1387                         vout->task.output.rotate = IPU_ROTATE_NONE;
1388                 else if (vout->ctrl_vflip)
1389                         vout->task.output.rotate = IPU_ROTATE_HORIZ_FLIP;
1390                 else if (vout->ctrl_hflip)
1391                         vout->task.output.rotate = IPU_ROTATE_VERT_FLIP;
1392                 else
1393                         vout->task.output.rotate = IPU_ROTATE_180;
1394         } else if (vout->ctrl_rotate == 270) {
1395                 if (vout->ctrl_vflip && vout->ctrl_hflip)
1396                         vout->task.output.rotate = IPU_ROTATE_90_RIGHT;
1397                 else if (vout->ctrl_vflip)
1398                         vout->task.output.rotate = IPU_ROTATE_90_RIGHT_HFLIP;
1399                 else if (vout->ctrl_hflip)
1400                         vout->task.output.rotate = IPU_ROTATE_90_RIGHT_VFLIP;
1401                 else
1402                         vout->task.output.rotate = IPU_ROTATE_90_LEFT;
1403         }
1404 }
1405
1406 static int mxc_vidioc_s_ctrl(struct file *file, void *fh, struct v4l2_control *ctrl)
1407 {
1408         int ret = 0;
1409         struct mxc_vout_output *vout = fh;
1410
1411         /* wait current work finish */
1412         if (vout->vbq.streaming)
1413                 cancel_work_sync(&vout->disp_work);
1414
1415         mutex_lock(&vout->task_lock);
1416         switch (ctrl->id) {
1417         case V4L2_CID_ROTATE:
1418         {
1419                 vout->ctrl_rotate = (ctrl->value/90) * 90;
1420                 if (vout->ctrl_rotate > 270)
1421                         vout->ctrl_rotate = 270;
1422                 setup_task_rotation(vout);
1423                 break;
1424         }
1425         case V4L2_CID_VFLIP:
1426         {
1427                 vout->ctrl_vflip = ctrl->value;
1428                 setup_task_rotation(vout);
1429                 break;
1430         }
1431         case V4L2_CID_HFLIP:
1432         {
1433                 vout->ctrl_hflip = ctrl->value;
1434                 setup_task_rotation(vout);
1435                 break;
1436         }
1437         case V4L2_CID_MXC_MOTION:
1438         {
1439                 vout->task.input.deinterlace.motion = ctrl->value;
1440                 break;
1441         }
1442         default:
1443                 ret = -EINVAL;
1444                 goto done;
1445         }
1446
1447         if (vout->fmt_init) {
1448                 if (vout->vbq.streaming)
1449                         release_disp_output(vout);
1450
1451                 ret = mxc_vout_try_task(vout);
1452                 if (ret < 0) {
1453                         v4l2_err(vout->vfd->v4l2_dev,
1454                                         "vout check task failed\n");
1455                         goto done;
1456                 }
1457                 if (vout->vbq.streaming) {
1458                         ret = config_disp_output(vout);
1459                         if (ret < 0) {
1460                                 v4l2_err(vout->vfd->v4l2_dev,
1461                                                 "Config display output failed\n");
1462                                 goto done;
1463                         }
1464                 }
1465         }
1466
1467 done:
1468         mutex_unlock(&vout->task_lock);
1469
1470         return ret;
1471 }
1472
1473 static int mxc_vidioc_reqbufs(struct file *file, void *fh,
1474                         struct v4l2_requestbuffers *req)
1475 {
1476         int ret = 0;
1477         struct mxc_vout_output *vout = fh;
1478         struct videobuf_queue *q = &vout->vbq;
1479
1480         if (req->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
1481                 return -EINVAL;
1482
1483         /* should not be here after streaming, videobuf_reqbufs will control */
1484         mutex_lock(&vout->task_lock);
1485
1486         ret = videobuf_reqbufs(q, req);
1487
1488         mutex_unlock(&vout->task_lock);
1489         return ret;
1490 }
1491
1492 static int mxc_vidioc_querybuf(struct file *file, void *fh,
1493                         struct v4l2_buffer *b)
1494 {
1495         int ret;
1496         struct mxc_vout_output *vout = fh;
1497
1498         ret = videobuf_querybuf(&vout->vbq, b);
1499         if (!ret) {
1500                 /* return physical address */
1501                 struct videobuf_buffer *vb = vout->vbq.bufs[b->index];
1502                 if (b->flags & V4L2_BUF_FLAG_MAPPED)
1503                         b->m.offset = videobuf_to_dma_contig(vb);
1504         }
1505
1506         return ret;
1507 }
1508
1509 static int mxc_vidioc_qbuf(struct file *file, void *fh,
1510                         struct v4l2_buffer *buffer)
1511 {
1512         struct mxc_vout_output *vout = fh;
1513
1514         return videobuf_qbuf(&vout->vbq, buffer);
1515 }
1516
1517 static int mxc_vidioc_dqbuf(struct file *file, void *fh, struct v4l2_buffer *b)
1518 {
1519         struct mxc_vout_output *vout = fh;
1520
1521         if (!vout->vbq.streaming)
1522                 return -EINVAL;
1523
1524         if (file->f_flags & O_NONBLOCK)
1525                 return videobuf_dqbuf(&vout->vbq, (struct v4l2_buffer *)b, 1);
1526         else
1527                 return videobuf_dqbuf(&vout->vbq, (struct v4l2_buffer *)b, 0);
1528 }
1529
1530 static int set_window_position(struct mxc_vout_output *vout, struct mxcfb_pos *pos)
1531 {
1532         struct fb_info *fbi = vout->fbi;
1533         mm_segment_t old_fs;
1534         int ret = 0;
1535
1536         if (vout->disp_support_windows) {
1537                 old_fs = get_fs();
1538                 set_fs(KERNEL_DS);
1539                 ret = fbi->fbops->fb_ioctl(fbi, MXCFB_SET_OVERLAY_POS,
1540                                 (unsigned long)pos);
1541                 set_fs(old_fs);
1542         }
1543
1544         return ret;
1545 }
1546
1547 static int config_disp_output(struct mxc_vout_output *vout)
1548 {
1549         struct fb_info *fbi = vout->fbi;
1550         struct fb_var_screeninfo var;
1551         int i, display_buf_size, fb_num, ret;
1552         u32 is_1080p;
1553
1554         memcpy(&var, &fbi->var, sizeof(var));
1555
1556         var.xres = vout->task.output.width;
1557         var.yres = vout->task.output.height;
1558         if (vout->bypass_pp) {
1559                 fb_num = 1;
1560                 /* input crop */
1561                 if (vout->task.input.width > vout->task.output.width)
1562                         var.xres_virtual = vout->task.input.width;
1563                 else
1564                         var.xres_virtual = var.xres;
1565                 if (vout->task.input.height > vout->task.output.height)
1566                         var.yres_virtual = vout->task.input.height;
1567                 else
1568                         var.yres_virtual = var.yres;
1569                 var.rotate = vout->task.output.rotate;
1570                 var.vmode |= FB_VMODE_YWRAP;
1571         } else {
1572                 fb_num = FB_BUFS;
1573                 var.xres_virtual = var.xres;
1574                 is_1080p = CHECK_TILED_1080P_DISPLAY(vout);
1575                 if (is_1080p)
1576                         var.yres_virtual = fb_num * FRAME_HEIGHT_1080P;
1577                 else
1578                         var.yres_virtual = fb_num * var.yres;
1579                 var.vmode &= ~FB_VMODE_YWRAP;
1580         }
1581         var.bits_per_pixel = fmt_to_bpp(vout->task.output.format);
1582         var.nonstd = vout->task.output.format;
1583
1584         v4l2_dbg(1, debug, vout->vfd->v4l2_dev,
1585                         "set display fb to %d %d\n",
1586                         var.xres, var.yres);
1587
1588         ret = set_window_position(vout, &vout->win_pos);
1589         if (ret < 0)
1590                 return ret;
1591
1592         /* Init display channel through fb API */
1593         var.yoffset = 0;
1594         var.activate |= FB_ACTIVATE_FORCE;
1595         console_lock();
1596         fbi->flags |= FBINFO_MISC_USEREVENT;
1597         ret = fb_set_var(fbi, &var);
1598         fbi->flags &= ~FBINFO_MISC_USEREVENT;
1599         console_unlock();
1600         if (ret < 0)
1601                 return ret;
1602
1603         is_1080p = CHECK_TILED_1080P_DISPLAY(vout);
1604         if (is_1080p)
1605                 display_buf_size = fbi->fix.line_length * FRAME_HEIGHT_1080P;
1606         else
1607                 display_buf_size = fbi->fix.line_length * fbi->var.yres;
1608         for (i = 0; i < fb_num; i++)
1609                 vout->disp_bufs[i] = fbi->fix.smem_start + i * display_buf_size;
1610
1611         console_lock();
1612         fbi->flags |= FBINFO_MISC_USEREVENT;
1613         ret = fb_blank(fbi, FB_BLANK_UNBLANK);
1614         fbi->flags &= ~FBINFO_MISC_USEREVENT;
1615         console_unlock();
1616
1617         return ret;
1618 }
1619
1620 static void release_disp_output(struct mxc_vout_output *vout)
1621 {
1622         struct fb_info *fbi = vout->fbi;
1623         struct mxcfb_pos pos;
1624
1625         console_lock();
1626         fbi->flags |= FBINFO_MISC_USEREVENT;
1627         fb_blank(fbi, FB_BLANK_POWERDOWN);
1628         fbi->flags &= ~FBINFO_MISC_USEREVENT;
1629         console_unlock();
1630
1631         /* restore pos to 0,0 avoid fb pan display hang? */
1632         pos.x = 0;
1633         pos.y = 0;
1634         set_window_position(vout, &pos);
1635
1636         /* fix if ic bypass crack smem_start */
1637         if (vout->bypass_pp) {
1638                 console_lock();
1639                 fbi->fix.smem_start = vout->disp_bufs[0];
1640                 console_unlock();
1641         }
1642
1643         if (get_ipu_channel(fbi) == MEM_BG_SYNC) {
1644                 console_lock();
1645                 fbi->flags |= FBINFO_MISC_USEREVENT;
1646                 fb_blank(fbi, FB_BLANK_UNBLANK);
1647                 fbi->flags &= ~FBINFO_MISC_USEREVENT;
1648                 console_unlock();
1649         }
1650 }
1651
1652 static int mxc_vidioc_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
1653 {
1654         struct mxc_vout_output *vout = fh;
1655         struct videobuf_queue *q = &vout->vbq;
1656         int ret;
1657
1658         if (q->streaming) {
1659                 v4l2_err(vout->vfd->v4l2_dev,
1660                                 "video output already run\n");
1661                 ret = -EBUSY;
1662                 goto done;
1663         }
1664
1665         if (deinterlace_3_field(vout) && list_is_singular(&q->stream)) {
1666                 v4l2_err(vout->vfd->v4l2_dev,
1667                                 "deinterlacing: need queue 2 frame before streamon\n");
1668                 ret = -EINVAL;
1669                 goto done;
1670         }
1671
1672         ret = config_disp_output(vout);
1673         if (ret < 0) {
1674                 v4l2_err(vout->vfd->v4l2_dev,
1675                                 "Config display output failed\n");
1676                 goto done;
1677         }
1678
1679         init_timer(&vout->timer);
1680         vout->timer.function = mxc_vout_timer_handler;
1681         vout->timer.data = (unsigned long)vout;
1682         vout->timer_stop = true;
1683
1684         vout->start_jiffies = jiffies;
1685
1686         vout->pre_vb = NULL;
1687
1688         ret = videobuf_streamon(q);
1689 done:
1690         return ret;
1691 }
1692
1693 static int mxc_vidioc_streamoff(struct file *file, void *fh, enum v4l2_buf_type i)
1694 {
1695         struct mxc_vout_output *vout = fh;
1696         struct videobuf_queue *q = &vout->vbq;
1697         int ret = 0;
1698
1699         if (q->streaming) {
1700                 cancel_work_sync(&vout->disp_work);
1701                 flush_workqueue(vout->v4l_wq);
1702
1703                 del_timer_sync(&vout->timer);
1704
1705                 release_disp_output(vout);
1706
1707                 ret = videobuf_streamoff(&vout->vbq);
1708         }
1709         INIT_LIST_HEAD(&vout->queue_list);
1710         INIT_LIST_HEAD(&vout->active_list);
1711
1712         return ret;
1713 }
1714
1715 static const struct v4l2_ioctl_ops mxc_vout_ioctl_ops = {
1716         .vidioc_querycap                        = mxc_vidioc_querycap,
1717         .vidioc_enum_fmt_vid_out                = mxc_vidioc_enum_fmt_vid_out,
1718         .vidioc_g_fmt_vid_out                   = mxc_vidioc_g_fmt_vid_out,
1719         .vidioc_s_fmt_vid_out                   = mxc_vidioc_s_fmt_vid_out,
1720         .vidioc_cropcap                         = mxc_vidioc_cropcap,
1721         .vidioc_g_crop                          = mxc_vidioc_g_crop,
1722         .vidioc_s_crop                          = mxc_vidioc_s_crop,
1723         .vidioc_queryctrl                       = mxc_vidioc_queryctrl,
1724         .vidioc_g_ctrl                          = mxc_vidioc_g_ctrl,
1725         .vidioc_s_ctrl                          = mxc_vidioc_s_ctrl,
1726         .vidioc_reqbufs                         = mxc_vidioc_reqbufs,
1727         .vidioc_querybuf                        = mxc_vidioc_querybuf,
1728         .vidioc_qbuf                            = mxc_vidioc_qbuf,
1729         .vidioc_dqbuf                           = mxc_vidioc_dqbuf,
1730         .vidioc_streamon                        = mxc_vidioc_streamon,
1731         .vidioc_streamoff                       = mxc_vidioc_streamoff,
1732 };
1733
1734 static const struct v4l2_file_operations mxc_vout_fops = {
1735         .owner          = THIS_MODULE,
1736         .unlocked_ioctl = video_ioctl2,
1737         .mmap           = mxc_vout_mmap,
1738         .open           = mxc_vout_open,
1739         .release        = mxc_vout_release,
1740 };
1741
1742 static struct video_device mxc_vout_template = {
1743         .name           = "MXC Video Output",
1744         .fops           = &mxc_vout_fops,
1745         .ioctl_ops      = &mxc_vout_ioctl_ops,
1746         .release        = video_device_release,
1747 };
1748
1749 static struct videobuf_queue_ops mxc_vout_vbq_ops = {
1750         .buf_setup = mxc_vout_buffer_setup,
1751         .buf_prepare = mxc_vout_buffer_prepare,
1752         .buf_release = mxc_vout_buffer_release,
1753         .buf_queue = mxc_vout_buffer_queue,
1754 };
1755
1756 static void mxc_vout_free_output(struct mxc_vout_dev *dev)
1757 {
1758         int i;
1759         struct mxc_vout_output *vout;
1760         struct video_device *vfd;
1761
1762         for (i = 0; i < dev->out_num; i++) {
1763                 vout = dev->out[i];
1764                 vfd = vout->vfd;
1765                 if (vout->vdoa_dma.vaddr) {
1766                         dma_free_coherent(vout->vbq.dev, vout->vdoa_dma.size,
1767                                 vout->vdoa_dma.vaddr, vout->vdoa_dma.paddr);
1768                         v4l2_dbg(1, debug, vout->vfd->v4l2_dev,
1769                                         "free vdoa_dma.size:0x%x, paddr:0x%x\n",
1770                                         vout->vdoa_dma.size,
1771                                         vout->vdoa_dma.paddr);
1772                         memset(&vout->vdoa_dma, 0, sizeof(vout->vdoa_dma));
1773                 }
1774                 if (vfd) {
1775                         if (!video_is_registered(vfd))
1776                                 video_device_release(vfd);
1777                         else
1778                                 video_unregister_device(vfd);
1779                 }
1780                 kfree(vout);
1781         }
1782 }
1783
1784 static int __init mxc_vout_setup_output(struct mxc_vout_dev *dev)
1785 {
1786         struct videobuf_queue *q;
1787         struct fb_info *fbi;
1788         struct mxc_vout_output *vout;
1789         int i, ret = 0;
1790
1791         update_display_setting();
1792
1793         /* all output/overlay based on fb */
1794         for (i = 0; i < num_registered_fb; i++) {
1795                 fbi = registered_fb[i];
1796
1797                 vout = kzalloc(sizeof(struct mxc_vout_output), GFP_KERNEL);
1798                 if (!vout) {
1799                         ret = -ENOMEM;
1800                         break;
1801                 }
1802
1803                 dev->out[dev->out_num] = vout;
1804                 dev->out_num++;
1805
1806                 vout->fbi = fbi;
1807                 vout->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
1808                 vout->vfd = video_device_alloc();
1809                 if (!vout->vfd) {
1810                         ret = -ENOMEM;
1811                         break;
1812                 }
1813
1814                 *vout->vfd = mxc_vout_template;
1815                 vout->vfd->debug = debug;
1816                 vout->vfd->v4l2_dev = &dev->v4l2_dev;
1817                 vout->vfd->lock = &vout->mutex;
1818
1819                 mutex_init(&vout->mutex);
1820                 mutex_init(&vout->task_lock);
1821
1822                 strlcpy(vout->vfd->name, fbi->fix.id, sizeof(vout->vfd->name));
1823
1824                 video_set_drvdata(vout->vfd, vout);
1825
1826                 if (video_register_device(vout->vfd,
1827                         VFL_TYPE_GRABBER, video_nr + i) < 0) {
1828                         ret = -ENODEV;
1829                         break;
1830                 }
1831
1832                 q = &vout->vbq;
1833                 q->dev = dev->dev;
1834                 spin_lock_init(&vout->vbq_lock);
1835                 videobuf_queue_dma_contig_init(q, &mxc_vout_vbq_ops, q->dev,
1836                                 &vout->vbq_lock, vout->type, V4L2_FIELD_NONE,
1837                                 sizeof(struct videobuf_buffer), vout, NULL);
1838
1839                 v4l2_info(vout->vfd->v4l2_dev, "V4L2 device registered as %s\n",
1840                                 video_device_node_name(vout->vfd));
1841
1842         }
1843
1844         return ret;
1845 }
1846
1847 static int mxc_vout_probe(struct platform_device *pdev)
1848 {
1849         int ret;
1850         struct mxc_vout_dev *dev;
1851
1852         dev = kzalloc(sizeof(*dev), GFP_KERNEL);
1853         if (!dev)
1854                 return -ENOMEM;
1855
1856         dev->dev = &pdev->dev;
1857         dev->dev->dma_mask = kmalloc(sizeof(*dev->dev->dma_mask), GFP_KERNEL);
1858         *dev->dev->dma_mask = DMA_BIT_MASK(32);
1859         dev->dev->coherent_dma_mask = DMA_BIT_MASK(32);
1860
1861         ret = v4l2_device_register(dev->dev, &dev->v4l2_dev);
1862         if (ret) {
1863                 dev_err(dev->dev, "v4l2_device_register failed\n");
1864                 goto free_dev;
1865         }
1866
1867         ret = mxc_vout_setup_output(dev);
1868         if (ret < 0)
1869                 goto rel_vdev;
1870
1871         return 0;
1872
1873 rel_vdev:
1874         mxc_vout_free_output(dev);
1875         v4l2_device_unregister(&dev->v4l2_dev);
1876 free_dev:
1877         kfree(dev);
1878         return ret;
1879 }
1880
1881 static int mxc_vout_remove(struct platform_device *pdev)
1882 {
1883         struct v4l2_device *v4l2_dev = platform_get_drvdata(pdev);
1884         struct mxc_vout_dev *dev = container_of(v4l2_dev, struct
1885                         mxc_vout_dev, v4l2_dev);
1886
1887         mxc_vout_free_output(dev);
1888         v4l2_device_unregister(v4l2_dev);
1889         kfree(dev);
1890         return 0;
1891 }
1892
1893 static struct platform_driver mxc_vout_driver = {
1894         .driver = {
1895                 .name = "mxc_v4l2_output",
1896         },
1897         .probe = mxc_vout_probe,
1898         .remove = mxc_vout_remove,
1899 };
1900
1901 static int __init mxc_vout_init(void)
1902 {
1903         if (platform_driver_register(&mxc_vout_driver) != 0) {
1904                 printk(KERN_ERR VOUT_NAME ":Could not register Video driver\n");
1905                 return -EINVAL;
1906         }
1907         return 0;
1908 }
1909
1910 static void mxc_vout_cleanup(void)
1911 {
1912         platform_driver_unregister(&mxc_vout_driver);
1913 }
1914
1915 module_init(mxc_vout_init);
1916 module_exit(mxc_vout_cleanup);
1917
1918 MODULE_AUTHOR("Freescale Semiconductor, Inc.");
1919 MODULE_DESCRIPTION("V4L2-driver for MXC video output");
1920 MODULE_LICENSE("GPL");