drivers/media/pci/tw686x/tw686x-video.c

   1 /*
   2  * Copyright (C) 2015 VanguardiaSur - www.vanguardiasur.com.ar
   3  *
   4  * Based on original driver by Krzysztof Ha?asa:
   5  * Copyright (C) 2015 Industrial Research Institute for Automation
   6  * and Measurements PIAP
   7  *
   8  * This program is free software; you can redistribute it and/or modify it
   9  * under the terms of version 2 of the GNU General Public License
  10  * as published by the Free Software Foundation.
  11  *
  12  */
  13
  14 #include <linux/init.h>
  15 #include <linux/delay.h>
  16 #include <linux/list.h>
  17 #include <linux/module.h>
  18 #include <linux/kernel.h>
  19 #include <linux/slab.h>
  20 #include <media/v4l2-common.h>
  21 #include <media/v4l2-event.h>
  22 #include <media/videobuf2-dma-contig.h>
  23 #include <media/videobuf2-dma-sg.h>
  24 #include <media/videobuf2-vmalloc.h>
  25 #include "tw686x.h"
  26 #include "tw686x-regs.h"
  27
  28 #define TW686X_INPUTS_PER_CH            4
  29 #define TW686X_VIDEO_WIDTH              720
  30 #define TW686X_VIDEO_HEIGHT(id)         ((id & V4L2_STD_525_60) ? 480 : 576)
  31 #define TW686X_MAX_FPS(id)              ((id & V4L2_STD_525_60) ? 30 : 25)
  32
  33 #define TW686X_MAX_SG_ENTRY_SIZE        4096
  34 #define TW686X_MAX_SG_DESC_COUNT        256 /* PAL 720x576 needs 203 4-KB pages */
  35 #define TW686X_SG_TABLE_SIZE            (TW686X_MAX_SG_DESC_COUNT * sizeof(struct tw686x_sg_desc))
  36
  37 static const struct tw686x_format formats[] = {
  38         {
  39                 .fourcc = V4L2_PIX_FMT_UYVY,
  40                 .mode = 0,
  41                 .depth = 16,
  42         }, {
  43                 .fourcc = V4L2_PIX_FMT_RGB565,
  44                 .mode = 5,
  45                 .depth = 16,
  46         }, {
  47                 .fourcc = V4L2_PIX_FMT_YUYV,
  48                 .mode = 6,
  49                 .depth = 16,
  50         }
  51 };
  52
  53 static void tw686x_buf_done(struct tw686x_video_channel *vc,
  54                             unsigned int pb)
  55 {
  56         struct tw686x_dma_desc *desc = &vc->dma_descs[pb];
  57         struct tw686x_dev *dev = vc->dev;
  58         struct vb2_v4l2_buffer *vb;
  59         struct vb2_buffer *vb2_buf;
  60
  61         if (vc->curr_bufs[pb]) {
  62                 vb = &vc->curr_bufs[pb]->vb;
  63
  64                 vb->field = dev->dma_ops->field;
  65                 vb->sequence = vc->sequence++;
  66                 vb2_buf = &vb->vb2_buf;
  67
  68                 if (dev->dma_mode == TW686X_DMA_MODE_MEMCPY)
  69                         memcpy(vb2_plane_vaddr(vb2_buf, 0), desc->virt,
  70                                desc->size);
  71                 vb2_buf->timestamp = ktime_get_ns();
  72                 vb2_buffer_done(vb2_buf, VB2_BUF_STATE_DONE);
  73         }
  74
  75         vc->pb = !pb;
  76 }
  77
  78 /*
  79  * We can call this even when alloc_dma failed for the given channel
  80  */
  81 static void tw686x_memcpy_dma_free(struct tw686x_video_channel *vc,
  82                                    unsigned int pb)
  83 {
  84         struct tw686x_dma_desc *desc = &vc->dma_descs[pb];
  85         struct tw686x_dev *dev = vc->dev;
  86         struct pci_dev *pci_dev;
  87         unsigned long flags;
  88
  89         /* Check device presence. Shouldn't really happen! */
  90         spin_lock_irqsave(&dev->lock, flags);
  91         pci_dev = dev->pci_dev;
  92         spin_unlock_irqrestore(&dev->lock, flags);
  93         if (!pci_dev) {
  94                 WARN(1, "trying to deallocate on missing device\n");
  95                 return;
  96         }
  97
  98         if (desc->virt) {
  99                 pci_free_consistent(dev->pci_dev, desc->size,
 100                                     desc->virt, desc->phys);
 101                 desc->virt = NULL;
 102         }
 103 }
 104
 105 static int tw686x_memcpy_dma_alloc(struct tw686x_video_channel *vc,
 106                                    unsigned int pb)
 107 {
 108         struct tw686x_dev *dev = vc->dev;
 109         u32 reg = pb ? VDMA_B_ADDR[vc->ch] : VDMA_P_ADDR[vc->ch];
 110         unsigned int len;
 111         void *virt;
 112
 113         WARN(vc->dma_descs[pb].virt,
 114              "Allocating buffer but previous still here\n");
 115
 116         len = (vc->width * vc->height * vc->format->depth) >> 3;
 117         virt = pci_alloc_consistent(dev->pci_dev, len,
 118                                     &vc->dma_descs[pb].phys);
 119         if (!virt) {
 120                 v4l2_err(&dev->v4l2_dev,
 121                          "dma%d: unable to allocate %s-buffer\n",
 122                          vc->ch, pb ? "B" : "P");
 123                 return -ENOMEM;
 124         }
 125         vc->dma_descs[pb].size = len;
 126         vc->dma_descs[pb].virt = virt;
 127         reg_write(dev, reg, vc->dma_descs[pb].phys);
 128
 129         return 0;
 130 }
 131
 132 static void tw686x_memcpy_buf_refill(struct tw686x_video_channel *vc,
 133                                      unsigned int pb)
 134 {
 135         struct tw686x_v4l2_buf *buf;
 136
 137         while (!list_empty(&vc->vidq_queued)) {
 138
 139                 buf = list_first_entry(&vc->vidq_queued,
 140                         struct tw686x_v4l2_buf, list);
 141                 list_del(&buf->list);
 142
 143                 vc->curr_bufs[pb] = buf;
 144                 return;
 145         }
 146         vc->curr_bufs[pb] = NULL;
 147 }
 148
 149 static const struct tw686x_dma_ops memcpy_dma_ops = {
 150         .alloc          = tw686x_memcpy_dma_alloc,
 151         .free           = tw686x_memcpy_dma_free,
 152         .buf_refill     = tw686x_memcpy_buf_refill,
 153         .mem_ops        = &vb2_vmalloc_memops,
 154         .hw_dma_mode    = TW686X_FRAME_MODE,
 155         .field          = V4L2_FIELD_INTERLACED,
 156 };
 157
 158 static void tw686x_contig_buf_refill(struct tw686x_video_channel *vc,
 159                                      unsigned int pb)
 160 {
 161         struct tw686x_v4l2_buf *buf;
 162
 163         while (!list_empty(&vc->vidq_queued)) {
 164                 u32 reg = pb ? VDMA_B_ADDR[vc->ch] : VDMA_P_ADDR[vc->ch];
 165                 dma_addr_t phys;
 166
 167                 buf = list_first_entry(&vc->vidq_queued,
 168                         struct tw686x_v4l2_buf, list);
 169                 list_del(&buf->list);
 170
 171                 phys = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
 172                 reg_write(vc->dev, reg, phys);
 173
 174                 buf->vb.vb2_buf.state = VB2_BUF_STATE_ACTIVE;
 175                 vc->curr_bufs[pb] = buf;
 176                 return;
 177         }
 178         vc->curr_bufs[pb] = NULL;
 179 }
 180
 181 static const struct tw686x_dma_ops contig_dma_ops = {
 182         .buf_refill     = tw686x_contig_buf_refill,
 183         .mem_ops        = &vb2_dma_contig_memops,
 184         .hw_dma_mode    = TW686X_FRAME_MODE,
 185         .field          = V4L2_FIELD_INTERLACED,
 186 };
 187
 188 static int tw686x_sg_desc_fill(struct tw686x_sg_desc *descs,
 189                                struct tw686x_v4l2_buf *buf,
 190                                unsigned int buf_len)
 191 {
 192         struct sg_table *vbuf = vb2_dma_sg_plane_desc(&buf->vb.vb2_buf, 0);
 193         unsigned int len, entry_len;
 194         struct scatterlist *sg;
 195         int i, count;
 196
 197         /* Clear the scatter-gather table */
 198         memset(descs, 0, TW686X_SG_TABLE_SIZE);
 199
 200         count = 0;
 201         for_each_sg(vbuf->sgl, sg, vbuf->nents, i) {
 202                 dma_addr_t phys = sg_dma_address(sg);
 203                 len = sg_dma_len(sg);
 204
 205                 while (len && buf_len) {
 206
 207                         if (count == TW686X_MAX_SG_DESC_COUNT)
 208                                 return -ENOMEM;
 209
 210                         entry_len = min_t(unsigned int, len,
 211                                           TW686X_MAX_SG_ENTRY_SIZE);
 212                         entry_len = min_t(unsigned int, entry_len, buf_len);
 213                         descs[count].phys = cpu_to_le32(phys);
 214                         descs[count++].flags_length =
 215                                         cpu_to_le32(BIT(30) | entry_len);
 216                         phys += entry_len;
 217                         len -= entry_len;
 218                         buf_len -= entry_len;
 219                 }
 220
 221                 if (!buf_len)
 222                         return 0;
 223         }
 224
 225         return -ENOMEM;
 226 }
 227
 228 static void tw686x_sg_buf_refill(struct tw686x_video_channel *vc,
 229                                  unsigned int pb)
 230 {
 231         struct tw686x_dev *dev = vc->dev;
 232         struct tw686x_v4l2_buf *buf;
 233
 234         while (!list_empty(&vc->vidq_queued)) {
 235                 unsigned int buf_len;
 236
 237                 buf = list_first_entry(&vc->vidq_queued,
 238                         struct tw686x_v4l2_buf, list);
 239                 list_del(&buf->list);
 240
 241                 buf_len = (vc->width * vc->height * vc->format->depth) >> 3;
 242                 if (tw686x_sg_desc_fill(vc->sg_descs[pb], buf, buf_len)) {
 243                         v4l2_err(&dev->v4l2_dev,
 244                                  "dma%d: unable to fill %s-buffer\n",
 245                                  vc->ch, pb ? "B" : "P");
 246                         vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 247                         continue;
 248                 }
 249
 250                 buf->vb.vb2_buf.state = VB2_BUF_STATE_ACTIVE;
 251                 vc->curr_bufs[pb] = buf;
 252                 return;
 253         }
 254
 255         vc->curr_bufs[pb] = NULL;
 256 }
 257
 258 static void tw686x_sg_dma_free(struct tw686x_video_channel *vc,
 259                                unsigned int pb)
 260 {
 261         struct tw686x_dma_desc *desc = &vc->dma_descs[pb];
 262         struct tw686x_dev *dev = vc->dev;
 263
 264         if (desc->size) {
 265                 pci_free_consistent(dev->pci_dev, desc->size,
 266                                     desc->virt, desc->phys);
 267                 desc->virt = NULL;
 268         }
 269
 270         vc->sg_descs[pb] = NULL;
 271 }
 272
 273 static int tw686x_sg_dma_alloc(struct tw686x_video_channel *vc,
 274                                unsigned int pb)
 275 {
 276         struct tw686x_dma_desc *desc = &vc->dma_descs[pb];
 277         struct tw686x_dev *dev = vc->dev;
 278         u32 reg = pb ? DMA_PAGE_TABLE1_ADDR[vc->ch] :
 279                        DMA_PAGE_TABLE0_ADDR[vc->ch];
 280         void *virt;
 281
 282         if (desc->size) {
 283
 284                 virt = pci_alloc_consistent(dev->pci_dev, desc->size,
 285                                             &desc->phys);
 286                 if (!virt) {
 287                         v4l2_err(&dev->v4l2_dev,
 288                                  "dma%d: unable to allocate %s-buffer\n",
 289                                  vc->ch, pb ? "B" : "P");
 290                         return -ENOMEM;
 291                 }
 292                 desc->virt = virt;
 293                 reg_write(dev, reg, desc->phys);
 294         } else {
 295                 virt = dev->video_channels[0].dma_descs[pb].virt +
 296                        vc->ch * TW686X_SG_TABLE_SIZE;
 297         }
 298
 299         vc->sg_descs[pb] = virt;
 300         return 0;
 301 }
 302
 303 static int tw686x_sg_setup(struct tw686x_dev *dev)
 304 {
 305         unsigned int sg_table_size, pb, ch, channels;
 306
 307         if (is_second_gen(dev)) {
 308                 /*
 309                  * TW6865/TW6869: each channel needs a pair of
 310                  * P-B descriptor tables.
 311                  */
 312                 channels = max_channels(dev);
 313                 sg_table_size = TW686X_SG_TABLE_SIZE;
 314         } else {
 315                 /*
 316                  * TW6864/TW6868: we need to allocate a pair of
 317                  * P-B descriptor tables, common for all channels.
 318                  * Each table will be bigger than 4 KB.
 319                  */
 320                 channels = 1;
 321                 sg_table_size = max_channels(dev) * TW686X_SG_TABLE_SIZE;
 322         }
 323
 324         for (ch = 0; ch < channels; ch++) {
 325                 struct tw686x_video_channel *vc = &dev->video_channels[ch];
 326
 327                 for (pb = 0; pb < 2; pb++)
 328                         vc->dma_descs[pb].size = sg_table_size;
 329         }
 330
 331         return 0;
 332 }
 333
 334 static const struct tw686x_dma_ops sg_dma_ops = {
 335         .setup          = tw686x_sg_setup,
 336         .alloc          = tw686x_sg_dma_alloc,
 337         .free           = tw686x_sg_dma_free,
 338         .buf_refill     = tw686x_sg_buf_refill,
 339         .mem_ops        = &vb2_dma_sg_memops,
 340         .hw_dma_mode    = TW686X_SG_MODE,
 341         .field          = V4L2_FIELD_SEQ_TB,
 342 };
 343
 344 static const unsigned int fps_map[15] = {
 345         /*
 346          * bit 31 enables selecting the field control register
 347          * bits 0-29 are a bitmask with fields that will be output.
 348          * For NTSC (and PAL-M, PAL-60), all 30 bits are used.
 349          * For other PAL standards, only the first 25 bits are used.
 350          */
 351         0x00000000, /* output all fields */
 352         0x80000006, /* 2 fps (60Hz), 2 fps (50Hz) */
 353         0x80018006, /* 4 fps (60Hz), 4 fps (50Hz) */
 354         0x80618006, /* 6 fps (60Hz), 6 fps (50Hz) */
 355         0x81818186, /* 8 fps (60Hz), 8 fps (50Hz) */
 356         0x86186186, /* 10 fps (60Hz), 8 fps (50Hz) */
 357         0x86619866, /* 12 fps (60Hz), 10 fps (50Hz) */
 358         0x86666666, /* 14 fps (60Hz), 12 fps (50Hz) */
 359         0x9999999e, /* 16 fps (60Hz), 14 fps (50Hz) */
 360         0x99e6799e, /* 18 fps (60Hz), 16 fps (50Hz) */
 361         0x9e79e79e, /* 20 fps (60Hz), 16 fps (50Hz) */
 362         0x9e7e7e7e, /* 22 fps (60Hz), 18 fps (50Hz) */
 363         0x9fe7f9fe, /* 24 fps (60Hz), 20 fps (50Hz) */
 364         0x9ffe7ffe, /* 26 fps (60Hz), 22 fps (50Hz) */
 365         0x9ffffffe, /* 28 fps (60Hz), 24 fps (50Hz) */
 366 };
 367
 368 static unsigned int tw686x_real_fps(unsigned int index, unsigned int max_fps)
 369 {
 370         unsigned long mask;
 371
 372         if (!index || index >= ARRAY_SIZE(fps_map))
 373                 return max_fps;
 374
 375         mask = GENMASK(max_fps - 1, 0);
 376         return hweight_long(fps_map[index] & mask);
 377 }
 378
 379 static unsigned int tw686x_fps_idx(unsigned int fps, unsigned int max_fps)
 380 {
 381         unsigned int idx, real_fps;
 382         int delta;
 383
 384         /* First guess */
 385         idx = (12 + 15 * fps) / max_fps;
 386
 387         /* Minimal possible framerate is 2 frames per second */
 388         if (!idx)
 389                 return 1;
 390
 391         /* Check if the difference is bigger than abs(1) and adjust */
 392         real_fps = tw686x_real_fps(idx, max_fps);
 393         delta = real_fps - fps;
 394         if (delta < -1)
 395                 idx++;
 396         else if (delta > 1)
 397                 idx--;
 398
 399         /* Max framerate */
 400         if (idx >= 15)
 401                 return 0;
 402
 403         return idx;
 404 }
 405
 406 static void tw686x_set_framerate(struct tw686x_video_channel *vc,
 407                                  unsigned int fps)
 408 {
 409         unsigned int i;
 410
 411         i = tw686x_fps_idx(fps, TW686X_MAX_FPS(vc->video_standard));
 412         reg_write(vc->dev, VIDEO_FIELD_CTRL[vc->ch], fps_map[i]);
 413         vc->fps = tw686x_real_fps(i, TW686X_MAX_FPS(vc->video_standard));
 414 }
 415
 416 static const struct tw686x_format *format_by_fourcc(unsigned int fourcc)
 417 {
 418         unsigned int cnt;
 419
 420         for (cnt = 0; cnt < ARRAY_SIZE(formats); cnt++)
 421                 if (formats[cnt].fourcc == fourcc)
 422                         return &formats[cnt];
 423         return NULL;
 424 }
 425
 426 static int tw686x_queue_setup(struct vb2_queue *vq,
 427                               unsigned int *nbuffers, unsigned int *nplanes,
 428                               unsigned int sizes[], struct device *alloc_devs[])
 429 {
 430         struct tw686x_video_channel *vc = vb2_get_drv_priv(vq);
 431         unsigned int szimage =
 432                 (vc->width * vc->height * vc->format->depth) >> 3;
 433
 434         /*
 435          * Let's request at least three buffers: two for the
 436          * DMA engine and one for userspace.
 437          */
 438         if (vq->num_buffers + *nbuffers < 3)
 439                 *nbuffers = 3 - vq->num_buffers;
 440
 441         if (*nplanes) {
 442                 if (*nplanes != 1 || sizes[0] < szimage)
 443                         return -EINVAL;
 444                 return 0;
 445         }
 446
 447         sizes[0] = szimage;
 448         *nplanes = 1;
 449         return 0;
 450 }
 451
 452 static void tw686x_buf_queue(struct vb2_buffer *vb)
 453 {
 454         struct tw686x_video_channel *vc = vb2_get_drv_priv(vb->vb2_queue);
 455         struct tw686x_dev *dev = vc->dev;
 456         struct pci_dev *pci_dev;
 457         unsigned long flags;
 458         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 459         struct tw686x_v4l2_buf *buf =
 460                 container_of(vbuf, struct tw686x_v4l2_buf, vb);
 461
 462         /* Check device presence */
 463         spin_lock_irqsave(&dev->lock, flags);
 464         pci_dev = dev->pci_dev;
 465         spin_unlock_irqrestore(&dev->lock, flags);
 466         if (!pci_dev) {
 467                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 468                 return;
 469         }
 470
 471         spin_lock_irqsave(&vc->qlock, flags);
 472         list_add_tail(&buf->list, &vc->vidq_queued);
 473         spin_unlock_irqrestore(&vc->qlock, flags);
 474 }
 475
 476 static void tw686x_clear_queue(struct tw686x_video_channel *vc,
 477                                enum vb2_buffer_state state)
 478 {
 479         unsigned int pb;
 480
 481         while (!list_empty(&vc->vidq_queued)) {
 482                 struct tw686x_v4l2_buf *buf;
 483
 484                 buf = list_first_entry(&vc->vidq_queued,
 485                         struct tw686x_v4l2_buf, list);
 486                 list_del(&buf->list);
 487                 vb2_buffer_done(&buf->vb.vb2_buf, state);
 488         }
 489
 490         for (pb = 0; pb < 2; pb++) {
 491                 if (vc->curr_bufs[pb])
 492                         vb2_buffer_done(&vc->curr_bufs[pb]->vb.vb2_buf, state);
 493                 vc->curr_bufs[pb] = NULL;
 494         }
 495 }
 496
 497 static int tw686x_start_streaming(struct vb2_queue *vq, unsigned int count)
 498 {
 499         struct tw686x_video_channel *vc = vb2_get_drv_priv(vq);
 500         struct tw686x_dev *dev = vc->dev;
 501         struct pci_dev *pci_dev;
 502         unsigned long flags;
 503         int pb, err;
 504
 505         /* Check device presence */
 506         spin_lock_irqsave(&dev->lock, flags);
 507         pci_dev = dev->pci_dev;
 508         spin_unlock_irqrestore(&dev->lock, flags);
 509         if (!pci_dev) {
 510                 err = -ENODEV;
 511                 goto err_clear_queue;
 512         }
 513
 514         spin_lock_irqsave(&vc->qlock, flags);
 515
 516         /* Sanity check */
 517         if (dev->dma_mode == TW686X_DMA_MODE_MEMCPY &&
 518             (!vc->dma_descs[0].virt || !vc->dma_descs[1].virt)) {
 519                 spin_unlock_irqrestore(&vc->qlock, flags);
 520                 v4l2_err(&dev->v4l2_dev,
 521                          "video%d: refusing to start without DMA buffers\n",
 522                          vc->num);
 523                 err = -ENOMEM;
 524                 goto err_clear_queue;
 525         }
 526
 527         for (pb = 0; pb < 2; pb++)
 528                 dev->dma_ops->buf_refill(vc, pb);
 529         spin_unlock_irqrestore(&vc->qlock, flags);
 530
 531         vc->sequence = 0;
 532         vc->pb = 0;
 533
 534         spin_lock_irqsave(&dev->lock, flags);
 535         tw686x_enable_channel(dev, vc->ch);
 536         spin_unlock_irqrestore(&dev->lock, flags);
 537
 538         mod_timer(&dev->dma_delay_timer, jiffies + msecs_to_jiffies(100));
 539
 540         return 0;
 541
 542 err_clear_queue:
 543         spin_lock_irqsave(&vc->qlock, flags);
 544         tw686x_clear_queue(vc, VB2_BUF_STATE_QUEUED);
 545         spin_unlock_irqrestore(&vc->qlock, flags);
 546         return err;
 547 }
 548
 549 static void tw686x_stop_streaming(struct vb2_queue *vq)
 550 {
 551         struct tw686x_video_channel *vc = vb2_get_drv_priv(vq);
 552         struct tw686x_dev *dev = vc->dev;
 553         struct pci_dev *pci_dev;
 554         unsigned long flags;
 555
 556         /* Check device presence */
 557         spin_lock_irqsave(&dev->lock, flags);
 558         pci_dev = dev->pci_dev;
 559         spin_unlock_irqrestore(&dev->lock, flags);
 560         if (pci_dev)
 561                 tw686x_disable_channel(dev, vc->ch);
 562
 563         spin_lock_irqsave(&vc->qlock, flags);
 564         tw686x_clear_queue(vc, VB2_BUF_STATE_ERROR);
 565         spin_unlock_irqrestore(&vc->qlock, flags);
 566 }
 567
 568 static int tw686x_buf_prepare(struct vb2_buffer *vb)
 569 {
 570         struct tw686x_video_channel *vc = vb2_get_drv_priv(vb->vb2_queue);
 571         unsigned int size =
 572                 (vc->width * vc->height * vc->format->depth) >> 3;
 573
 574         if (vb2_plane_size(vb, 0) < size)
 575                 return -EINVAL;
 576         vb2_set_plane_payload(vb, 0, size);
 577         return 0;
 578 }
 579
 580 static const struct vb2_ops tw686x_video_qops = {
 581         .queue_setup            = tw686x_queue_setup,
 582         .buf_queue              = tw686x_buf_queue,
 583         .buf_prepare            = tw686x_buf_prepare,
 584         .start_streaming        = tw686x_start_streaming,
 585         .stop_streaming         = tw686x_stop_streaming,
 586         .wait_prepare           = vb2_ops_wait_prepare,
 587         .wait_finish            = vb2_ops_wait_finish,
 588 };
 589
 590 static int tw686x_s_ctrl(struct v4l2_ctrl *ctrl)
 591 {
 592         struct tw686x_video_channel *vc;
 593         struct tw686x_dev *dev;
 594         unsigned int ch;
 595
 596         vc = container_of(ctrl->handler, struct tw686x_video_channel,
 597                           ctrl_handler);
 598         dev = vc->dev;
 599         ch = vc->ch;
 600
 601         switch (ctrl->id) {
 602         case V4L2_CID_BRIGHTNESS:
 603                 reg_write(dev, BRIGHT[ch], ctrl->val & 0xff);
 604                 return 0;
 605
 606         case V4L2_CID_CONTRAST:
 607                 reg_write(dev, CONTRAST[ch], ctrl->val);
 608                 return 0;
 609
 610         case V4L2_CID_SATURATION:
 611                 reg_write(dev, SAT_U[ch], ctrl->val);
 612                 reg_write(dev, SAT_V[ch], ctrl->val);
 613                 return 0;
 614
 615         case V4L2_CID_HUE:
 616                 reg_write(dev, HUE[ch], ctrl->val & 0xff);
 617                 return 0;
 618         }
 619
 620         return -EINVAL;
 621 }
 622
 623 static const struct v4l2_ctrl_ops ctrl_ops = {
 624         .s_ctrl = tw686x_s_ctrl,
 625 };
 626
 627 static int tw686x_g_fmt_vid_cap(struct file *file, void *priv,
 628                                 struct v4l2_format *f)
 629 {
 630         struct tw686x_video_channel *vc = video_drvdata(file);
 631         struct tw686x_dev *dev = vc->dev;
 632
 633         f->fmt.pix.width = vc->width;
 634         f->fmt.pix.height = vc->height;
 635         f->fmt.pix.field = dev->dma_ops->field;
 636         f->fmt.pix.pixelformat = vc->format->fourcc;
 637         f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 638         f->fmt.pix.bytesperline = (f->fmt.pix.width * vc->format->depth) / 8;
 639         f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
 640         return 0;
 641 }
 642
 643 static int tw686x_try_fmt_vid_cap(struct file *file, void *priv,
 644                                   struct v4l2_format *f)
 645 {
 646         struct tw686x_video_channel *vc = video_drvdata(file);
 647         struct tw686x_dev *dev = vc->dev;
 648         unsigned int video_height = TW686X_VIDEO_HEIGHT(vc->video_standard);
 649         const struct tw686x_format *format;
 650
 651         format = format_by_fourcc(f->fmt.pix.pixelformat);
 652         if (!format) {
 653                 format = &formats[0];
 654                 f->fmt.pix.pixelformat = format->fourcc;
 655         }
 656
 657         if (f->fmt.pix.width <= TW686X_VIDEO_WIDTH / 2)
 658                 f->fmt.pix.width = TW686X_VIDEO_WIDTH / 2;
 659         else
 660                 f->fmt.pix.width = TW686X_VIDEO_WIDTH;
 661
 662         if (f->fmt.pix.height <= video_height / 2)
 663                 f->fmt.pix.height = video_height / 2;
 664         else
 665                 f->fmt.pix.height = video_height;
 666
 667         f->fmt.pix.bytesperline = (f->fmt.pix.width * format->depth) / 8;
 668         f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
 669         f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 670         f->fmt.pix.field = dev->dma_ops->field;
 671
 672         return 0;
 673 }
 674
 675 static int tw686x_set_format(struct tw686x_video_channel *vc,
 676                              unsigned int pixelformat, unsigned int width,
 677                              unsigned int height, bool realloc)
 678 {
 679         struct tw686x_dev *dev = vc->dev;
 680         u32 val, dma_width, dma_height, dma_line_width;
 681         int err, pb;
 682
 683         vc->format = format_by_fourcc(pixelformat);
 684         vc->width = width;
 685         vc->height = height;
 686
 687         /* We need new DMA buffers if the framesize has changed */
 688         if (dev->dma_ops->alloc && realloc) {
 689                 for (pb = 0; pb < 2; pb++)
 690                         dev->dma_ops->free(vc, pb);
 691
 692                 for (pb = 0; pb < 2; pb++) {
 693                         err = dev->dma_ops->alloc(vc, pb);
 694                         if (err) {
 695                                 if (pb > 0)
 696                                         dev->dma_ops->free(vc, 0);
 697                                 return err;
 698                         }
 699                 }
 700         }
 701
 702         val = reg_read(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch]);
 703
 704         if (vc->width <= TW686X_VIDEO_WIDTH / 2)
 705                 val |= BIT(23);
 706         else
 707                 val &= ~BIT(23);
 708
 709         if (vc->height <= TW686X_VIDEO_HEIGHT(vc->video_standard) / 2)
 710                 val |= BIT(24);
 711         else
 712                 val &= ~BIT(24);
 713
 714         val &= ~0x7ffff;
 715
 716         /* Program the DMA scatter-gather */
 717         if (dev->dma_mode == TW686X_DMA_MODE_SG) {
 718                 u32 start_idx, end_idx;
 719
 720                 start_idx = is_second_gen(dev) ?
 721                                 0 : vc->ch * TW686X_MAX_SG_DESC_COUNT;
 722                 end_idx = start_idx + TW686X_MAX_SG_DESC_COUNT - 1;
 723
 724                 val |= (end_idx << 10) | start_idx;
 725         }
 726
 727         val &= ~(0x7 << 20);
 728         val |= vc->format->mode << 20;
 729         reg_write(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch], val);
 730
 731         /* Program the DMA frame size */
 732         dma_width = (vc->width * 2) & 0x7ff;
 733         dma_height = vc->height / 2;
 734         dma_line_width = (vc->width * 2) & 0x7ff;
 735         val = (dma_height << 22) | (dma_line_width << 11)  | dma_width;
 736         reg_write(vc->dev, VDMA_WHP[vc->ch], val);
 737         return 0;
 738 }
 739
 740 static int tw686x_s_fmt_vid_cap(struct file *file, void *priv,
 741                                 struct v4l2_format *f)
 742 {
 743         struct tw686x_video_channel *vc = video_drvdata(file);
 744         unsigned long area;
 745         bool realloc;
 746         int err;
 747
 748         if (vb2_is_busy(&vc->vidq))
 749                 return -EBUSY;
 750
 751         area = vc->width * vc->height;
 752         err = tw686x_try_fmt_vid_cap(file, priv, f);
 753         if (err)
 754                 return err;
 755
 756         realloc = area != (f->fmt.pix.width * f->fmt.pix.height);
 757         return tw686x_set_format(vc, f->fmt.pix.pixelformat,
 758                                  f->fmt.pix.width, f->fmt.pix.height,
 759                                  realloc);
 760 }
 761
 762 static int tw686x_querycap(struct file *file, void *priv,
 763                            struct v4l2_capability *cap)
 764 {
 765         struct tw686x_video_channel *vc = video_drvdata(file);
 766         struct tw686x_dev *dev = vc->dev;
 767
 768         strlcpy(cap->driver, "tw686x", sizeof(cap->driver));
 769         strlcpy(cap->card, dev->name, sizeof(cap->card));
 770         snprintf(cap->bus_info, sizeof(cap->bus_info),
 771                  "PCI:%s", pci_name(dev->pci_dev));
 772         cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
 773                            V4L2_CAP_READWRITE;
 774         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 775         return 0;
 776 }
 777
 778 static int tw686x_set_standard(struct tw686x_video_channel *vc, v4l2_std_id id)
 779 {
 780         u32 val;
 781
 782         if (id & V4L2_STD_NTSC)
 783                 val = 0;
 784         else if (id & V4L2_STD_PAL)
 785                 val = 1;
 786         else if (id & V4L2_STD_SECAM)
 787                 val = 2;
 788         else if (id & V4L2_STD_NTSC_443)
 789                 val = 3;
 790         else if (id & V4L2_STD_PAL_M)
 791                 val = 4;
 792         else if (id & V4L2_STD_PAL_Nc)
 793                 val = 5;
 794         else if (id & V4L2_STD_PAL_60)
 795                 val = 6;
 796         else
 797                 return -EINVAL;
 798
 799         vc->video_standard = id;
 800         reg_write(vc->dev, SDT[vc->ch], val);
 801
 802         val = reg_read(vc->dev, VIDEO_CONTROL1);
 803         if (id & V4L2_STD_525_60)
 804                 val &= ~(1 << (SYS_MODE_DMA_SHIFT + vc->ch));
 805         else
 806                 val |= (1 << (SYS_MODE_DMA_SHIFT + vc->ch));
 807         reg_write(vc->dev, VIDEO_CONTROL1, val);
 808
 809         return 0;
 810 }
 811
 812 static int tw686x_s_std(struct file *file, void *priv, v4l2_std_id id)
 813 {
 814         struct tw686x_video_channel *vc = video_drvdata(file);
 815         struct v4l2_format f;
 816         int ret;
 817
 818         if (vc->video_standard == id)
 819                 return 0;
 820
 821         if (vb2_is_busy(&vc->vidq))
 822                 return -EBUSY;
 823
 824         ret = tw686x_set_standard(vc, id);
 825         if (ret)
 826                 return ret;
 827         /*
 828          * Adjust format after V4L2_STD_525_60/V4L2_STD_625_50 change,
 829          * calling g_fmt and s_fmt will sanitize the height
 830          * according to the standard.
 831          */
 832         tw686x_g_fmt_vid_cap(file, priv, &f);
 833         tw686x_s_fmt_vid_cap(file, priv, &f);
 834
 835         /*
 836          * Frame decimation depends on the chosen standard,
 837          * so reset it to the current value.
 838          */
 839         tw686x_set_framerate(vc, vc->fps);
 840         return 0;
 841 }
 842
 843 static int tw686x_querystd(struct file *file, void *priv, v4l2_std_id *std)
 844 {
 845         struct tw686x_video_channel *vc = video_drvdata(file);
 846         struct tw686x_dev *dev = vc->dev;
 847         unsigned int old_std, detected_std = 0;
 848         unsigned long end;
 849
 850         if (vb2_is_streaming(&vc->vidq))
 851                 return -EBUSY;
 852
 853         /* Enable and start standard detection */
 854         old_std = reg_read(dev, SDT[vc->ch]);
 855         reg_write(dev, SDT[vc->ch], 0x7);
 856         reg_write(dev, SDT_EN[vc->ch], 0xff);
 857
 858         end = jiffies + msecs_to_jiffies(500);
 859         while (time_is_after_jiffies(end)) {
 860
 861                 detected_std = reg_read(dev, SDT[vc->ch]);
 862                 if (!(detected_std & BIT(7)))
 863                         break;
 864                 msleep(100);
 865         }
 866         reg_write(dev, SDT[vc->ch], old_std);
 867
 868         /* Exit if still busy */
 869         if (detected_std & BIT(7))
 870                 return 0;
 871
 872         detected_std = (detected_std >> 4) & 0x7;
 873         switch (detected_std) {
 874         case TW686X_STD_NTSC_M:
 875                 *std &= V4L2_STD_NTSC;
 876                 break;
 877         case TW686X_STD_NTSC_443:
 878                 *std &= V4L2_STD_NTSC_443;
 879                 break;
 880         case TW686X_STD_PAL_M:
 881                 *std &= V4L2_STD_PAL_M;
 882                 break;
 883         case TW686X_STD_PAL_60:
 884                 *std &= V4L2_STD_PAL_60;
 885                 break;
 886         case TW686X_STD_PAL:
 887                 *std &= V4L2_STD_PAL;
 888                 break;
 889         case TW686X_STD_PAL_CN:
 890                 *std &= V4L2_STD_PAL_Nc;
 891                 break;
 892         case TW686X_STD_SECAM:
 893                 *std &= V4L2_STD_SECAM;
 894                 break;
 895         default:
 896                 *std = 0;
 897         }
 898         return 0;
 899 }
 900
 901 static int tw686x_g_std(struct file *file, void *priv, v4l2_std_id *id)
 902 {
 903         struct tw686x_video_channel *vc = video_drvdata(file);
 904
 905         *id = vc->video_standard;
 906         return 0;
 907 }
 908
 909 static int tw686x_enum_framesizes(struct file *file, void *priv,
 910                                   struct v4l2_frmsizeenum *fsize)
 911 {
 912         struct tw686x_video_channel *vc = video_drvdata(file);
 913
 914         if (fsize->index)
 915                 return -EINVAL;
 916         fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
 917         fsize->stepwise.max_width = TW686X_VIDEO_WIDTH;
 918         fsize->stepwise.min_width = fsize->stepwise.max_width / 2;
 919         fsize->stepwise.step_width = fsize->stepwise.min_width;
 920         fsize->stepwise.max_height = TW686X_VIDEO_HEIGHT(vc->video_standard);
 921         fsize->stepwise.min_height = fsize->stepwise.max_height / 2;
 922         fsize->stepwise.step_height = fsize->stepwise.min_height;
 923         return 0;
 924 }
 925
 926 static int tw686x_enum_frameintervals(struct file *file, void *priv,
 927                                       struct v4l2_frmivalenum *ival)
 928 {
 929         struct tw686x_video_channel *vc = video_drvdata(file);
 930         int max_fps = TW686X_MAX_FPS(vc->video_standard);
 931         int max_rates = DIV_ROUND_UP(max_fps, 2);
 932
 933         if (ival->index >= max_rates)
 934                 return -EINVAL;
 935
 936         ival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
 937         ival->discrete.numerator = 1;
 938         if (ival->index < (max_rates - 1))
 939                 ival->discrete.denominator = (ival->index + 1) * 2;
 940         else
 941                 ival->discrete.denominator = max_fps;
 942         return 0;
 943 }
 944
 945 static int tw686x_g_parm(struct file *file, void *priv,
 946                          struct v4l2_streamparm *sp)
 947 {
 948         struct tw686x_video_channel *vc = video_drvdata(file);
 949         struct v4l2_captureparm *cp = &sp->parm.capture;
 950
 951         if (sp->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 952                 return -EINVAL;
 953         sp->parm.capture.readbuffers = 3;
 954
 955         cp->capability = V4L2_CAP_TIMEPERFRAME;
 956         cp->timeperframe.numerator = 1;
 957         cp->timeperframe.denominator = vc->fps;
 958         return 0;
 959 }
 960
 961 static int tw686x_s_parm(struct file *file, void *priv,
 962                          struct v4l2_streamparm *sp)
 963 {
 964         struct tw686x_video_channel *vc = video_drvdata(file);
 965         struct v4l2_captureparm *cp = &sp->parm.capture;
 966         unsigned int denominator = cp->timeperframe.denominator;
 967         unsigned int numerator = cp->timeperframe.numerator;
 968         unsigned int fps;
 969
 970         if (vb2_is_busy(&vc->vidq))
 971                 return -EBUSY;
 972
 973         fps = (!numerator || !denominator) ? 0 : denominator / numerator;
 974         if (vc->fps != fps)
 975                 tw686x_set_framerate(vc, fps);
 976         return tw686x_g_parm(file, priv, sp);
 977 }
 978
 979 static int tw686x_enum_fmt_vid_cap(struct file *file, void *priv,
 980                                    struct v4l2_fmtdesc *f)
 981 {
 982         if (f->index >= ARRAY_SIZE(formats))
 983                 return -EINVAL;
 984         f->pixelformat = formats[f->index].fourcc;
 985         return 0;
 986 }
 987
 988 static void tw686x_set_input(struct tw686x_video_channel *vc, unsigned int i)
 989 {
 990         u32 val;
 991
 992         vc->input = i;
 993
 994         val = reg_read(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch]);
 995         val &= ~(0x3 << 30);
 996         val |= i << 30;
 997         reg_write(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch], val);
 998 }
 999
