treewide: Replace GPLv2 boilerplate/reference with SPDX - rule 206
[muen/linux.git] / 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                 pci_free_consistent(dev->pci_dev, desc->size,
96                                     desc->virt, 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 = 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
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                 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
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
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
299 static 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
330 static 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
340 static 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
364 static 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
375 static 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
402 static 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
412 static 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
422 static 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
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         snprintf(cap->bus_info, sizeof(cap->bus_info),
767                  "PCI:%s", pci_name(dev->pci_dev));
768         cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
769                            V4L2_CAP_READWRITE;
770         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
771         return 0;
772 }
773
774 static int tw686x_set_standard(struct tw686x_video_channel *vc, v4l2_std_id id)
775 {
776         u32 val;
777
778         if (id & V4L2_STD_NTSC)
779                 val = 0;
780         else if (id & V4L2_STD_PAL)
781                 val = 1;
782         else if (id & V4L2_STD_SECAM)
783                 val = 2;
784         else if (id & V4L2_STD_NTSC_443)
785                 val = 3;
786         else if (id & V4L2_STD_PAL_M)
787                 val = 4;
788         else if (id & V4L2_STD_PAL_Nc)
789                 val = 5;
790         else if (id & V4L2_STD_PAL_60)
791                 val = 6;
792         else
793                 return -EINVAL;
794
795         vc->video_standard = id;
796         reg_write(vc->dev, SDT[vc->ch], val);
797
798         val = reg_read(vc->dev, VIDEO_CONTROL1);
799         if (id & V4L2_STD_525_60)
800                 val &= ~(1 << (SYS_MODE_DMA_SHIFT + vc->ch));
801         else
802                 val |= (1 << (SYS_MODE_DMA_SHIFT + vc->ch));
803         reg_write(vc->dev, VIDEO_CONTROL1, val);
804
805         return 0;
806 }
807
808 static int tw686x_s_std(struct file *file, void *priv, v4l2_std_id id)
809 {
810         struct tw686x_video_channel *vc = video_drvdata(file);
811         struct v4l2_format f;
812         int ret;
813
814         if (vc->video_standard == id)
815                 return 0;
816
817         if (vb2_is_busy(&vc->vidq))
818                 return -EBUSY;
819
820         ret = tw686x_set_standard(vc, id);
821         if (ret)
822                 return ret;
823         /*
824          * Adjust format after V4L2_STD_525_60/V4L2_STD_625_50 change,
825          * calling g_fmt and s_fmt will sanitize the height
826          * according to the standard.
827          */
828         tw686x_g_fmt_vid_cap(file, priv, &f);
829         tw686x_s_fmt_vid_cap(file, priv, &f);
830
831         /*
832          * Frame decimation depends on the chosen standard,
833          * so reset it to the current value.
834          */
835         tw686x_set_framerate(vc, vc->fps);
836         return 0;
837 }
838
839 static int tw686x_querystd(struct file *file, void *priv, v4l2_std_id *std)
840 {
841         struct tw686x_video_channel *vc = video_drvdata(file);
842         struct tw686x_dev *dev = vc->dev;
843         unsigned int old_std, detected_std = 0;
844         unsigned long end;
845
846         if (vb2_is_streaming(&vc->vidq))
847                 return -EBUSY;
848
849         /* Enable and start standard detection */
850         old_std = reg_read(dev, SDT[vc->ch]);
851         reg_write(dev, SDT[vc->ch], 0x7);
852         reg_write(dev, SDT_EN[vc->ch], 0xff);
853
854         end = jiffies + msecs_to_jiffies(500);
855         while (time_is_after_jiffies(end)) {
856
857                 detected_std = reg_read(dev, SDT[vc->ch]);
858                 if (!(detected_std & BIT(7)))
859                         break;
860                 msleep(100);
861         }
862         reg_write(dev, SDT[vc->ch], old_std);
863
864         /* Exit if still busy */
865         if (detected_std & BIT(7))
866                 return 0;
867
868         detected_std = (detected_std >> 4) & 0x7;
869         switch (detected_std) {
870         case TW686X_STD_NTSC_M:
871                 *std &= V4L2_STD_NTSC;
872                 break;
873         case TW686X_STD_NTSC_443:
874                 *std &= V4L2_STD_NTSC_443;
875                 break;
876         case TW686X_STD_PAL_M:
877                 *std &= V4L2_STD_PAL_M;
878                 break;
879         case TW686X_STD_PAL_60:
880                 *std &= V4L2_STD_PAL_60;
881                 break;
882         case TW686X_STD_PAL:
883                 *std &= V4L2_STD_PAL;
884                 break;
885         case TW686X_STD_PAL_CN:
886                 *std &= V4L2_STD_PAL_Nc;
887                 break;
888         case TW686X_STD_SECAM:
889                 *std &= V4L2_STD_SECAM;
890                 break;
891         default:
892                 *std = 0;
893         }
894         return 0;
895 }
896
897 static int tw686x_g_std(struct file *file, void *priv, v4l2_std_id *id)
898 {
899         struct tw686x_video_channel *vc = video_drvdata(file);
900
901         *id = vc->video_standard;
902         return 0;
903 }
904
905 static int tw686x_enum_framesizes(struct file *file, void *priv,
906                                   struct v4l2_frmsizeenum *fsize)
907 {
908         struct tw686x_video_channel *vc = video_drvdata(file);
909
910         if (fsize->index)
911                 return -EINVAL;
912         fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
913         fsize->stepwise.max_width = TW686X_VIDEO_WIDTH;
914         fsize->stepwise.min_width = fsize->stepwise.max_width / 2;
915         fsize->stepwise.step_width = fsize->stepwise.min_width;
916         fsize->stepwise.max_height = TW686X_VIDEO_HEIGHT(vc->video_standard);
917         fsize->stepwise.min_height = fsize->stepwise.max_height / 2;
918         fsize->stepwise.step_height = fsize->stepwise.min_height;
919         return 0;
920 }
921
922 static int tw686x_enum_frameintervals(struct file *file, void *priv,
923                                       struct v4l2_frmivalenum *ival)
924 {
925         struct tw686x_video_channel *vc = video_drvdata(file);
926         int max_fps = TW686X_MAX_FPS(vc->video_standard);
927         int max_rates = DIV_ROUND_UP(max_fps, 2);
928
929         if (ival->index >= max_rates)
930                 return -EINVAL;
931
932         ival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
933         ival->discrete.numerator = 1;
934         if (ival->index < (max_rates - 1))
935                 ival->discrete.denominator = (ival->index + 1) * 2;
936         else
937                 ival->discrete.denominator = max_fps;
938         return 0;
939 }
940
941 static int tw686x_g_parm(struct file *file, void *priv,
942                          struct v4l2_streamparm *sp)
943 {
944         struct tw686x_video_channel *vc = video_drvdata(file);
945         struct v4l2_captureparm *cp = &sp->parm.capture;
946
947         if (sp->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
948                 return -EINVAL;
949         sp->parm.capture.readbuffers = 3;
950
951         cp->capability = V4L2_CAP_TIMEPERFRAME;
952         cp->timeperframe.numerator = 1;
953         cp->timeperframe.denominator = vc->fps;
954         return 0;
955 }
956
957 static int tw686x_s_parm(struct file *file, void *priv,
958                          struct v4l2_streamparm *sp)
959 {
960         struct tw686x_video_channel *vc = video_drvdata(file);
961         struct v4l2_captureparm *cp = &sp->parm.capture;
962         unsigned int denominator = cp->timeperframe.denominator;
963         unsigned int numerator = cp->timeperframe.numerator;
964         unsigned int fps;
965
966         if (vb2_is_busy(&vc->vidq))
967                 return -EBUSY;
968
969         fps = (!numerator || !denominator) ? 0 : denominator / numerator;
970         if (vc->fps != fps)
971                 tw686x_set_framerate(vc, fps);
972         return tw686x_g_parm(file, priv, sp);
973 }
974
975 static int tw686x_enum_fmt_vid_cap(struct file *file, void *priv,
976                                    struct v4l2_fmtdesc *f)
977 {
978         if (f->index >= ARRAY_SIZE(formats))
979                 return -EINVAL;
980         f->pixelformat = formats[f->index].fourcc;
981         return 0;
982 }
983
984 static void tw686x_set_input(struct tw686x_video_channel *vc, unsigned int i)
985 {
986         u32 val;
987
988         vc->input = i;
989
990         val = reg_read(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch]);
991         val &= ~(0x3 << 30);
992         val |= i << 30;
993         reg_write(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch], val);
994 }
995
996 static int tw686x_s_input(struct file *file, void *priv, unsigned int i)
997 {
998         struct tw686x_video_channel *vc = video_drvdata(file);
999
1000         if (i >= TW686X_INPUTS_PER_CH)
1001                 return -EINVAL;
1002         if (i == vc->input)
1003                 return 0;
1004         /*
1005          * Not sure we are able to support on the fly input change
1006          */
1007         if (vb2_is_busy(&vc->vidq))
1008                 return -EBUSY;
1009
1010         tw686x_set_input(vc, i);
1011         return 0;
1012 }
1013
1014 static int tw686x_g_input(struct file *file, void *priv, unsigned int *i)
1015 {
1016         struct tw686x_video_channel *vc = video_drvdata(file);
1017
1018         *i = vc->input;
1019         return 0;
1020 }
1021
1022 static int tw686x_enum_input(struct file *file, void *priv,
1023                              struct v4l2_input *i)
1024 {
1025         struct tw686x_video_channel *vc = video_drvdata(file);
1026         unsigned int vidstat;
1027
1028         if (i->index >= TW686X_INPUTS_PER_CH)
1029                 return -EINVAL;
1030
1031         snprintf(i->name, sizeof(i->name), "Composite%d", i->index);
1032         i->type = V4L2_INPUT_TYPE_CAMERA;
1033         i->std = vc->device->tvnorms;
1034         i->capabilities = V4L2_IN_CAP_STD;
1035
1036         vidstat = reg_read(vc->dev, VIDSTAT[vc->ch]);
1037         i->status = 0;
1038         if (vidstat & TW686X_VIDSTAT_VDLOSS)
1039                 i->status |= V4L2_IN_ST_NO_SIGNAL;
1040         if (!(vidstat & TW686X_VIDSTAT_HLOCK))
1041                 i->status |= V4L2_IN_ST_NO_H_LOCK;
1042
1043         return 0;
1044 }
1045
1046 static const struct v4l2_file_operations tw686x_video_fops = {
1047         .owner          = THIS_MODULE,
1048         .open           = v4l2_fh_open,
1049         .unlocked_ioctl = video_ioctl2,
1050         .release        = vb2_fop_release,
1051         .poll           = vb2_fop_poll,
1052         .read           = vb2_fop_read,
1053         .mmap           = vb2_fop_mmap,
1054 };
1055
1056 static const struct v4l2_ioctl_ops tw686x_video_ioctl_ops = {
1057         .vidioc_querycap                = tw686x_querycap,
1058         .vidioc_g_fmt_vid_cap           = tw686x_g_fmt_vid_cap,
1059         .vidioc_s_fmt_vid_cap           = tw686x_s_fmt_vid_cap,
1060         .vidioc_enum_fmt_vid_cap        = tw686x_enum_fmt_vid_cap,
1061         .vidioc_try_fmt_vid_cap         = tw686x_try_fmt_vid_cap,
1062
1063         .vidioc_querystd                = tw686x_querystd,
1064         .vidioc_g_std                   = tw686x_g_std,
1065         .vidioc_s_std                   = tw686x_s_std,
1066
1067         .vidioc_g_parm                  = tw686x_g_parm,
1068         .vidioc_s_parm                  = tw686x_s_parm,
1069         .vidioc_enum_framesizes         = tw686x_enum_framesizes,
1070         .vidioc_enum_frameintervals     = tw686x_enum_frameintervals,
1071
1072         .vidioc_enum_input              = tw686x_enum_input,
1073         .vidioc_g_input                 = tw686x_g_input,
1074         .vidioc_s_input                 = tw686x_s_input,
1075
1076         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
1077         .vidioc_querybuf                = vb2_ioctl_querybuf,
1078         .vidioc_qbuf                    = vb2_ioctl_qbuf,
1079         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
1080         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
1081         .vidioc_streamon                = vb2_ioctl_streamon,
1082         .vidioc_streamoff               = vb2_ioctl_streamoff,
1083         .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
1084
1085         .vidioc_log_status              = v4l2_ctrl_log_status,
1086         .vidioc_subscribe_event         = v4l2_ctrl_subscribe_event,
1087         .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
1088 };
1089
1090 void tw686x_video_irq(struct tw686x_dev *dev, unsigned long requests,
1091                       unsigned int pb_status, unsigned int fifo_status,
1092                       unsigned int *reset_ch)
1093 {
1094         struct tw686x_video_channel *vc;
1095         unsigned long flags;
1096         unsigned int ch, pb;
1097
1098         for_each_set_bit(ch, &requests, max_channels(dev)) {
1099                 vc = &dev->video_channels[ch];
1100
1101                 /*
1102                  * This can either be a blue frame (with signal-lost bit set)
1103                  * or a good frame (with signal-lost bit clear). If we have just
1104                  * got signal, then this channel needs resetting.
1105                  */
1106                 if (vc->no_signal && !(fifo_status & BIT(ch))) {
1107                         v4l2_printk(KERN_DEBUG, &dev->v4l2_dev,
1108                                     "video%d: signal recovered\n", vc->num);
1109                         vc->no_signal = false;
1110                         *reset_ch |= BIT(ch);
1111                         vc->pb = 0;
1112                         continue;
1113                 }
1114                 vc->no_signal = !!(fifo_status & BIT(ch));
1115
1116                 /* Check FIFO errors only if there's signal */
1117                 if (!vc->no_signal) {
1118                         u32 fifo_ov, fifo_bad;
1119
1120                         fifo_ov = (fifo_status >> 24) & BIT(ch);
1121                         fifo_bad = (fifo_status >> 16) & BIT(ch);
1122                         if (fifo_ov || fifo_bad) {
1123                                 /* Mark this channel for reset */
1124                                 v4l2_printk(KERN_DEBUG, &dev->v4l2_dev,
1125                                             "video%d: FIFO error\n", vc->num);
1126                                 *reset_ch |= BIT(ch);
1127                                 vc->pb = 0;
1128                                 continue;
1129                         }
1130                 }
1131
1132                 pb = !!(pb_status & BIT(ch));
1133                 if (vc->pb != pb) {
1134                         /* Mark this channel for reset */
1135                         v4l2_printk(KERN_DEBUG, &dev->v4l2_dev,
1136                                     "video%d: unexpected p-b buffer!\n",
1137                                     vc->num);
1138                         *reset_ch |= BIT(ch);
1139                         vc->pb = 0;
1140                         continue;
1141                 }
1142
1143                 spin_lock_irqsave(&vc->qlock, flags);
1144                 tw686x_buf_done(vc, pb);
1145                 dev->dma_ops->buf_refill(vc, pb);
1146                 spin_unlock_irqrestore(&vc->qlock, flags);
1147         }
1148 }
1149
1150 void tw686x_video_free(struct tw686x_dev *dev)
1151 {
1152         unsigned int ch, pb;
1153
1154         for (ch = 0; ch < max_channels(dev); ch++) {
1155                 struct tw686x_video_channel *vc = &dev->video_channels[ch];
1156
1157                 video_unregister_device(vc->device);
1158
1159                 if (dev->dma_ops->free)
1160                         for (pb = 0; pb < 2; pb++)
1161                                 dev->dma_ops->free(vc, pb);
1162         }
1163 }
1164
1165 int tw686x_video_init(struct tw686x_dev *dev)
1166 {
1167         unsigned int ch, val;
1168         int err;
1169
1170         if (dev->dma_mode == TW686X_DMA_MODE_MEMCPY)
1171                 dev->dma_ops = &memcpy_dma_ops;
1172         else if (dev->dma_mode == TW686X_DMA_MODE_CONTIG)
1173                 dev->dma_ops = &contig_dma_ops;
1174         else if (dev->dma_mode == TW686X_DMA_MODE_SG)
1175                 dev->dma_ops = &sg_dma_ops;
1176         else
1177                 return -EINVAL;
1178
1179         err = v4l2_device_register(&dev->pci_dev->dev, &dev->v4l2_dev);
1180         if (err)
1181                 return err;
1182
1183         if (dev->dma_ops->setup) {
1184                 err = dev->dma_ops->setup(dev);
1185                 if (err)
1186                         return err;
1187         }
1188
1189         /* Initialize vc->dev and vc->ch for the error path */
1190         for (ch = 0; ch < max_channels(dev); ch++) {
1191                 struct tw686x_video_channel *vc = &dev->video_channels[ch];
1192
1193                 vc->dev = dev;
1194                 vc->ch = ch;
1195         }
1196
1197         for (ch = 0; ch < max_channels(dev); ch++) {
1198                 struct tw686x_video_channel *vc = &dev->video_channels[ch];
1199                 struct video_device *vdev;
1200
1201                 mutex_init(&vc->vb_mutex);
1202                 spin_lock_init(&vc->qlock);
1203                 INIT_LIST_HEAD(&vc->vidq_queued);
1204
1205                 /* default settings */
1206                 err = tw686x_set_standard(vc, V4L2_STD_NTSC);
1207                 if (err)
1208                         goto error;
1209
1210                 err = tw686x_set_format(vc, formats[0].fourcc,
1211                                 TW686X_VIDEO_WIDTH,
1212                                 TW686X_VIDEO_HEIGHT(vc->video_standard),
1213                                 true);
1214                 if (err)
1215                         goto error;
1216
1217                 tw686x_set_input(vc, 0);
1218                 tw686x_set_framerate(vc, 30);
1219                 reg_write(dev, VDELAY_LO[ch], 0x14);
1220                 reg_write(dev, HACTIVE_LO[ch], 0xd0);
1221                 reg_write(dev, VIDEO_SIZE[ch], 0);
1222
1223                 vc->vidq.io_modes = VB2_READ | VB2_MMAP | VB2_DMABUF;
1224                 vc->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1225                 vc->vidq.drv_priv = vc;
1226                 vc->vidq.buf_struct_size = sizeof(struct tw686x_v4l2_buf);
1227                 vc->vidq.ops = &tw686x_video_qops;
1228                 vc->vidq.mem_ops = dev->dma_ops->mem_ops;
1229                 vc->vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1230                 vc->vidq.min_buffers_needed = 2;
1231                 vc->vidq.lock = &vc->vb_mutex;
1232                 vc->vidq.gfp_flags = dev->dma_mode != TW686X_DMA_MODE_MEMCPY ?
1233                                      GFP_DMA32 : 0;
1234                 vc->vidq.dev = &dev->pci_dev->dev;
1235
1236                 err = vb2_queue_init(&vc->vidq);
1237                 if (err) {
1238                         v4l2_err(&dev->v4l2_dev,
1239                                  "dma%d: cannot init vb2 queue\n", ch);
1240                         goto error;
1241                 }
1242
1243                 err = v4l2_ctrl_handler_init(&vc->ctrl_handler, 4);
1244                 if (err) {
1245                         v4l2_err(&dev->v4l2_dev,
1246                                  "dma%d: cannot init ctrl handler\n", ch);
1247                         goto error;
1248                 }
1249                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
1250                                   V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
1251                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
1252                                   V4L2_CID_CONTRAST, 0, 255, 1, 100);
1253                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
1254                                   V4L2_CID_SATURATION, 0, 255, 1, 128);
1255                 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,
1256                                   V4L2_CID_HUE, -128, 127, 1, 0);
1257                 err = vc->ctrl_handler.error;
1258                 if (err)
1259                         goto error;
1260
1261                 err = v4l2_ctrl_handler_setup(&vc->ctrl_handler);
1262                 if (err)
1263                         goto error;
1264
1265                 vdev = video_device_alloc();
1266                 if (!vdev) {
1267                         v4l2_err(&dev->v4l2_dev,
1268                                  "dma%d: unable to allocate device\n", ch);
1269                         err = -ENOMEM;
1270                         goto error;
1271                 }
1272
1273                 snprintf(vdev->name, sizeof(vdev->name), "%s video", dev->name);
1274                 vdev->fops = &tw686x_video_fops;
1275                 vdev->ioctl_ops = &tw686x_video_ioctl_ops;
1276                 vdev->release = video_device_release;
1277                 vdev->v4l2_dev = &dev->v4l2_dev;
1278                 vdev->queue = &vc->vidq;
1279                 vdev->tvnorms = V4L2_STD_525_60 | V4L2_STD_625_50;
1280                 vdev->minor = -1;
1281                 vdev->lock = &vc->vb_mutex;
1282                 vdev->ctrl_handler = &vc->ctrl_handler;
1283                 vc->device = vdev;
1284                 video_set_drvdata(vdev, vc);
1285
1286                 err = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
1287                 if (err < 0)
1288                         goto error;
1289                 vc->num = vdev->num;
1290         }
1291
1292         val = TW686X_DEF_PHASE_REF;
1293         for (ch = 0; ch < max_channels(dev); ch++)
1294                 val |= dev->dma_ops->hw_dma_mode << (16 + ch * 2);
1295         reg_write(dev, PHASE_REF, val);
1296
1297         reg_write(dev, MISC2[0], 0xe7);
1298         reg_write(dev, VCTRL1[0], 0xcc);
1299         reg_write(dev, LOOP[0], 0xa5);
1300         if (max_channels(dev) > 4) {
1301                 reg_write(dev, VCTRL1[1], 0xcc);
1302                 reg_write(dev, LOOP[1], 0xa5);
1303                 reg_write(dev, MISC2[1], 0xe7);
1304         }
1305         return 0;
1306
1307 error:
1308         tw686x_video_free(dev);
1309         return err;
1310 }