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 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_s_fmt_vid_cap(struct file *file, void *priv,
 676                                 struct v4l2_format *f)
 677 {
 678         struct tw686x_video_channel *vc = video_drvdata(file);
 679         struct tw686x_dev *dev = vc->dev;
 680         u32 val, width, line_width, height;
 681         unsigned long bitsperframe;
 682         int err, pb;
 683
 684         if (vb2_is_busy(&vc->vidq))
 685                 return -EBUSY;
 686
 687         bitsperframe = vc->width * vc->height * vc->format->depth;
 688         err = tw686x_try_fmt_vid_cap(file, priv, f);
 689         if (err)
 690                 return err;
 691
 692         vc->format = format_by_fourcc(f->fmt.pix.pixelformat);
 693         vc->width = f->fmt.pix.width;
 694         vc->height = f->fmt.pix.height;
 695
 696         /* We need new DMA buffers if the framesize has changed */
 697         if (dev->dma_ops->alloc &&
 698             bitsperframe != vc->width * vc->height * vc->format->depth) {
 699                 for (pb = 0; pb < 2; pb++)
 700                         dev->dma_ops->free(vc, pb);
 701
 702                 for (pb = 0; pb < 2; pb++) {
 703                         err = dev->dma_ops->alloc(vc, pb);
 704                         if (err) {
 705                                 if (pb > 0)
 706                                         dev->dma_ops->free(vc, 0);
 707                                 return err;
 708                         }
 709                 }
 710         }
 711
 712         val = reg_read(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch]);
 713
 714         if (vc->width <= TW686X_VIDEO_WIDTH / 2)
 715                 val |= BIT(23);
 716         else
 717                 val &= ~BIT(23);
 718
 719         if (vc->height <= TW686X_VIDEO_HEIGHT(vc->video_standard) / 2)
 720                 val |= BIT(24);
 721         else
 722                 val &= ~BIT(24);
 723
 724         val &= ~0x7ffff;
 725
 726         /* Program the DMA scatter-gather */
 727         if (dev->dma_mode == TW686X_DMA_MODE_SG) {
 728                 u32 start_idx, end_idx;
 729
 730                 start_idx = is_second_gen(dev) ?
 731                                 0 : vc->ch * TW686X_MAX_SG_DESC_COUNT;
 732                 end_idx = start_idx + TW686X_MAX_SG_DESC_COUNT - 1;
 733
 734                 val |= (end_idx << 10) | start_idx;
 735         }
 736
 737         val &= ~(0x7 << 20);
 738         val |= vc->format->mode << 20;
 739         reg_write(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch], val);
 740
 741         /* Program the DMA frame size */
 742         width = (vc->width * 2) & 0x7ff;
 743         height = vc->height / 2;
 744         line_width = (vc->width * 2) & 0x7ff;
 745         val = (height << 22) | (line_width << 11)  | width;
 746         reg_write(vc->dev, VDMA_WHP[vc->ch], val);
 747         return 0;
 748 }
 749
 750 static int tw686x_querycap(struct file *file, void *priv,
 751                            struct v4l2_capability *cap)
 752 {
 753         struct tw686x_video_channel *vc = video_drvdata(file);
 754         struct tw686x_dev *dev = vc->dev;
 755
 756         strlcpy(cap->driver, "tw686x", sizeof(cap->driver));
 757         strlcpy(cap->card, dev->name, sizeof(cap->card));
 758         snprintf(cap->bus_info, sizeof(cap->bus_info),
 759                  "PCI:%s", pci_name(dev->pci_dev));
 760         cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
 761                            V4L2_CAP_READWRITE;
 762         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 763         return 0;
 764 }
 765
 766 static int tw686x_s_std(struct file *file, void *priv, v4l2_std_id id)
 767 {
 768         struct tw686x_video_channel *vc = video_drvdata(file);
 769         struct v4l2_format f;
 770         u32 val, ret;
 771
 772         if (vc->video_standard == id)
 773                 return 0;
 774
 775         if (vb2_is_busy(&vc->vidq))
 776                 return -EBUSY;
 777
 778         if (id & V4L2_STD_NTSC)
 779                 val = 0;
 780         else if (id & V4L2_STD_PAL)
 781                 val = 1;
 782         else if (id & V4L2_STD_SECAM)
 783                 val = 2;
 784         else if (id & V4L2_STD_NTSC_443)
 785                 val = 3;
 786         else if (id & V4L2_STD_PAL_M)
 787                 val = 4;
 788         else if (id & V4L2_STD_PAL_Nc)
 789                 val = 5;
 790         else if (id & V4L2_STD_PAL_60)
 791                 val = 6;
 792         else
 793                 return -EINVAL;
 794
 795         vc->video_standard = id;
 796         reg_write(vc->dev, SDT[vc->ch], val);
 797
 798         val = reg_read(vc->dev, VIDEO_CONTROL1);
 799         if (id & V4L2_STD_525_60)
 800                 val &= ~(1 << (SYS_MODE_DMA_SHIFT + vc->ch));
 801         else
 802                 val |= (1 << (SYS_MODE_DMA_SHIFT + vc->ch));
 803         reg_write(vc->dev, VIDEO_CONTROL1, val);
 804
 805         /*
 806          * Adjust format after V4L2_STD_525_60/V4L2_STD_625_50 change,
 807          * calling g_fmt and s_fmt will sanitize the height
 808          * according to the standard.
 809          */
 810         ret = tw686x_g_fmt_vid_cap(file, priv, &f);
 811         if (!ret)
 812                 tw686x_s_fmt_vid_cap(file, priv, &f);
 813
 814         /*
 815          * Frame decimation depends on the chosen standard,
 816          * so reset it to the current value.
 817          */
 818         tw686x_set_framerate(vc, vc->fps);
 819         return 0;
 820 }
 821
 822 static int tw686x_querystd(struct file *file, void *priv, v4l2_std_id *std)
 823 {
 824         struct tw686x_video_channel *vc = video_drvdata(file);
 825         struct tw686x_dev *dev = vc->dev;
 826         unsigned int old_std, detected_std = 0;
 827         unsigned long end;
 828
 829         if (vb2_is_streaming(&vc->vidq))
 830                 return -EBUSY;
 831
 832         /* Enable and start standard detection */
 833         old_std = reg_read(dev, SDT[vc->ch]);
 834         reg_write(dev, SDT[vc->ch], 0x7);
 835         reg_write(dev, SDT_EN[vc->ch], 0xff);
 836
 837         end = jiffies + msecs_to_jiffies(500);
 838         while (time_is_after_jiffies(end)) {
 839
 840                 detected_std = reg_read(dev, SDT[vc->ch]);
 841                 if (!(detected_std & BIT(7)))
 842                         break;
 843                 msleep(100);
 844         }
 845         reg_write(dev, SDT[vc->ch], old_std);
 846
 847         /* Exit if still busy */
 848         if (detected_std & BIT(7))
 849                 return 0;
 850
 851         detected_std = (detected_std >> 4) & 0x7;
 852         switch (detected_std) {
 853         case TW686X_STD_NTSC_M:
 854                 *std &= V4L2_STD_NTSC;
 855                 break;
 856         case TW686X_STD_NTSC_443:
 857                 *std &= V4L2_STD_NTSC_443;
 858                 break;
 859         case TW686X_STD_PAL_M:
 860                 *std &= V4L2_STD_PAL_M;
 861                 break;
 862         case TW686X_STD_PAL_60:
 863                 *std &= V4L2_STD_PAL_60;
 864                 break;
 865         case TW686X_STD_PAL:
 866                 *std &= V4L2_STD_PAL;
 867                 break;
 868         case TW686X_STD_PAL_CN:
 869                 *std &= V4L2_STD_PAL_Nc;
 870                 break;
 871         case TW686X_STD_SECAM:
 872                 *std &= V4L2_STD_SECAM;
 873                 break;
 874         default:
 875                 *std = 0;
 876         }
 877         return 0;
 878 }
 879
 880 static int tw686x_g_std(struct file *file, void *priv, v4l2_std_id *id)
 881 {
 882         struct tw686x_video_channel *vc = video_drvdata(file);
 883
 884         *id = vc->video_standard;
 885         return 0;
 886 }
 887
 888 static int tw686x_g_parm(struct file *file, void *priv,
 889                          struct v4l2_streamparm *sp)
 890 {
 891         struct tw686x_video_channel *vc = video_drvdata(file);
 892         struct v4l2_captureparm *cp = &sp->parm.capture;
 893
 894         if (sp->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 895                 return -EINVAL;
 896         sp->parm.capture.readbuffers = 3;
 897
 898         cp->capability = V4L2_CAP_TIMEPERFRAME;
 899         cp->timeperframe.numerator = 1;
 900         cp->timeperframe.denominator = vc->fps;
 901         return 0;
 902 }
 903
 904 static int tw686x_s_parm(struct file *file, void *priv,
 905                          struct v4l2_streamparm *sp)
 906 {
 907         struct tw686x_video_channel *vc = video_drvdata(file);
 908         struct v4l2_captureparm *cp = &sp->parm.capture;
 909         unsigned int denominator = cp->timeperframe.denominator;
 910         unsigned int numerator = cp->timeperframe.numerator;
 911         unsigned int fps;
 912
 913         if (vb2_is_busy(&vc->vidq))
 914                 return -EBUSY;
 915
 916         fps = (!numerator || !denominator) ? 0 : denominator / numerator;
 917         if (vc->fps != fps)
 918                 tw686x_set_framerate(vc, fps);
 919         return tw686x_g_parm(file, priv, sp);
 920 }
 921
 922 static int tw686x_enum_fmt_vid_cap(struct file *file, void *priv,
 923                                    struct v4l2_fmtdesc *f)
 924 {
 925         if (f->index >= ARRAY_SIZE(formats))
 926                 return -EINVAL;
 927         f->pixelformat = formats[f->index].fourcc;
 928         return 0;
 929 }
 930
 931 static int tw686x_s_input(struct file *file, void *priv, unsigned int i)
 932 {
 933         struct tw686x_video_channel *vc = video_drvdata(file);
 934         u32 val;
 935
 936         if (i >= TW686X_INPUTS_PER_CH)
 937                 return -EINVAL;
 938         if (i == vc->input)
 939                 return 0;
 940         /*
 941          * Not sure we are able to support on the fly input change
 942          */
 943         if (vb2_is_busy(&vc->vidq))
 944                 return -EBUSY;
 945
 946         vc->input = i;
 947
 948         val = reg_read(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch]);
 949         val &= ~(0x3 << 30);
 950         val |= i << 30;
 951         reg_write(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch], val);
 952         return 0;
 953 }
 954
 955 static int tw686x_g_input(struct file *file, void *priv, unsigned int *i)
 956 {
 957         struct tw686x_video_channel *vc = video_drvdata(file);
 958
 959         *i = vc->input;
 960         return 0;
 961 }
 962
 963 static int tw686x_enum_input(struct file *file, void *priv,
 964                              struct v4l2_input *i)
 965 {
 966         struct tw686x_video_channel *vc = video_drvdata(file);
 967         unsigned int vidstat;
 968
 969         if (i->index >= TW686X_INPUTS_PER_CH)
 970                 return -EINVAL;
 971
 972         snprintf(i->name, sizeof(i->name), "Composite%d", i->index);
 973         i->type = V4L2_INPUT_TYPE_CAMERA;
 974         i->std = vc->device->tvnorms;
 975         i->capabilities = V4L2_IN_CAP_STD;
 976
 977         vidstat = reg_read(vc->dev, VIDSTAT[vc->ch]);
 978         i->status = 0;
 979         if (vidstat & TW686X_VIDSTAT_VDLOSS)
 980                 i->status |= V4L2_IN_ST_NO_SIGNAL;
 981         if (!(vidstat & TW686X_VIDSTAT_HLOCK))
 982                 i->status |= V4L2_IN_ST_NO_H_LOCK;
 983
 984         return 0;
 985 }
 986
 987 static const struct v4l2_file_operations tw686x_video_fops = {
 988         .owner          = THIS_MODULE,
 989         .open           = v4l2_fh_open,
 990         .unlocked_ioctl = video_ioctl2,
 991         .release        = vb2_fop_release,
 992         .poll           = vb2_fop_poll,
 993         .read           = vb2_fop_read,
 994         .mmap           = vb2_fop_mmap,
 995 };
 996
 997 static const struct v4l2_ioctl_ops tw686x_video_ioctl_ops = {
 998         .vidioc_querycap                = tw686x_querycap,
 999         .vidioc_g_fmt_vid_cap           = tw686x_g_fmt_vid_cap,
1000         .vidioc_s_fmt_vid_cap           = tw686x_s_fmt_vid_cap,
1001         .vidioc_enum_fmt_vid_cap        = tw686x_enum_fmt_vid_cap,
1002         .vidioc_try_fmt_vid_cap         = tw686x_try_fmt_vid_cap,
1003
1004         .vidioc_querystd                = tw686x_querystd,
1005         .vidioc_g_std                   = tw686x_g_std,
1006         .vidioc_s_std                   = tw686x_s_std,
1007
1008         .vidioc_g_parm                  = tw686x_g_parm,
1009         .vidioc_s_parm                  = tw686x_s_parm,
1010
1011         .vidioc_enum_input              = tw686x_enum_input,
1012         .vidioc_g_input                 = tw686x_g_input,
1013         .vidioc_s_input                 = tw686x_s_input,
1014
1015         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
1016         .vidioc_querybuf                = vb2_ioctl_querybuf,
1017         .vidioc_qbuf                    = vb2_ioctl_qbuf,
1018         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
1019         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
1020         .vidioc_streamon                = vb2_ioctl_streamon,
1021         .vidioc_streamoff               = vb2_ioctl_streamoff,
1022         .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
1023
1024         .vidioc_log_status              = v4l2_ctrl_log_status,
1025         .vidioc_subscribe_event         = v4l2_ctrl_subscribe_event,
1026         .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
1027 };
1028
1029 void tw686x_video_irq(struct tw686x_dev *dev, unsigned long requests,
1030                       unsigned int pb_status, unsigned int fifo_status,
1031                       unsigned int *reset_ch)
1032 {
1033         struct tw686x_video_channel *vc;
1034         unsigned long flags;
1035         unsigned int ch, pb;
1036
1037         for_each_set_bit(ch, &requests, max_channels(dev)) {
1038                 vc = &dev->video_channels[ch];
1039
1040                 /*
1041                  * This can either be a blue frame (with signal-lost bit set)
1042                  * or a good frame (with signal-lost bit clear). If we have just
1043                  * got signal, then this channel needs resetting.
1044                  */
1045                 if (vc->no_signal && !(fifo_status & BIT(ch))) {
1046                         v4l2_printk(KERN_DEBUG, &dev->v4l2_dev,
1047                                     "video%d: signal recovered\n", vc->num);
1048                         vc->no_signal = false;
1049                         *reset_ch |= BIT(ch);
1050                         vc->pb = 0;
1051                         continue;
1052                 }
1053                 vc->no_signal = !!(fifo_status & BIT(ch));
1054
1055                 /* Check FIFO errors only if there's signal */
1056                 if (!vc->no_signal) {
1057                         u32 fifo_ov, fifo_bad;
1058
1059                         fifo_ov = (fifo_status >> 24) & BIT(ch);
1060                         fifo_bad = (fifo_status >> 16) & BIT(ch);
1061                         if (fifo_ov || fifo_bad) {
1062                                 /* Mark this channel for reset */
1063                                 v4l2_printk(KERN_DEBUG, &dev->v4l2_dev,
1064                                             "video%d: FIFO error\n", vc->num);
1065                                 *reset_ch |= BIT(ch);
1066                                 vc->pb = 0;
1067                                 continue;
1068                         }
1069                 }
1070
1071                 pb = !!(pb_status & BIT(ch));
1072                 if (vc->pb != pb) {
1073                         /* Mark this channel for reset */
1074                         v4l2_printk(KERN_DEBUG, &dev->v4l2_dev,
1075                                     "video%d: unexpected p-b buffer!\n",
1076                                     vc->num);
1077                         *reset_ch |= BIT(ch);
1078                         vc->pb = 0;
1079                         continue;
1080                 }
1081
1082                 spin_lock_irqsave(&vc->qlock, flags);
1083                 tw686x_buf_done(vc, pb);
1084                 dev->dma_ops->buf_refill(vc, pb);
1085                 spin_unlock_irqrestore(&vc->qlock, flags);
1086         }
1087 }
1088
1089 void tw686x_video_free(struct tw686x_dev *dev)
1090 {
1091         unsigned int ch, pb;
1092
1093         for (ch = 0; ch < max_channels(dev); ch++) {
1094                 struct tw686x_video_channel *vc = &dev->video_channels[ch];
1095
1096                 if (vc->device)
1097                         video_unregister_device(vc->device);
1098
1099                 if (dev->dma_ops->free)
1100                         for (pb = 0; pb < 2; pb++)
1101                                 dev->dma_ops->free(vc, pb);
1102         }
1103 }
1104
1105 int tw686x_video_init(struct tw686x_dev *dev)
1106 {
1107         unsigned int ch, val, pb;
1108         int err;
1109
1110         if (dev->dma_mode == TW686X_DMA_MODE_MEMCPY)
1111                 dev->dma_ops = &memcpy_dma_ops;
1112         else if (dev->dma_mode == TW686X_DMA_MODE_CONTIG)
1113                 dev->dma_ops = &contig_dma_ops;
1114         else if (dev->dma_mode == TW686X_DMA_MODE_SG)
1115                 dev->dma_ops = &sg_dma_ops;
1116         else
1117                 return -EINVAL;
1118
1119         err = v4l2_device_register(&dev->pci_dev->dev, &dev->v4l2_dev);
1120         if (err)
1121                 return err;
1122
1123         if (dev->dma_ops->setup) {
1124                 err = dev->dma_ops->setup(dev);
1125                 if (err)
1126                         return err;
1127         }
1128
1129         for (ch = 0; ch < max_channels(dev); ch++) {
1130                 struct tw686x_video_channel *vc = &dev->video_channels[ch];
1131                 struct video_device *vdev;
1132
1133                 mutex_init(&vc->vb_mutex);
1134                 spin_lock_init(&vc->qlock);
1135                 INIT_LIST_HEAD(&vc->vidq_queued);
1136
1137                 vc->dev = dev;
1138                 vc->ch = ch;
1139
1140                 /* default settings */
1141                 vc->format = &formats[0];
1142                 vc->video_standard = V4L2_STD_NTSC;
1143                 vc->width = TW686X_VIDEO_WIDTH;
1144                 vc->height = TW686X_VIDEO_HEIGHT(vc->video_standard);
1145                 vc->input = 0;
1146
1147                 reg_write(vc->dev, SDT[ch], 0);
1148                 tw686x_set_framerate(vc, 30);
1149
1150                 reg_write(dev, VDELAY_LO[ch], 0x14);
1151                 reg_write(dev, HACTIVE_LO[ch], 0xd0);
1152                 reg_write(dev, VIDEO_SIZE[ch], 0);
1153
1154                 if (dev->dma_ops->alloc) {
1155                         for (pb = 0; pb < 2; pb++) {
1156                                 err = dev->dma_ops->alloc(vc, pb);
1157                                 if (err)
1158                                         goto error;
1159                         }
1160                 }
1161
1162                 vc->vidq.io_modes = VB2_READ | VB2_MMAP | VB2_DMABUF;
1163                 vc->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1164                 vc->vidq.drv_priv = vc;
1165                 vc->vidq.buf_struct_size = sizeof(struct tw686x_v4l2_buf);
1166                 vc->vidq.ops = &tw686x_video_qops;
1167                 vc->vidq.mem_ops = dev->dma_ops->mem_ops;
1168                 vc->vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1169                 vc->vidq.min_buffers_needed = 2;
1170                 vc->vidq.lock = &vc->vb_mutex;
1171                 vc->vidq.gfp_flags = GFP_DMA32;
1172                 vc->vidq.dev = &dev->pci_dev->dev;
1173
1174                 err = vb2_queue_init(&vc->vidq);
1175                 if (err) {
1176                         v4l2_err(&dev->v4l2_dev,
1177                                  "dma%d: cannot init vb2 queue\n", ch);
1178                         goto error;
1179                 }
1180
1181                 err = v4l2_ctrl_handler_init(&vc->ctrl_handler, 4);
1182                 if (err) {
1183                         v4l2_err(&dev->v4l2_dev,
1184                                  "dma%d: cannot init ctrl handler\n", ch);
1185                         goto error;
1186                 }
1187                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
1188                                   V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
1189                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
1190                                   V4L2_CID_CONTRAST, 0, 255, 1, 100);
1191                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
1192                                   V4L2_CID_SATURATION, 0, 255, 1, 128);
1193                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
1194                                   V4L2_CID_HUE, -128, 127, 1, 0);
1195                 err = vc->ctrl_handler.error;
1196                 if (err)
1197                         goto error;
1198
1199                 err = v4l2_ctrl_handler_setup(&vc->ctrl_handler);
1200                 if (err)
1201                         goto error;
1202
1203                 vdev = video_device_alloc();
1204                 if (!vdev) {
1205                         v4l2_err(&dev->v4l2_dev,
1206                                  "dma%d: unable to allocate device\n", ch);
1207                         err = -ENOMEM;
1208                         goto error;
1209                 }
1210
1211                 snprintf(vdev->name, sizeof(vdev->name), "%s video", dev->name);
1212                 vdev->fops = &tw686x_video_fops;
1213                 vdev->ioctl_ops = &tw686x_video_ioctl_ops;
1214                 vdev->release = video_device_release;
1215                 vdev->v4l2_dev = &dev->v4l2_dev;
1216                 vdev->queue = &vc->vidq;
1217                 vdev->tvnorms = V4L2_STD_525_60 | V4L2_STD_625_50;
1218                 vdev->minor = -1;
1219                 vdev->lock = &vc->vb_mutex;
1220                 vdev->ctrl_handler = &vc->ctrl_handler;
1221                 vc->device = vdev;
1222                 video_set_drvdata(vdev, vc);
1223
1224                 err = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
1225                 if (err < 0)
1226                         goto error;
1227                 vc->num = vdev->num;
1228         }
1229
1230         val = TW686X_DEF_PHASE_REF;
1231         for (ch = 0; ch < max_channels(dev); ch++)
1232                 val |= dev->dma_ops->hw_dma_mode << (16 + ch * 2);
1233         reg_write(dev, PHASE_REF, val);
1234
1235         reg_write(dev, MISC2[0], 0xe7);
1236         reg_write(dev, VCTRL1[0], 0xcc);
1237         reg_write(dev, LOOP[0], 0xa5);
1238         if (max_channels(dev) > 4) {
1239                 reg_write(dev, VCTRL1[1], 0xcc);
1240                 reg_write(dev, LOOP[1], 0xa5);
1241                 reg_write(dev, MISC2[1], 0xe7);
1242         }
1243         return 0;
1244
1245 error:
1246         tw686x_video_free(dev);
1247         return err;
1248 }