1000 static int tw686x_s_input(struct file *file, void *priv, unsigned int i)
1001 {
1002         struct tw686x_video_channel *vc = video_drvdata(file);
1003
1004         if (i >= TW686X_INPUTS_PER_CH)
1005                 return -EINVAL;
1006         if (i == vc->input)
1007                 return 0;
1008         /*
1009          * Not sure we are able to support on the fly input change
1010          */
1011         if (vb2_is_busy(&vc->vidq))
1012                 return -EBUSY;
1013
1014         tw686x_set_input(vc, i);
1015         return 0;
1016 }
1017
1018 static int tw686x_g_input(struct file *file, void *priv, unsigned int *i)
1019 {
1020         struct tw686x_video_channel *vc = video_drvdata(file);
1021
1022         *i = vc->input;
1023         return 0;
1024 }
1025
1026 static int tw686x_enum_input(struct file *file, void *priv,
1027                              struct v4l2_input *i)
1028 {
1029         struct tw686x_video_channel *vc = video_drvdata(file);
1030         unsigned int vidstat;
1031
1032         if (i->index >= TW686X_INPUTS_PER_CH)
1033                 return -EINVAL;
1034
1035         snprintf(i->name, sizeof(i->name), "Composite%d", i->index);
1036         i->type = V4L2_INPUT_TYPE_CAMERA;
1037         i->std = vc->device->tvnorms;
1038         i->capabilities = V4L2_IN_CAP_STD;
1039
1040         vidstat = reg_read(vc->dev, VIDSTAT[vc->ch]);
1041         i->status = 0;
1042         if (vidstat & TW686X_VIDSTAT_VDLOSS)
1043                 i->status |= V4L2_IN_ST_NO_SIGNAL;
1044         if (!(vidstat & TW686X_VIDSTAT_HLOCK))
1045                 i->status |= V4L2_IN_ST_NO_H_LOCK;
1046
1047         return 0;
1048 }
1049
1050 static const struct v4l2_file_operations tw686x_video_fops = {
1051         .owner          = THIS_MODULE,
1052         .open           = v4l2_fh_open,
1053         .unlocked_ioctl = video_ioctl2,
1054         .release        = vb2_fop_release,
1055         .poll           = vb2_fop_poll,
1056         .read           = vb2_fop_read,
1057         .mmap           = vb2_fop_mmap,
1058 };
1059
1060 static const struct v4l2_ioctl_ops tw686x_video_ioctl_ops = {
1061         .vidioc_querycap                = tw686x_querycap,
1062         .vidioc_g_fmt_vid_cap           = tw686x_g_fmt_vid_cap,
1063         .vidioc_s_fmt_vid_cap           = tw686x_s_fmt_vid_cap,
1064         .vidioc_enum_fmt_vid_cap        = tw686x_enum_fmt_vid_cap,
1065         .vidioc_try_fmt_vid_cap         = tw686x_try_fmt_vid_cap,
1066
1067         .vidioc_querystd                = tw686x_querystd,
1068         .vidioc_g_std                   = tw686x_g_std,
1069         .vidioc_s_std                   = tw686x_s_std,
1070
1071         .vidioc_g_parm                  = tw686x_g_parm,
1072         .vidioc_s_parm                  = tw686x_s_parm,
1073         .vidioc_enum_framesizes         = tw686x_enum_framesizes,
1074         .vidioc_enum_frameintervals     = tw686x_enum_frameintervals,
1075
1076         .vidioc_enum_input              = tw686x_enum_input,
1077         .vidioc_g_input                 = tw686x_g_input,
1078         .vidioc_s_input                 = tw686x_s_input,
1079
1080         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
1081         .vidioc_querybuf                = vb2_ioctl_querybuf,
1082         .vidioc_qbuf                    = vb2_ioctl_qbuf,
1083         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
1084         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
1085         .vidioc_streamon                = vb2_ioctl_streamon,
1086         .vidioc_streamoff               = vb2_ioctl_streamoff,
1087         .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
1088
1089         .vidioc_log_status              = v4l2_ctrl_log_status,
1090         .vidioc_subscribe_event         = v4l2_ctrl_subscribe_event,
1091         .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
1092 };
1093
1094 void tw686x_video_irq(struct tw686x_dev *dev, unsigned long requests,
1095                       unsigned int pb_status, unsigned int fifo_status,
1096                       unsigned int *reset_ch)
1097 {
1098         struct tw686x_video_channel *vc;
1099         unsigned long flags;
1100         unsigned int ch, pb;
1101
1102         for_each_set_bit(ch, &requests, max_channels(dev)) {
1103                 vc = &dev->video_channels[ch];
1104
1105                 /*
1106                  * This can either be a blue frame (with signal-lost bit set)
1107                  * or a good frame (with signal-lost bit clear). If we have just
1108                  * got signal, then this channel needs resetting.
1109                  */
1110                 if (vc->no_signal && !(fifo_status & BIT(ch))) {
1111                         v4l2_printk(KERN_DEBUG, &dev->v4l2_dev,
1112                                     "video%d: signal recovered\n", vc->num);
1113                         vc->no_signal = false;
1114                         *reset_ch |= BIT(ch);
1115                         vc->pb = 0;
1116                         continue;
1117                 }
1118                 vc->no_signal = !!(fifo_status & BIT(ch));
1119
1120                 /* Check FIFO errors only if there's signal */
1121                 if (!vc->no_signal) {
1122                         u32 fifo_ov, fifo_bad;
1123
1124                         fifo_ov = (fifo_status >> 24) & BIT(ch);
1125                         fifo_bad = (fifo_status >> 16) & BIT(ch);
1126                         if (fifo_ov || fifo_bad) {
1127                                 /* Mark this channel for reset */
1128                                 v4l2_printk(KERN_DEBUG, &dev->v4l2_dev,
1129                                             "video%d: FIFO error\n", vc->num);
1130                                 *reset_ch |= BIT(ch);
1131                                 vc->pb = 0;
1132                                 continue;
1133                         }
1134                 }
1135
1136                 pb = !!(pb_status & BIT(ch));
1137                 if (vc->pb != pb) {
1138                         /* Mark this channel for reset */
1139                         v4l2_printk(KERN_DEBUG, &dev->v4l2_dev,
1140                                     "video%d: unexpected p-b buffer!\n",
1141                                     vc->num);
1142                         *reset_ch |= BIT(ch);
1143                         vc->pb = 0;
1144                         continue;
1145                 }
1146
1147                 spin_lock_irqsave(&vc->qlock, flags);
1148                 tw686x_buf_done(vc, pb);
1149                 dev->dma_ops->buf_refill(vc, pb);
1150                 spin_unlock_irqrestore(&vc->qlock, flags);
1151         }
1152 }
1153
1154 void tw686x_video_free(struct tw686x_dev *dev)
1155 {
1156         unsigned int ch, pb;
1157
1158         for (ch = 0; ch < max_channels(dev); ch++) {
1159                 struct tw686x_video_channel *vc = &dev->video_channels[ch];
1160
1161                 video_unregister_device(vc->device);
1162
1163                 if (dev->dma_ops->free)
1164                         for (pb = 0; pb < 2; pb++)
1165                                 dev->dma_ops->free(vc, pb);
1166         }
1167 }
1168
1169 int tw686x_video_init(struct tw686x_dev *dev)
1170 {
1171         unsigned int ch, val;
1172         int err;
1173
1174         if (dev->dma_mode == TW686X_DMA_MODE_MEMCPY)
1175                 dev->dma_ops = &memcpy_dma_ops;
1176         else if (dev->dma_mode == TW686X_DMA_MODE_CONTIG)
1177                 dev->dma_ops = &contig_dma_ops;
1178         else if (dev->dma_mode == TW686X_DMA_MODE_SG)
1179                 dev->dma_ops = &sg_dma_ops;
1180         else
1181                 return -EINVAL;
1182
1183         err = v4l2_device_register(&dev->pci_dev->dev, &dev->v4l2_dev);
1184         if (err)
1185                 return err;
1186
1187         if (dev->dma_ops->setup) {
1188                 err = dev->dma_ops->setup(dev);
1189                 if (err)
1190                         return err;
1191         }
1192
1193         for (ch = 0; ch < max_channels(dev); ch++) {
1194                 struct tw686x_video_channel *vc = &dev->video_channels[ch];
1195                 struct video_device *vdev;
1196
1197                 mutex_init(&vc->vb_mutex);
1198                 spin_lock_init(&vc->qlock);
1199                 INIT_LIST_HEAD(&vc->vidq_queued);
1200
1201                 vc->dev = dev;
1202                 vc->ch = ch;
1203
1204                 /* default settings */
1205                 err = tw686x_set_standard(vc, V4L2_STD_NTSC);
1206                 if (err)
1207                         goto error;
1208
1209                 err = tw686x_set_format(vc, formats[0].fourcc,
1210                                 TW686X_VIDEO_WIDTH,
1211                                 TW686X_VIDEO_HEIGHT(vc->video_standard),
1212                                 true);
1213                 if (err)
1214                         goto error;
1215
1216                 tw686x_set_input(vc, 0);
1217                 tw686x_set_framerate(vc, 30);
1218                 reg_write(dev, VDELAY_LO[ch], 0x14);
1219                 reg_write(dev, HACTIVE_LO[ch], 0xd0);
1220                 reg_write(dev, VIDEO_SIZE[ch], 0);
1221
1222                 vc->vidq.io_modes = VB2_READ | VB2_MMAP | VB2_DMABUF;
1223                 vc->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1224                 vc->vidq.drv_priv = vc;
1225                 vc->vidq.buf_struct_size = sizeof(struct tw686x_v4l2_buf);
1226                 vc->vidq.ops = &tw686x_video_qops;
1227                 vc->vidq.mem_ops = dev->dma_ops->mem_ops;
1228                 vc->vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1229                 vc->vidq.min_buffers_needed = 2;
1230                 vc->vidq.lock = &vc->vb_mutex;
1231                 vc->vidq.gfp_flags = GFP_DMA32;
1232                 vc->vidq.dev = &dev->pci_dev->dev;
1233
1234                 err = vb2_queue_init(&vc->vidq);
1235                 if (err) {
1236                         v4l2_err(&dev->v4l2_dev,
1237                                  "dma%d: cannot init vb2 queue\n", ch);
1238                         goto error;
1239                 }
1240
1241                 err = v4l2_ctrl_handler_init(&vc->ctrl_handler, 4);
1242                 if (err) {
1243                         v4l2_err(&dev->v4l2_dev,
1244                                  "dma%d: cannot init ctrl handler\n", ch);
1245                         goto error;
1246                 }
1247                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
1248                                   V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
1249                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
1250                                   V4L2_CID_CONTRAST, 0, 255, 1, 100);
1251                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
1252                                   V4L2_CID_SATURATION, 0, 255, 1, 128);
1253                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
1254                                   V4L2_CID_HUE, -128, 127, 1, 0);
1255                 err = vc->ctrl_handler.error;
1256                 if (err)
1257                         goto error;
1258
1259                 err = v4l2_ctrl_handler_setup(&vc->ctrl_handler);
1260                 if (err)
1261                         goto error;
1262
1263                 vdev = video_device_alloc();
1264                 if (!vdev) {
1265                         v4l2_err(&dev->v4l2_dev,
1266                                  "dma%d: unable to allocate device\n", ch);
1267                         err = -ENOMEM;
1268                         goto error;
1269                 }
1270
1271                 snprintf(vdev->name, sizeof(vdev->name), "%s video", dev->name);
1272                 vdev->fops = &tw686x_video_fops;
1273                 vdev->ioctl_ops = &tw686x_video_ioctl_ops;
1274                 vdev->release = video_device_release;
1275                 vdev->v4l2_dev = &dev->v4l2_dev;
1276                 vdev->queue = &vc->vidq;
1277                 vdev->tvnorms = V4L2_STD_525_60 | V4L2_STD_625_50;
1278                 vdev->minor = -1;
1279                 vdev->lock = &vc->vb_mutex;
1280                 vdev->ctrl_handler = &vc->ctrl_handler;
1281                 vc->device = vdev;
1282                 video_set_drvdata(vdev, vc);
1283
1284                 err = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
1285                 if (err < 0)
1286                         goto error;
1287                 vc->num = vdev->num;
1288         }
1289
1290         val = TW686X_DEF_PHASE_REF;
1291         for (ch = 0; ch < max_channels(dev); ch++)
1292                 val |= dev->dma_ops->hw_dma_mode << (16 + ch * 2);
1293         reg_write(dev, PHASE_REF, val);
1294
1295         reg_write(dev, MISC2[0], 0xe7);
1296         reg_write(dev, VCTRL1[0], 0xcc);
1297         reg_write(dev, LOOP[0], 0xa5);
1298         if (max_channels(dev) > 4) {
1299                 reg_write(dev, VCTRL1[1], 0xcc);
1300                 reg_write(dev, LOOP[1], 0xa5);
1301                 reg_write(dev, MISC2[1], 0xe7);
1302         }
1303         return 0;
1304
1305 error:
1306         tw686x_video_free(dev);
1307         return err;
1308 }