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

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