linux/drivers/media/pci/tw686x/tw686x-video.c
<<
>>
Prefs
   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
  33static 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
  49static 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 */
  77static 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                pci_free_consistent(dev->pci_dev, desc->size,
  96                                    desc->virt, desc->phys);
  97                desc->virt = NULL;
  98        }
  99}
 100
 101static 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 = pci_alloc_consistent(dev->pci_dev, len,
 114                                    &vc->dma_descs[pb].phys);
 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
 128static 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
 145static 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
 154static 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
 177static 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
 184static 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
 224static 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
 254static 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                pci_free_consistent(dev->pci_dev, desc->size,
 262                                    desc->virt, desc->phys);
 263                desc->virt = NULL;
 264        }
 265
 266        vc->sg_descs[pb] = NULL;
 267}
 268
 269static 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
 280                virt = pci_alloc_consistent(dev->pci_dev, desc->size,
 281                                            &desc->phys);
 282                if (!virt) {
 283                        v4l2_err(&dev->v4l2_dev,
 284                                 "dma%d: unable to allocate %s-buffer\n",
 285                                 vc->ch, pb ? "B" : "P");
 286                        return -ENOMEM;
 287                }
 288                desc->virt = virt;
 289                reg_write(dev, reg, desc->phys);
 290        } else {
 291                virt = dev->video_channels[0].dma_descs[pb].virt +
 292                       vc->ch * TW686X_SG_TABLE_SIZE;
 293        }
 294
 295        vc->sg_descs[pb] = virt;
 296        return 0;
 297}
 298
 299static int tw686x_sg_setup(struct tw686x_dev *dev)
 300{
 301        unsigned int sg_table_size, pb, ch, channels;
 302
 303        if (is_second_gen(dev)) {
 304                /*
 305                 * TW6865/TW6869: each channel needs a pair of
 306                 * P-B descriptor tables.
 307                 */
 308                channels = max_channels(dev);
 309                sg_table_size = TW686X_SG_TABLE_SIZE;
 310        } else {
 311                /*
 312                 * TW6864/TW6868: we need to allocate a pair of
 313                 * P-B descriptor tables, common for all channels.
 314                 * Each table will be bigger than 4 KB.
 315                 */
 316                channels = 1;
 317                sg_table_size = max_channels(dev) * TW686X_SG_TABLE_SIZE;
 318        }
 319
 320        for (ch = 0; ch < channels; ch++) {
 321                struct tw686x_video_channel *vc = &dev->video_channels[ch];
 322
 323                for (pb = 0; pb < 2; pb++)
 324                        vc->dma_descs[pb].size = sg_table_size;
 325        }
 326
 327        return 0;
 328}
 329
 330static const struct tw686x_dma_ops sg_dma_ops = {
 331        .setup          = tw686x_sg_setup,
 332        .alloc          = tw686x_sg_dma_alloc,
 333        .free           = tw686x_sg_dma_free,
 334        .buf_refill     = tw686x_sg_buf_refill,
 335        .mem_ops        = &vb2_dma_sg_memops,
 336        .hw_dma_mode    = TW686X_SG_MODE,
 337        .field          = V4L2_FIELD_SEQ_TB,
 338};
 339
 340static const unsigned int fps_map[15] = {
 341        /*
 342         * bit 31 enables selecting the field control register
 343         * bits 0-29 are a bitmask with fields that will be output.
 344         * For NTSC (and PAL-M, PAL-60), all 30 bits are used.
 345         * For other PAL standards, only the first 25 bits are used.
 346         */
 347        0x00000000, /* output all fields */
 348        0x80000006, /* 2 fps (60Hz), 2 fps (50Hz) */
 349        0x80018006, /* 4 fps (60Hz), 4 fps (50Hz) */
 350        0x80618006, /* 6 fps (60Hz), 6 fps (50Hz) */
 351        0x81818186, /* 8 fps (60Hz), 8 fps (50Hz) */
 352        0x86186186, /* 10 fps (60Hz), 8 fps (50Hz) */
 353        0x86619866, /* 12 fps (60Hz), 10 fps (50Hz) */
 354        0x86666666, /* 14 fps (60Hz), 12 fps (50Hz) */
 355        0x9999999e, /* 16 fps (60Hz), 14 fps (50Hz) */
 356        0x99e6799e, /* 18 fps (60Hz), 16 fps (50Hz) */
 357        0x9e79e79e, /* 20 fps (60Hz), 16 fps (50Hz) */
 358        0x9e7e7e7e, /* 22 fps (60Hz), 18 fps (50Hz) */
 359        0x9fe7f9fe, /* 24 fps (60Hz), 20 fps (50Hz) */
 360        0x9ffe7ffe, /* 26 fps (60Hz), 22 fps (50Hz) */
 361        0x9ffffffe, /* 28 fps (60Hz), 24 fps (50Hz) */
 362};
 363
 364static unsigned int tw686x_real_fps(unsigned int index, unsigned int max_fps)
 365{
 366        unsigned long mask;
 367
 368        if (!index || index >= ARRAY_SIZE(fps_map))
 369                return max_fps;
 370
 371        mask = GENMASK(max_fps - 1, 0);
 372        return hweight_long(fps_map[index] & mask);
 373}
 374
 375static unsigned int tw686x_fps_idx(unsigned int fps, unsigned int max_fps)
 376{
 377        unsigned int idx, real_fps;
 378        int delta;
 379
 380        /* First guess */
 381        idx = (12 + 15 * fps) / max_fps;
 382
 383        /* Minimal possible framerate is 2 frames per second */
 384        if (!idx)
 385                return 1;
 386
 387        /* Check if the difference is bigger than abs(1) and adjust */
 388        real_fps = tw686x_real_fps(idx, max_fps);
 389        delta = real_fps - fps;
 390        if (delta < -1)
 391                idx++;
 392        else if (delta > 1)
 393                idx--;
 394
 395        /* Max framerate */
 396        if (idx >= 15)
 397                return 0;
 398
 399        return idx;
 400}
 401
 402static void tw686x_set_framerate(struct tw686x_video_channel *vc,
 403                                 unsigned int fps)
 404{
 405        unsigned int i;
 406
 407        i = tw686x_fps_idx(fps, TW686X_MAX_FPS(vc->video_standard));
 408        reg_write(vc->dev, VIDEO_FIELD_CTRL[vc->ch], fps_map[i]);
 409        vc->fps = tw686x_real_fps(i, TW686X_MAX_FPS(vc->video_standard));
 410}
 411
 412static const struct tw686x_format *format_by_fourcc(unsigned int fourcc)
 413{
 414        unsigned int cnt;
 415
 416        for (cnt = 0; cnt < ARRAY_SIZE(formats); cnt++)
 417                if (formats[cnt].fourcc == fourcc)
 418                        return &formats[cnt];
 419        return NULL;
 420}
 421
 422static int tw686x_queue_setup(struct vb2_queue *vq,
 423                              unsigned int *nbuffers, unsigned int *nplanes,
 424                              unsigned int sizes[], struct device *alloc_devs[])
 425{
 426        struct tw686x_video_channel *vc = vb2_get_drv_priv(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 (vq->num_buffers + *nbuffers < 3)
 435                *nbuffers = 3 - vq->num_buffers;
 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
 448static 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
 472static 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
 493static 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
 538err_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
 545static 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
 564static 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
 576static 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
 586static 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
 619static const struct v4l2_ctrl_ops ctrl_ops = {
 620        .s_ctrl = tw686x_s_ctrl,
 621};
 622
 623static 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
 639static 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
 671static 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
 736static 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
 758static 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        snprintf(cap->bus_info, sizeof(cap->bus_info),
 767                 "PCI:%s", pci_name(dev->pci_dev));
 768        return 0;
 769}
 770
 771static int tw686x_set_standard(struct tw686x_video_channel *vc, v4l2_std_id id)
 772{
 773        u32 val;
 774
 775        if (id & V4L2_STD_NTSC)
 776                val = 0;
 777        else if (id & V4L2_STD_PAL)
 778                val = 1;
 779        else if (id & V4L2_STD_SECAM)
 780                val = 2;
 781        else if (id & V4L2_STD_NTSC_443)
 782                val = 3;
 783        else if (id & V4L2_STD_PAL_M)
 784                val = 4;
 785        else if (id & V4L2_STD_PAL_Nc)
 786                val = 5;
 787        else if (id & V4L2_STD_PAL_60)
 788                val = 6;
 789        else
 790                return -EINVAL;
 791
 792        vc->video_standard = id;
 793        reg_write(vc->dev, SDT[vc->ch], val);
 794
 795        val = reg_read(vc->dev, VIDEO_CONTROL1);
 796        if (id & V4L2_STD_525_60)
 797                val &= ~(1 << (SYS_MODE_DMA_SHIFT + vc->ch));
 798        else
 799                val |= (1 << (SYS_MODE_DMA_SHIFT + vc->ch));
 800        reg_write(vc->dev, VIDEO_CONTROL1, val);
 801
 802        return 0;
 803}
 804
 805static int tw686x_s_std(struct file *file, void *priv, v4l2_std_id id)
 806{
 807        struct tw686x_video_channel *vc = video_drvdata(file);
 808        struct v4l2_format f;
 809        int ret;
 810
 811        if (vc->video_standard == id)
 812                return 0;
 813
 814        if (vb2_is_busy(&vc->vidq))
 815                return -EBUSY;
 816
 817        ret = tw686x_set_standard(vc, id);
 818        if (ret)
 819                return ret;
 820        /*
 821         * Adjust format after V4L2_STD_525_60/V4L2_STD_625_50 change,
 822         * calling g_fmt and s_fmt will sanitize the height
 823         * according to the standard.
 824         */
 825        tw686x_g_fmt_vid_cap(file, priv, &f);
 826        tw686x_s_fmt_vid_cap(file, priv, &f);
 827
 828        /*
 829         * Frame decimation depends on the chosen standard,
 830         * so reset it to the current value.
 831         */
 832        tw686x_set_framerate(vc, vc->fps);
 833        return 0;
 834}
 835
 836static int tw686x_querystd(struct file *file, void *priv, v4l2_std_id *std)
 837{
 838        struct tw686x_video_channel *vc = video_drvdata(file);
 839        struct tw686x_dev *dev = vc->dev;
 840        unsigned int old_std, detected_std = 0;
 841        unsigned long end;
 842
 843        if (vb2_is_streaming(&vc->vidq))
 844                return -EBUSY;
 845
 846        /* Enable and start standard detection */
 847        old_std = reg_read(dev, SDT[vc->ch]);
 848        reg_write(dev, SDT[vc->ch], 0x7);
 849        reg_write(dev, SDT_EN[vc->ch], 0xff);
 850
 851        end = jiffies + msecs_to_jiffies(500);
 852        while (time_is_after_jiffies(end)) {
 853
 854                detected_std = reg_read(dev, SDT[vc->ch]);
 855                if (!(detected_std & BIT(7)))
 856                        break;
 857                msleep(100);
 858        }
 859        reg_write(dev, SDT[vc->ch], old_std);
 860
 861        /* Exit if still busy */
 862        if (detected_std & BIT(7))
 863                return 0;
 864
 865        detected_std = (detected_std >> 4) & 0x7;
 866        switch (detected_std) {
 867        case TW686X_STD_NTSC_M:
 868                *std &= V4L2_STD_NTSC;
 869                break;
 870        case TW686X_STD_NTSC_443:
 871                *std &= V4L2_STD_NTSC_443;
 872                break;
 873        case TW686X_STD_PAL_M:
 874                *std &= V4L2_STD_PAL_M;
 875                break;
 876        case TW686X_STD_PAL_60:
 877                *std &= V4L2_STD_PAL_60;
 878                break;
 879        case TW686X_STD_PAL:
 880                *std &= V4L2_STD_PAL;
 881                break;
 882        case TW686X_STD_PAL_CN:
 883                *std &= V4L2_STD_PAL_Nc;
 884                break;
 885        case TW686X_STD_SECAM:
 886                *std &= V4L2_STD_SECAM;
 887                break;
 888        default:
 889                *std = 0;
 890        }
 891        return 0;
 892}
 893
 894static int tw686x_g_std(struct file *file, void *priv, v4l2_std_id *id)
 895{
 896        struct tw686x_video_channel *vc = video_drvdata(file);
 897
 898        *id = vc->video_standard;
 899        return 0;
 900}
 901
 902static int tw686x_enum_framesizes(struct file *file, void *priv,
 903                                  struct v4l2_frmsizeenum *fsize)
 904{
 905        struct tw686x_video_channel *vc = video_drvdata(file);
 906
 907        if (fsize->index)
 908                return -EINVAL;
 909        fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
 910        fsize->stepwise.max_width = TW686X_VIDEO_WIDTH;
 911        fsize->stepwise.min_width = fsize->stepwise.max_width / 2;
 912        fsize->stepwise.step_width = fsize->stepwise.min_width;
 913        fsize->stepwise.max_height = TW686X_VIDEO_HEIGHT(vc->video_standard);
 914        fsize->stepwise.min_height = fsize->stepwise.max_height / 2;
 915        fsize->stepwise.step_height = fsize->stepwise.min_height;
 916        return 0;
 917}
 918
 919static int tw686x_enum_frameintervals(struct file *file, void *priv,
 920                                      struct v4l2_frmivalenum *ival)
 921{
 922        struct tw686x_video_channel *vc = video_drvdata(file);
 923        int max_fps = TW686X_MAX_FPS(vc->video_standard);
 924        int max_rates = DIV_ROUND_UP(max_fps, 2);
 925
 926        if (ival->index >= max_rates)
 927                return -EINVAL;
 928
 929        ival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
 930        ival->discrete.numerator = 1;
 931        if (ival->index < (max_rates - 1))
 932                ival->discrete.denominator = (ival->index + 1) * 2;
 933        else
 934                ival->discrete.denominator = max_fps;
 935        return 0;
 936}
 937
 938static int tw686x_g_parm(struct file *file, void *priv,
 939                         struct v4l2_streamparm *sp)
 940{
 941        struct tw686x_video_channel *vc = video_drvdata(file);
 942        struct v4l2_captureparm *cp = &sp->parm.capture;
 943
 944        if (sp->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 945                return -EINVAL;
 946        sp->parm.capture.readbuffers = 3;
 947
 948        cp->capability = V4L2_CAP_TIMEPERFRAME;
 949        cp->timeperframe.numerator = 1;
 950        cp->timeperframe.denominator = vc->fps;
 951        return 0;
 952}
 953
 954static int tw686x_s_parm(struct file *file, void *priv,
 955                         struct v4l2_streamparm *sp)
 956{
 957        struct tw686x_video_channel *vc = video_drvdata(file);
 958        struct v4l2_captureparm *cp = &sp->parm.capture;
 959        unsigned int denominator = cp->timeperframe.denominator;
 960        unsigned int numerator = cp->timeperframe.numerator;
 961        unsigned int fps;
 962
 963        if (vb2_is_busy(&vc->vidq))
 964                return -EBUSY;
 965
 966        fps = (!numerator || !denominator) ? 0 : denominator / numerator;
 967        if (vc->fps != fps)
 968                tw686x_set_framerate(vc, fps);
 969        return tw686x_g_parm(file, priv, sp);
 970}
 971
 972static int tw686x_enum_fmt_vid_cap(struct file *file, void *priv,
 973                                   struct v4l2_fmtdesc *f)
 974{
 975        if (f->index >= ARRAY_SIZE(formats))
 976                return -EINVAL;
 977        f->pixelformat = formats[f->index].fourcc;
 978        return 0;
 979}
 980
 981static void tw686x_set_input(struct tw686x_video_channel *vc, unsigned int i)
 982{
 983        u32 val;
 984
 985        vc->input = i;
 986
 987        val = reg_read(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch]);
 988        val &= ~(0x3 << 30);
 989        val |= i << 30;
 990        reg_write(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch], val);
 991}
 992
 993static int tw686x_s_input(struct file *file, void *priv, unsigned int i)
 994{
 995        struct tw686x_video_channel *vc = video_drvdata(file);
 996
 997        if (i >= TW686X_INPUTS_PER_CH)
 998                return -EINVAL;
 999        if (i == vc->input)
1000                return 0;
1001        /*
1002         * Not sure we are able to support on the fly input change
1003         */
1004        if (vb2_is_busy(&vc->vidq))
1005                return -EBUSY;
1006
1007        tw686x_set_input(vc, i);
1008        return 0;
1009}
1010
1011static int tw686x_g_input(struct file *file, void *priv, unsigned int *i)
1012{
1013        struct tw686x_video_channel *vc = video_drvdata(file);
1014
1015        *i = vc->input;
1016        return 0;
1017}
1018
1019static int tw686x_enum_input(struct file *file, void *priv,
1020                             struct v4l2_input *i)
1021{
1022        struct tw686x_video_channel *vc = video_drvdata(file);
1023        unsigned int vidstat;
1024
1025        if (i->index >= TW686X_INPUTS_PER_CH)
1026                return -EINVAL;
1027
1028        snprintf(i->name, sizeof(i->name), "Composite%d", i->index);
1029        i->type = V4L2_INPUT_TYPE_CAMERA;
1030        i->std = vc->device->tvnorms;
1031        i->capabilities = V4L2_IN_CAP_STD;
1032
1033        vidstat = reg_read(vc->dev, VIDSTAT[vc->ch]);
1034        i->status = 0;
1035        if (vidstat & TW686X_VIDSTAT_VDLOSS)
1036                i->status |= V4L2_IN_ST_NO_SIGNAL;
1037        if (!(vidstat & TW686X_VIDSTAT_HLOCK))
1038                i->status |= V4L2_IN_ST_NO_H_LOCK;
1039
1040        return 0;
1041}
1042
1043static const struct v4l2_file_operations tw686x_video_fops = {
1044        .owner          = THIS_MODULE,
1045        .open           = v4l2_fh_open,
1046        .unlocked_ioctl = video_ioctl2,
1047        .release        = vb2_fop_release,
1048        .poll           = vb2_fop_poll,
1049        .read           = vb2_fop_read,
1050        .mmap           = vb2_fop_mmap,
1051};
1052
1053static const struct v4l2_ioctl_ops tw686x_video_ioctl_ops = {
1054        .vidioc_querycap                = tw686x_querycap,
1055        .vidioc_g_fmt_vid_cap           = tw686x_g_fmt_vid_cap,
1056        .vidioc_s_fmt_vid_cap           = tw686x_s_fmt_vid_cap,
1057        .vidioc_enum_fmt_vid_cap        = tw686x_enum_fmt_vid_cap,
1058        .vidioc_try_fmt_vid_cap         = tw686x_try_fmt_vid_cap,
1059
1060        .vidioc_querystd                = tw686x_querystd,
1061        .vidioc_g_std                   = tw686x_g_std,
1062        .vidioc_s_std                   = tw686x_s_std,
1063
1064        .vidioc_g_parm                  = tw686x_g_parm,
1065        .vidioc_s_parm                  = tw686x_s_parm,
1066        .vidioc_enum_framesizes         = tw686x_enum_framesizes,
1067        .vidioc_enum_frameintervals     = tw686x_enum_frameintervals,
1068
1069        .vidioc_enum_input              = tw686x_enum_input,
1070        .vidioc_g_input                 = tw686x_g_input,
1071        .vidioc_s_input                 = tw686x_s_input,
1072
1073        .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
1074        .vidioc_querybuf                = vb2_ioctl_querybuf,
1075        .vidioc_qbuf                    = vb2_ioctl_qbuf,
1076        .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
1077        .vidioc_create_bufs             = vb2_ioctl_create_bufs,
1078        .vidioc_streamon                = vb2_ioctl_streamon,
1079        .vidioc_streamoff               = vb2_ioctl_streamoff,
1080        .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
1081
1082        .vidioc_log_status              = v4l2_ctrl_log_status,
1083        .vidioc_subscribe_event         = v4l2_ctrl_subscribe_event,
1084        .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
1085};
1086
1087void tw686x_video_irq(struct tw686x_dev *dev, unsigned long requests,
1088                      unsigned int pb_status, unsigned int fifo_status,
1089                      unsigned int *reset_ch)
1090{
1091        struct tw686x_video_channel *vc;
1092        unsigned long flags;
1093        unsigned int ch, pb;
1094
1095        for_each_set_bit(ch, &requests, max_channels(dev)) {
1096                vc = &dev->video_channels[ch];
1097
1098                /*
1099                 * This can either be a blue frame (with signal-lost bit set)
1100                 * or a good frame (with signal-lost bit clear). If we have just
1101                 * got signal, then this channel needs resetting.
1102                 */
1103                if (vc->no_signal && !(fifo_status & BIT(ch))) {
1104                        v4l2_printk(KERN_DEBUG, &dev->v4l2_dev,
1105                                    "video%d: signal recovered\n", vc->num);
1106                        vc->no_signal = false;
1107                        *reset_ch |= BIT(ch);
1108                        vc->pb = 0;
1109                        continue;
1110                }
1111                vc->no_signal = !!(fifo_status & BIT(ch));
1112
1113                /* Check FIFO errors only if there's signal */
1114                if (!vc->no_signal) {
1115                        u32 fifo_ov, fifo_bad;
1116
1117                        fifo_ov = (fifo_status >> 24) & BIT(ch);
1118                        fifo_bad = (fifo_status >> 16) & BIT(ch);
1119                        if (fifo_ov || fifo_bad) {
1120                                /* Mark this channel for reset */
1121                                v4l2_printk(KERN_DEBUG, &dev->v4l2_dev,
1122                                            "video%d: FIFO error\n", vc->num);
1123                                *reset_ch |= BIT(ch);
1124                                vc->pb = 0;
1125                                continue;
1126                        }
1127                }
1128
1129                pb = !!(pb_status & BIT(ch));
1130                if (vc->pb != pb) {
1131                        /* Mark this channel for reset */
1132                        v4l2_printk(KERN_DEBUG, &dev->v4l2_dev,
1133                                    "video%d: unexpected p-b buffer!\n",
1134                                    vc->num);
1135                        *reset_ch |= BIT(ch);
1136                        vc->pb = 0;
1137                        continue;
1138                }
1139
1140                spin_lock_irqsave(&vc->qlock, flags);
1141                tw686x_buf_done(vc, pb);
1142                dev->dma_ops->buf_refill(vc, pb);
1143                spin_unlock_irqrestore(&vc->qlock, flags);
1144        }
1145}
1146
1147void tw686x_video_free(struct tw686x_dev *dev)
1148{
1149        unsigned int ch, pb;
1150
1151        for (ch = 0; ch < max_channels(dev); ch++) {
1152                struct tw686x_video_channel *vc = &dev->video_channels[ch];
1153
1154                video_unregister_device(vc->device);
1155
1156                if (dev->dma_ops->free)
1157                        for (pb = 0; pb < 2; pb++)
1158                                dev->dma_ops->free(vc, pb);
1159        }
1160}
1161
1162int tw686x_video_init(struct tw686x_dev *dev)
1163{
1164        unsigned int ch, val;
1165        int err;
1166
1167        if (dev->dma_mode == TW686X_DMA_MODE_MEMCPY)
1168                dev->dma_ops = &memcpy_dma_ops;
1169        else if (dev->dma_mode == TW686X_DMA_MODE_CONTIG)
1170                dev->dma_ops = &contig_dma_ops;
1171        else if (dev->dma_mode == TW686X_DMA_MODE_SG)
1172                dev->dma_ops = &sg_dma_ops;
1173        else
1174                return -EINVAL;
1175
1176        err = v4l2_device_register(&dev->pci_dev->dev, &dev->v4l2_dev);
1177        if (err)
1178                return err;
1179
1180        if (dev->dma_ops->setup) {
1181                err = dev->dma_ops->setup(dev);
1182                if (err)
1183                        return err;
1184        }
1185
1186        /* Initialize vc->dev and vc->ch for the error path */
1187        for (ch = 0; ch < max_channels(dev); ch++) {
1188                struct tw686x_video_channel *vc = &dev->video_channels[ch];
1189
1190                vc->dev = dev;
1191                vc->ch = ch;
1192        }
1193
1194        for (ch = 0; ch < max_channels(dev); ch++) {
1195                struct tw686x_video_channel *vc = &dev->video_channels[ch];
1196                struct video_device *vdev;
1197
1198                mutex_init(&vc->vb_mutex);
1199                spin_lock_init(&vc->qlock);
1200                INIT_LIST_HEAD(&vc->vidq_queued);
1201
1202                /* default settings */
1203                err = tw686x_set_standard(vc, V4L2_STD_NTSC);
1204                if (err)
1205                        goto error;
1206
1207                err = tw686x_set_format(vc, formats[0].fourcc,
1208                                TW686X_VIDEO_WIDTH,
1209                                TW686X_VIDEO_HEIGHT(vc->video_standard),
1210                                true);
1211                if (err)
1212                        goto error;
1213
1214                tw686x_set_input(vc, 0);
1215                tw686x_set_framerate(vc, 30);
1216                reg_write(dev, VDELAY_LO[ch], 0x14);
1217                reg_write(dev, HACTIVE_LO[ch], 0xd0);
1218                reg_write(dev, VIDEO_SIZE[ch], 0);
1219
1220                vc->vidq.io_modes = VB2_READ | VB2_MMAP | VB2_DMABUF;
1221                vc->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1222                vc->vidq.drv_priv = vc;
1223                vc->vidq.buf_struct_size = sizeof(struct tw686x_v4l2_buf);
1224                vc->vidq.ops = &tw686x_video_qops;
1225                vc->vidq.mem_ops = dev->dma_ops->mem_ops;
1226                vc->vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1227                vc->vidq.min_buffers_needed = 2;
1228                vc->vidq.lock = &vc->vb_mutex;
1229                vc->vidq.gfp_flags = dev->dma_mode != TW686X_DMA_MODE_MEMCPY ?
1230                                     GFP_DMA32 : 0;
1231                vc->vidq.dev = &dev->pci_dev->dev;
1232
1233                err = vb2_queue_init(&vc->vidq);
1234                if (err) {
1235                        v4l2_err(&dev->v4l2_dev,
1236                                 "dma%d: cannot init vb2 queue\n", ch);
1237                        goto error;
1238                }
1239
1240                err = v4l2_ctrl_handler_init(&vc->ctrl_handler, 4);
1241                if (err) {
1242                        v4l2_err(&dev->v4l2_dev,
1243                                 "dma%d: cannot init ctrl handler\n", ch);
1244                        goto error;
1245                }
1246                v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
1247                                  V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
1248                v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
1249                                  V4L2_CID_CONTRAST, 0, 255, 1, 100);
1250                v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
1251                                  V4L2_CID_SATURATION, 0, 255, 1, 128);
1252                v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
1253                                  V4L2_CID_HUE, -128, 127, 1, 0);
1254                err = vc->ctrl_handler.error;
1255                if (err)
1256                        goto error;
1257
1258                err = v4l2_ctrl_handler_setup(&vc->ctrl_handler);
1259                if (err)
1260                        goto error;
1261
1262                vdev = video_device_alloc();
1263                if (!vdev) {
1264                        v4l2_err(&dev->v4l2_dev,
1265                                 "dma%d: unable to allocate device\n", ch);
1266                        err = -ENOMEM;
1267                        goto error;
1268                }
1269
1270                snprintf(vdev->name, sizeof(vdev->name), "%s video", dev->name);
1271                vdev->fops = &tw686x_video_fops;
1272                vdev->ioctl_ops = &tw686x_video_ioctl_ops;
1273                vdev->release = video_device_release;
1274                vdev->v4l2_dev = &dev->v4l2_dev;
1275                vdev->queue = &vc->vidq;
1276                vdev->tvnorms = V4L2_STD_525_60 | V4L2_STD_625_50;
1277                vdev->minor = -1;
1278                vdev->lock = &vc->vb_mutex;
1279                vdev->ctrl_handler = &vc->ctrl_handler;
1280                vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE |
1281                                    V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
1282                vc->device = vdev;
1283                video_set_drvdata(vdev, vc);
1284
1285                err = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
1286                if (err < 0)
1287                        goto error;
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
1306error:
1307        tw686x_video_free(dev);
1308        return err;
1309}
1310