2 * Copyright (C) 2015 VanguardiaSur - www.vanguardiasur.com.ar
4 * Based on original driver by Krzysztof Ha?asa:
5 * Copyright (C) 2015 Industrial Research Institute for Automation
6 * and Measurements PIAP
8 * This program is free software; you can redistribute it and/or modify it
9 * under the terms of version 2 of the GNU General Public License
10 * as published by the Free Software Foundation.
14 #include <linux/init.h>
15 #include <linux/delay.h>
16 #include <linux/list.h>
17 #include <linux/module.h>
18 #include <linux/kernel.h>
19 #include <linux/slab.h>
20 #include <media/v4l2-common.h>
21 #include <media/v4l2-event.h>
22 #include <media/videobuf2-dma-contig.h>
23 #include <media/videobuf2-dma-sg.h>
24 #include <media/videobuf2-vmalloc.h>
26 #include "tw686x-regs.h"
28 #define TW686X_INPUTS_PER_CH 4
29 #define TW686X_VIDEO_WIDTH 720
30 #define TW686X_VIDEO_HEIGHT(id) ((id & V4L2_STD_525_60) ? 480 : 576)
31 #define TW686X_MAX_FPS(id) ((id & V4L2_STD_525_60) ? 30 : 25)
33 #define TW686X_MAX_SG_ENTRY_SIZE 4096
34 #define TW686X_MAX_SG_DESC_COUNT 256 /* PAL 720x576 needs 203 4-KB pages */
35 #define TW686X_SG_TABLE_SIZE (TW686X_MAX_SG_DESC_COUNT * sizeof(struct tw686x_sg_desc))
37 static const struct tw686x_format formats
[] = {
39 .fourcc
= V4L2_PIX_FMT_UYVY
,
43 .fourcc
= V4L2_PIX_FMT_RGB565
,
47 .fourcc
= V4L2_PIX_FMT_YUYV
,
53 static void tw686x_buf_done(struct tw686x_video_channel
*vc
,
56 struct tw686x_dma_desc
*desc
= &vc
->dma_descs
[pb
];
57 struct tw686x_dev
*dev
= vc
->dev
;
58 struct vb2_v4l2_buffer
*vb
;
59 struct vb2_buffer
*vb2_buf
;
61 if (vc
->curr_bufs
[pb
]) {
62 vb
= &vc
->curr_bufs
[pb
]->vb
;
64 vb
->field
= dev
->dma_ops
->field
;
65 vb
->sequence
= vc
->sequence
++;
66 vb2_buf
= &vb
->vb2_buf
;
68 if (dev
->dma_mode
== TW686X_DMA_MODE_MEMCPY
)
69 memcpy(vb2_plane_vaddr(vb2_buf
, 0), desc
->virt
,
71 vb2_buf
->timestamp
= ktime_get_ns();
72 vb2_buffer_done(vb2_buf
, VB2_BUF_STATE_DONE
);
79 * We can call this even when alloc_dma failed for the given channel
81 static void tw686x_memcpy_dma_free(struct tw686x_video_channel
*vc
,
84 struct tw686x_dma_desc
*desc
= &vc
->dma_descs
[pb
];
85 struct tw686x_dev
*dev
= vc
->dev
;
86 struct pci_dev
*pci_dev
;
89 /* Check device presence. Shouldn't really happen! */
90 spin_lock_irqsave(&dev
->lock
, flags
);
91 pci_dev
= dev
->pci_dev
;
92 spin_unlock_irqrestore(&dev
->lock
, flags
);
94 WARN(1, "trying to deallocate on missing device\n");
99 pci_free_consistent(dev
->pci_dev
, desc
->size
,
100 desc
->virt
, desc
->phys
);
105 static int tw686x_memcpy_dma_alloc(struct tw686x_video_channel
*vc
,
108 struct tw686x_dev
*dev
= vc
->dev
;
109 u32 reg
= pb
? VDMA_B_ADDR
[vc
->ch
] : VDMA_P_ADDR
[vc
->ch
];
113 WARN(vc
->dma_descs
[pb
].virt
,
114 "Allocating buffer but previous still here\n");
116 len
= (vc
->width
* vc
->height
* vc
->format
->depth
) >> 3;
117 virt
= pci_alloc_consistent(dev
->pci_dev
, len
,
118 &vc
->dma_descs
[pb
].phys
);
120 v4l2_err(&dev
->v4l2_dev
,
121 "dma%d: unable to allocate %s-buffer\n",
122 vc
->ch
, pb
? "B" : "P");
125 vc
->dma_descs
[pb
].size
= len
;
126 vc
->dma_descs
[pb
].virt
= virt
;
127 reg_write(dev
, reg
, vc
->dma_descs
[pb
].phys
);
132 static void tw686x_memcpy_buf_refill(struct tw686x_video_channel
*vc
,
135 struct tw686x_v4l2_buf
*buf
;
137 while (!list_empty(&vc
->vidq_queued
)) {
139 buf
= list_first_entry(&vc
->vidq_queued
,
140 struct tw686x_v4l2_buf
, list
);
141 list_del(&buf
->list
);
143 vc
->curr_bufs
[pb
] = buf
;
146 vc
->curr_bufs
[pb
] = NULL
;
149 static const struct tw686x_dma_ops memcpy_dma_ops
= {
150 .alloc
= tw686x_memcpy_dma_alloc
,
151 .free
= tw686x_memcpy_dma_free
,
152 .buf_refill
= tw686x_memcpy_buf_refill
,
153 .mem_ops
= &vb2_vmalloc_memops
,
154 .hw_dma_mode
= TW686X_FRAME_MODE
,
155 .field
= V4L2_FIELD_INTERLACED
,
158 static void tw686x_contig_buf_refill(struct tw686x_video_channel
*vc
,
161 struct tw686x_v4l2_buf
*buf
;
163 while (!list_empty(&vc
->vidq_queued
)) {
164 u32 reg
= pb
? VDMA_B_ADDR
[vc
->ch
] : VDMA_P_ADDR
[vc
->ch
];
167 buf
= list_first_entry(&vc
->vidq_queued
,
168 struct tw686x_v4l2_buf
, list
);
169 list_del(&buf
->list
);
171 phys
= vb2_dma_contig_plane_dma_addr(&buf
->vb
.vb2_buf
, 0);
172 reg_write(vc
->dev
, reg
, phys
);
174 buf
->vb
.vb2_buf
.state
= VB2_BUF_STATE_ACTIVE
;
175 vc
->curr_bufs
[pb
] = buf
;
178 vc
->curr_bufs
[pb
] = NULL
;
181 static const struct tw686x_dma_ops contig_dma_ops
= {
182 .buf_refill
= tw686x_contig_buf_refill
,
183 .mem_ops
= &vb2_dma_contig_memops
,
184 .hw_dma_mode
= TW686X_FRAME_MODE
,
185 .field
= V4L2_FIELD_INTERLACED
,
188 static int tw686x_sg_desc_fill(struct tw686x_sg_desc
*descs
,
189 struct tw686x_v4l2_buf
*buf
,
190 unsigned int buf_len
)
192 struct sg_table
*vbuf
= vb2_dma_sg_plane_desc(&buf
->vb
.vb2_buf
, 0);
193 unsigned int len
, entry_len
;
194 struct scatterlist
*sg
;
197 /* Clear the scatter-gather table */
198 memset(descs
, 0, TW686X_SG_TABLE_SIZE
);
201 for_each_sg(vbuf
->sgl
, sg
, vbuf
->nents
, i
) {
202 dma_addr_t phys
= sg_dma_address(sg
);
203 len
= sg_dma_len(sg
);
205 while (len
&& buf_len
) {
207 if (count
== TW686X_MAX_SG_DESC_COUNT
)
210 entry_len
= min_t(unsigned int, len
,
211 TW686X_MAX_SG_ENTRY_SIZE
);
212 entry_len
= min_t(unsigned int, entry_len
, buf_len
);
213 descs
[count
].phys
= cpu_to_le32(phys
);
214 descs
[count
++].flags_length
=
215 cpu_to_le32(BIT(30) | entry_len
);
218 buf_len
-= entry_len
;
228 static void tw686x_sg_buf_refill(struct tw686x_video_channel
*vc
,
231 struct tw686x_dev
*dev
= vc
->dev
;
232 struct tw686x_v4l2_buf
*buf
;
234 while (!list_empty(&vc
->vidq_queued
)) {
235 unsigned int buf_len
;
237 buf
= list_first_entry(&vc
->vidq_queued
,
238 struct tw686x_v4l2_buf
, list
);
239 list_del(&buf
->list
);
241 buf_len
= (vc
->width
* vc
->height
* vc
->format
->depth
) >> 3;
242 if (tw686x_sg_desc_fill(vc
->sg_descs
[pb
], buf
, buf_len
)) {
243 v4l2_err(&dev
->v4l2_dev
,
244 "dma%d: unable to fill %s-buffer\n",
245 vc
->ch
, pb
? "B" : "P");
246 vb2_buffer_done(&buf
->vb
.vb2_buf
, VB2_BUF_STATE_ERROR
);
250 buf
->vb
.vb2_buf
.state
= VB2_BUF_STATE_ACTIVE
;
251 vc
->curr_bufs
[pb
] = buf
;
255 vc
->curr_bufs
[pb
] = NULL
;
258 static void tw686x_sg_dma_free(struct tw686x_video_channel
*vc
,
261 struct tw686x_dma_desc
*desc
= &vc
->dma_descs
[pb
];
262 struct tw686x_dev
*dev
= vc
->dev
;
265 pci_free_consistent(dev
->pci_dev
, desc
->size
,
266 desc
->virt
, desc
->phys
);
270 vc
->sg_descs
[pb
] = NULL
;
273 static int tw686x_sg_dma_alloc(struct tw686x_video_channel
*vc
,
276 struct tw686x_dma_desc
*desc
= &vc
->dma_descs
[pb
];
277 struct tw686x_dev
*dev
= vc
->dev
;
278 u32 reg
= pb
? DMA_PAGE_TABLE1_ADDR
[vc
->ch
] :
279 DMA_PAGE_TABLE0_ADDR
[vc
->ch
];
284 virt
= pci_alloc_consistent(dev
->pci_dev
, desc
->size
,
287 v4l2_err(&dev
->v4l2_dev
,
288 "dma%d: unable to allocate %s-buffer\n",
289 vc
->ch
, pb
? "B" : "P");
293 reg_write(dev
, reg
, desc
->phys
);
295 virt
= dev
->video_channels
[0].dma_descs
[pb
].virt
+
296 vc
->ch
* TW686X_SG_TABLE_SIZE
;
299 vc
->sg_descs
[pb
] = virt
;
303 static int tw686x_sg_setup(struct tw686x_dev
*dev
)
305 unsigned int sg_table_size
, pb
, ch
, channels
;
307 if (is_second_gen(dev
)) {
309 * TW6865/TW6869: each channel needs a pair of
310 * P-B descriptor tables.
312 channels
= max_channels(dev
);
313 sg_table_size
= TW686X_SG_TABLE_SIZE
;
316 * TW6864/TW6868: we need to allocate a pair of
317 * P-B descriptor tables, common for all channels.
318 * Each table will be bigger than 4 KB.
321 sg_table_size
= max_channels(dev
) * TW686X_SG_TABLE_SIZE
;
324 for (ch
= 0; ch
< channels
; ch
++) {
325 struct tw686x_video_channel
*vc
= &dev
->video_channels
[ch
];
327 for (pb
= 0; pb
< 2; pb
++)
328 vc
->dma_descs
[pb
].size
= sg_table_size
;
334 static const struct tw686x_dma_ops sg_dma_ops
= {
335 .setup
= tw686x_sg_setup
,
336 .alloc
= tw686x_sg_dma_alloc
,
337 .free
= tw686x_sg_dma_free
,
338 .buf_refill
= tw686x_sg_buf_refill
,
339 .mem_ops
= &vb2_dma_sg_memops
,
340 .hw_dma_mode
= TW686X_SG_MODE
,
341 .field
= V4L2_FIELD_SEQ_TB
,
344 static const unsigned int fps_map
[15] = {
346 * bit 31 enables selecting the field control register
347 * bits 0-29 are a bitmask with fields that will be output.
348 * For NTSC (and PAL-M, PAL-60), all 30 bits are used.
349 * For other PAL standards, only the first 25 bits are used.
351 0x00000000, /* output all fields */
352 0x80000006, /* 2 fps (60Hz), 2 fps (50Hz) */
353 0x80018006, /* 4 fps (60Hz), 4 fps (50Hz) */
354 0x80618006, /* 6 fps (60Hz), 6 fps (50Hz) */
355 0x81818186, /* 8 fps (60Hz), 8 fps (50Hz) */
356 0x86186186, /* 10 fps (60Hz), 8 fps (50Hz) */
357 0x86619866, /* 12 fps (60Hz), 10 fps (50Hz) */
358 0x86666666, /* 14 fps (60Hz), 12 fps (50Hz) */
359 0x9999999e, /* 16 fps (60Hz), 14 fps (50Hz) */
360 0x99e6799e, /* 18 fps (60Hz), 16 fps (50Hz) */
361 0x9e79e79e, /* 20 fps (60Hz), 16 fps (50Hz) */
362 0x9e7e7e7e, /* 22 fps (60Hz), 18 fps (50Hz) */
363 0x9fe7f9fe, /* 24 fps (60Hz), 20 fps (50Hz) */
364 0x9ffe7ffe, /* 26 fps (60Hz), 22 fps (50Hz) */
365 0x9ffffffe, /* 28 fps (60Hz), 24 fps (50Hz) */
368 static unsigned int tw686x_real_fps(unsigned int index
, unsigned int max_fps
)
372 if (!index
|| index
>= ARRAY_SIZE(fps_map
))
375 mask
= GENMASK(max_fps
- 1, 0);
376 return hweight_long(fps_map
[index
] & mask
);
379 static unsigned int tw686x_fps_idx(unsigned int fps
, unsigned int max_fps
)
381 unsigned int idx
, real_fps
;
385 idx
= (12 + 15 * fps
) / max_fps
;
387 /* Minimal possible framerate is 2 frames per second */
391 /* Check if the difference is bigger than abs(1) and adjust */
392 real_fps
= tw686x_real_fps(idx
, max_fps
);
393 delta
= real_fps
- fps
;
406 static void tw686x_set_framerate(struct tw686x_video_channel
*vc
,
411 i
= tw686x_fps_idx(fps
, TW686X_MAX_FPS(vc
->video_standard
));
412 reg_write(vc
->dev
, VIDEO_FIELD_CTRL
[vc
->ch
], fps_map
[i
]);
413 vc
->fps
= tw686x_real_fps(i
, TW686X_MAX_FPS(vc
->video_standard
));
416 static const struct tw686x_format
*format_by_fourcc(unsigned int fourcc
)
420 for (cnt
= 0; cnt
< ARRAY_SIZE(formats
); cnt
++)
421 if (formats
[cnt
].fourcc
== fourcc
)
422 return &formats
[cnt
];
426 static int tw686x_queue_setup(struct vb2_queue
*vq
,
427 unsigned int *nbuffers
, unsigned int *nplanes
,
428 unsigned int sizes
[], struct device
*alloc_devs
[])
430 struct tw686x_video_channel
*vc
= vb2_get_drv_priv(vq
);
431 unsigned int szimage
=
432 (vc
->width
* vc
->height
* vc
->format
->depth
) >> 3;
435 * Let's request at least three buffers: two for the
436 * DMA engine and one for userspace.
438 if (vq
->num_buffers
+ *nbuffers
< 3)
439 *nbuffers
= 3 - vq
->num_buffers
;
442 if (*nplanes
!= 1 || sizes
[0] < szimage
)
452 static void tw686x_buf_queue(struct vb2_buffer
*vb
)
454 struct tw686x_video_channel
*vc
= vb2_get_drv_priv(vb
->vb2_queue
);
455 struct tw686x_dev
*dev
= vc
->dev
;
456 struct pci_dev
*pci_dev
;
458 struct vb2_v4l2_buffer
*vbuf
= to_vb2_v4l2_buffer(vb
);
459 struct tw686x_v4l2_buf
*buf
=
460 container_of(vbuf
, struct tw686x_v4l2_buf
, vb
);
462 /* Check device presence */
463 spin_lock_irqsave(&dev
->lock
, flags
);
464 pci_dev
= dev
->pci_dev
;
465 spin_unlock_irqrestore(&dev
->lock
, flags
);
467 vb2_buffer_done(&buf
->vb
.vb2_buf
, VB2_BUF_STATE_ERROR
);
471 spin_lock_irqsave(&vc
->qlock
, flags
);
472 list_add_tail(&buf
->list
, &vc
->vidq_queued
);
473 spin_unlock_irqrestore(&vc
->qlock
, flags
);
476 static void tw686x_clear_queue(struct tw686x_video_channel
*vc
,
477 enum vb2_buffer_state state
)
481 while (!list_empty(&vc
->vidq_queued
)) {
482 struct tw686x_v4l2_buf
*buf
;
484 buf
= list_first_entry(&vc
->vidq_queued
,
485 struct tw686x_v4l2_buf
, list
);
486 list_del(&buf
->list
);
487 vb2_buffer_done(&buf
->vb
.vb2_buf
, state
);
490 for (pb
= 0; pb
< 2; pb
++) {
491 if (vc
->curr_bufs
[pb
])
492 vb2_buffer_done(&vc
->curr_bufs
[pb
]->vb
.vb2_buf
, state
);
493 vc
->curr_bufs
[pb
] = NULL
;
497 static int tw686x_start_streaming(struct vb2_queue
*vq
, unsigned int count
)
499 struct tw686x_video_channel
*vc
= vb2_get_drv_priv(vq
);
500 struct tw686x_dev
*dev
= vc
->dev
;
501 struct pci_dev
*pci_dev
;
505 /* Check device presence */
506 spin_lock_irqsave(&dev
->lock
, flags
);
507 pci_dev
= dev
->pci_dev
;
508 spin_unlock_irqrestore(&dev
->lock
, flags
);
511 goto err_clear_queue
;
514 spin_lock_irqsave(&vc
->qlock
, flags
);
517 if (dev
->dma_mode
== TW686X_DMA_MODE_MEMCPY
&&
518 (!vc
->dma_descs
[0].virt
|| !vc
->dma_descs
[1].virt
)) {
519 spin_unlock_irqrestore(&vc
->qlock
, flags
);
520 v4l2_err(&dev
->v4l2_dev
,
521 "video%d: refusing to start without DMA buffers\n",
524 goto err_clear_queue
;
527 for (pb
= 0; pb
< 2; pb
++)
528 dev
->dma_ops
->buf_refill(vc
, pb
);
529 spin_unlock_irqrestore(&vc
->qlock
, flags
);
534 spin_lock_irqsave(&dev
->lock
, flags
);
535 tw686x_enable_channel(dev
, vc
->ch
);
536 spin_unlock_irqrestore(&dev
->lock
, flags
);
538 mod_timer(&dev
->dma_delay_timer
, jiffies
+ msecs_to_jiffies(100));
543 spin_lock_irqsave(&vc
->qlock
, flags
);
544 tw686x_clear_queue(vc
, VB2_BUF_STATE_QUEUED
);
545 spin_unlock_irqrestore(&vc
->qlock
, flags
);
549 static void tw686x_stop_streaming(struct vb2_queue
*vq
)
551 struct tw686x_video_channel
*vc
= vb2_get_drv_priv(vq
);
552 struct tw686x_dev
*dev
= vc
->dev
;
553 struct pci_dev
*pci_dev
;
556 /* Check device presence */
557 spin_lock_irqsave(&dev
->lock
, flags
);
558 pci_dev
= dev
->pci_dev
;
559 spin_unlock_irqrestore(&dev
->lock
, flags
);
561 tw686x_disable_channel(dev
, vc
->ch
);
563 spin_lock_irqsave(&vc
->qlock
, flags
);
564 tw686x_clear_queue(vc
, VB2_BUF_STATE_ERROR
);
565 spin_unlock_irqrestore(&vc
->qlock
, flags
);
568 static int tw686x_buf_prepare(struct vb2_buffer
*vb
)
570 struct tw686x_video_channel
*vc
= vb2_get_drv_priv(vb
->vb2_queue
);
572 (vc
->width
* vc
->height
* vc
->format
->depth
) >> 3;
574 if (vb2_plane_size(vb
, 0) < size
)
576 vb2_set_plane_payload(vb
, 0, size
);
580 static struct vb2_ops tw686x_video_qops
= {
581 .queue_setup
= tw686x_queue_setup
,
582 .buf_queue
= tw686x_buf_queue
,
583 .buf_prepare
= tw686x_buf_prepare
,
584 .start_streaming
= tw686x_start_streaming
,
585 .stop_streaming
= tw686x_stop_streaming
,
586 .wait_prepare
= vb2_ops_wait_prepare
,
587 .wait_finish
= vb2_ops_wait_finish
,
590 static int tw686x_s_ctrl(struct v4l2_ctrl
*ctrl
)
592 struct tw686x_video_channel
*vc
;
593 struct tw686x_dev
*dev
;
596 vc
= container_of(ctrl
->handler
, struct tw686x_video_channel
,
602 case V4L2_CID_BRIGHTNESS
:
603 reg_write(dev
, BRIGHT
[ch
], ctrl
->val
& 0xff);
606 case V4L2_CID_CONTRAST
:
607 reg_write(dev
, CONTRAST
[ch
], ctrl
->val
);
610 case V4L2_CID_SATURATION
:
611 reg_write(dev
, SAT_U
[ch
], ctrl
->val
);
612 reg_write(dev
, SAT_V
[ch
], ctrl
->val
);
616 reg_write(dev
, HUE
[ch
], ctrl
->val
& 0xff);
623 static const struct v4l2_ctrl_ops ctrl_ops
= {
624 .s_ctrl
= tw686x_s_ctrl
,
627 static int tw686x_g_fmt_vid_cap(struct file
*file
, void *priv
,
628 struct v4l2_format
*f
)
630 struct tw686x_video_channel
*vc
= video_drvdata(file
);
631 struct tw686x_dev
*dev
= vc
->dev
;
633 f
->fmt
.pix
.width
= vc
->width
;
634 f
->fmt
.pix
.height
= vc
->height
;
635 f
->fmt
.pix
.field
= dev
->dma_ops
->field
;
636 f
->fmt
.pix
.pixelformat
= vc
->format
->fourcc
;
637 f
->fmt
.pix
.colorspace
= V4L2_COLORSPACE_SMPTE170M
;
638 f
->fmt
.pix
.bytesperline
= (f
->fmt
.pix
.width
* vc
->format
->depth
) / 8;
639 f
->fmt
.pix
.sizeimage
= f
->fmt
.pix
.height
* f
->fmt
.pix
.bytesperline
;
643 static int tw686x_try_fmt_vid_cap(struct file
*file
, void *priv
,
644 struct v4l2_format
*f
)
646 struct tw686x_video_channel
*vc
= video_drvdata(file
);
647 struct tw686x_dev
*dev
= vc
->dev
;
648 unsigned int video_height
= TW686X_VIDEO_HEIGHT(vc
->video_standard
);
649 const struct tw686x_format
*format
;
651 format
= format_by_fourcc(f
->fmt
.pix
.pixelformat
);
653 format
= &formats
[0];
654 f
->fmt
.pix
.pixelformat
= format
->fourcc
;
657 if (f
->fmt
.pix
.width
<= TW686X_VIDEO_WIDTH
/ 2)
658 f
->fmt
.pix
.width
= TW686X_VIDEO_WIDTH
/ 2;
660 f
->fmt
.pix
.width
= TW686X_VIDEO_WIDTH
;
662 if (f
->fmt
.pix
.height
<= video_height
/ 2)
663 f
->fmt
.pix
.height
= video_height
/ 2;
665 f
->fmt
.pix
.height
= video_height
;
667 f
->fmt
.pix
.bytesperline
= (f
->fmt
.pix
.width
* format
->depth
) / 8;
668 f
->fmt
.pix
.sizeimage
= f
->fmt
.pix
.height
* f
->fmt
.pix
.bytesperline
;
669 f
->fmt
.pix
.colorspace
= V4L2_COLORSPACE_SMPTE170M
;
670 f
->fmt
.pix
.field
= dev
->dma_ops
->field
;
675 static int tw686x_set_format(struct tw686x_video_channel
*vc
,
676 unsigned int pixelformat
, unsigned int width
,
677 unsigned int height
, bool realloc
)
679 struct tw686x_dev
*dev
= vc
->dev
;
680 u32 val
, dma_width
, dma_height
, dma_line_width
;
683 vc
->format
= format_by_fourcc(pixelformat
);
687 /* We need new DMA buffers if the framesize has changed */
688 if (dev
->dma_ops
->alloc
&& realloc
) {
689 for (pb
= 0; pb
< 2; pb
++)
690 dev
->dma_ops
->free(vc
, pb
);
692 for (pb
= 0; pb
< 2; pb
++) {
693 err
= dev
->dma_ops
->alloc(vc
, pb
);
696 dev
->dma_ops
->free(vc
, 0);
702 val
= reg_read(vc
->dev
, VDMA_CHANNEL_CONFIG
[vc
->ch
]);
704 if (vc
->width
<= TW686X_VIDEO_WIDTH
/ 2)
709 if (vc
->height
<= TW686X_VIDEO_HEIGHT(vc
->video_standard
) / 2)
716 /* Program the DMA scatter-gather */
717 if (dev
->dma_mode
== TW686X_DMA_MODE_SG
) {
718 u32 start_idx
, end_idx
;
720 start_idx
= is_second_gen(dev
) ?
721 0 : vc
->ch
* TW686X_MAX_SG_DESC_COUNT
;
722 end_idx
= start_idx
+ TW686X_MAX_SG_DESC_COUNT
- 1;
724 val
|= (end_idx
<< 10) | start_idx
;
728 val
|= vc
->format
->mode
<< 20;
729 reg_write(vc
->dev
, VDMA_CHANNEL_CONFIG
[vc
->ch
], val
);
731 /* Program the DMA frame size */
732 dma_width
= (vc
->width
* 2) & 0x7ff;
733 dma_height
= vc
->height
/ 2;
734 dma_line_width
= (vc
->width
* 2) & 0x7ff;
735 val
= (dma_height
<< 22) | (dma_line_width
<< 11) | dma_width
;
736 reg_write(vc
->dev
, VDMA_WHP
[vc
->ch
], val
);
740 static int tw686x_s_fmt_vid_cap(struct file
*file
, void *priv
,
741 struct v4l2_format
*f
)
743 struct tw686x_video_channel
*vc
= video_drvdata(file
);
748 if (vb2_is_busy(&vc
->vidq
))
751 area
= vc
->width
* vc
->height
;
752 err
= tw686x_try_fmt_vid_cap(file
, priv
, f
);
756 realloc
= area
!= (f
->fmt
.pix
.width
* f
->fmt
.pix
.height
);
757 return tw686x_set_format(vc
, f
->fmt
.pix
.pixelformat
,
758 f
->fmt
.pix
.width
, f
->fmt
.pix
.height
,
762 static int tw686x_querycap(struct file
*file
, void *priv
,
763 struct v4l2_capability
*cap
)
765 struct tw686x_video_channel
*vc
= video_drvdata(file
);
766 struct tw686x_dev
*dev
= vc
->dev
;
768 strlcpy(cap
->driver
, "tw686x", sizeof(cap
->driver
));
769 strlcpy(cap
->card
, dev
->name
, sizeof(cap
->card
));
770 snprintf(cap
->bus_info
, sizeof(cap
->bus_info
),
771 "PCI:%s", pci_name(dev
->pci_dev
));
772 cap
->device_caps
= V4L2_CAP_VIDEO_CAPTURE
| V4L2_CAP_STREAMING
|
774 cap
->capabilities
= cap
->device_caps
| V4L2_CAP_DEVICE_CAPS
;
778 static int tw686x_set_standard(struct tw686x_video_channel
*vc
, v4l2_std_id id
)
782 if (id
& V4L2_STD_NTSC
)
784 else if (id
& V4L2_STD_PAL
)
786 else if (id
& V4L2_STD_SECAM
)
788 else if (id
& V4L2_STD_NTSC_443
)
790 else if (id
& V4L2_STD_PAL_M
)
792 else if (id
& V4L2_STD_PAL_Nc
)
794 else if (id
& V4L2_STD_PAL_60
)
799 vc
->video_standard
= id
;
800 reg_write(vc
->dev
, SDT
[vc
->ch
], val
);
802 val
= reg_read(vc
->dev
, VIDEO_CONTROL1
);
803 if (id
& V4L2_STD_525_60
)
804 val
&= ~(1 << (SYS_MODE_DMA_SHIFT
+ vc
->ch
));
806 val
|= (1 << (SYS_MODE_DMA_SHIFT
+ vc
->ch
));
807 reg_write(vc
->dev
, VIDEO_CONTROL1
, val
);
812 static int tw686x_s_std(struct file
*file
, void *priv
, v4l2_std_id id
)
814 struct tw686x_video_channel
*vc
= video_drvdata(file
);
815 struct v4l2_format f
;
818 if (vc
->video_standard
== id
)
821 if (vb2_is_busy(&vc
->vidq
))
824 ret
= tw686x_set_standard(vc
, id
);
828 * Adjust format after V4L2_STD_525_60/V4L2_STD_625_50 change,
829 * calling g_fmt and s_fmt will sanitize the height
830 * according to the standard.
832 tw686x_g_fmt_vid_cap(file
, priv
, &f
);
833 tw686x_s_fmt_vid_cap(file
, priv
, &f
);
836 * Frame decimation depends on the chosen standard,
837 * so reset it to the current value.
839 tw686x_set_framerate(vc
, vc
->fps
);
843 static int tw686x_querystd(struct file
*file
, void *priv
, v4l2_std_id
*std
)
845 struct tw686x_video_channel
*vc
= video_drvdata(file
);
846 struct tw686x_dev
*dev
= vc
->dev
;
847 unsigned int old_std
, detected_std
= 0;
850 if (vb2_is_streaming(&vc
->vidq
))
853 /* Enable and start standard detection */
854 old_std
= reg_read(dev
, SDT
[vc
->ch
]);
855 reg_write(dev
, SDT
[vc
->ch
], 0x7);
856 reg_write(dev
, SDT_EN
[vc
->ch
], 0xff);
858 end
= jiffies
+ msecs_to_jiffies(500);
859 while (time_is_after_jiffies(end
)) {
861 detected_std
= reg_read(dev
, SDT
[vc
->ch
]);
862 if (!(detected_std
& BIT(7)))
866 reg_write(dev
, SDT
[vc
->ch
], old_std
);
868 /* Exit if still busy */
869 if (detected_std
& BIT(7))
872 detected_std
= (detected_std
>> 4) & 0x7;
873 switch (detected_std
) {
874 case TW686X_STD_NTSC_M
:
875 *std
&= V4L2_STD_NTSC
;
877 case TW686X_STD_NTSC_443
:
878 *std
&= V4L2_STD_NTSC_443
;
880 case TW686X_STD_PAL_M
:
881 *std
&= V4L2_STD_PAL_M
;
883 case TW686X_STD_PAL_60
:
884 *std
&= V4L2_STD_PAL_60
;
887 *std
&= V4L2_STD_PAL
;
889 case TW686X_STD_PAL_CN
:
890 *std
&= V4L2_STD_PAL_Nc
;
892 case TW686X_STD_SECAM
:
893 *std
&= V4L2_STD_SECAM
;
901 static int tw686x_g_std(struct file
*file
, void *priv
, v4l2_std_id
*id
)
903 struct tw686x_video_channel
*vc
= video_drvdata(file
);
905 *id
= vc
->video_standard
;
909 static int tw686x_g_parm(struct file
*file
, void *priv
,
910 struct v4l2_streamparm
*sp
)
912 struct tw686x_video_channel
*vc
= video_drvdata(file
);
913 struct v4l2_captureparm
*cp
= &sp
->parm
.capture
;
915 if (sp
->type
!= V4L2_BUF_TYPE_VIDEO_CAPTURE
)
917 sp
->parm
.capture
.readbuffers
= 3;
919 cp
->capability
= V4L2_CAP_TIMEPERFRAME
;
920 cp
->timeperframe
.numerator
= 1;
921 cp
->timeperframe
.denominator
= vc
->fps
;
925 static int tw686x_s_parm(struct file
*file
, void *priv
,
926 struct v4l2_streamparm
*sp
)
928 struct tw686x_video_channel
*vc
= video_drvdata(file
);
929 struct v4l2_captureparm
*cp
= &sp
->parm
.capture
;
930 unsigned int denominator
= cp
->timeperframe
.denominator
;
931 unsigned int numerator
= cp
->timeperframe
.numerator
;
934 if (vb2_is_busy(&vc
->vidq
))
937 fps
= (!numerator
|| !denominator
) ? 0 : denominator
/ numerator
;
939 tw686x_set_framerate(vc
, fps
);
940 return tw686x_g_parm(file
, priv
, sp
);
943 static int tw686x_enum_fmt_vid_cap(struct file
*file
, void *priv
,
944 struct v4l2_fmtdesc
*f
)
946 if (f
->index
>= ARRAY_SIZE(formats
))
948 f
->pixelformat
= formats
[f
->index
].fourcc
;
952 static void tw686x_set_input(struct tw686x_video_channel
*vc
, unsigned int i
)
958 val
= reg_read(vc
->dev
, VDMA_CHANNEL_CONFIG
[vc
->ch
]);
961 reg_write(vc
->dev
, VDMA_CHANNEL_CONFIG
[vc
->ch
], val
);
964 static int tw686x_s_input(struct file
*file
, void *priv
, unsigned int i
)
966 struct tw686x_video_channel
*vc
= video_drvdata(file
);
968 if (i
>= TW686X_INPUTS_PER_CH
)
973 * Not sure we are able to support on the fly input change
975 if (vb2_is_busy(&vc
->vidq
))
978 tw686x_set_input(vc
, i
);
982 static int tw686x_g_input(struct file
*file
, void *priv
, unsigned int *i
)
984 struct tw686x_video_channel
*vc
= video_drvdata(file
);
990 static int tw686x_enum_input(struct file
*file
, void *priv
,
991 struct v4l2_input
*i
)
993 struct tw686x_video_channel
*vc
= video_drvdata(file
);
994 unsigned int vidstat
;
996 if (i
->index
>= TW686X_INPUTS_PER_CH
)
999 snprintf(i
->name
, sizeof(i
->name
), "Composite%d", i
->index
);
1000 i
->type
= V4L2_INPUT_TYPE_CAMERA
;
1001 i
->std
= vc
->device
->tvnorms
;
1002 i
->capabilities
= V4L2_IN_CAP_STD
;
1004 vidstat
= reg_read(vc
->dev
, VIDSTAT
[vc
->ch
]);
1006 if (vidstat
& TW686X_VIDSTAT_VDLOSS
)
1007 i
->status
|= V4L2_IN_ST_NO_SIGNAL
;
1008 if (!(vidstat
& TW686X_VIDSTAT_HLOCK
))
1009 i
->status
|= V4L2_IN_ST_NO_H_LOCK
;
1014 static const struct v4l2_file_operations tw686x_video_fops
= {
1015 .owner
= THIS_MODULE
,
1016 .open
= v4l2_fh_open
,
1017 .unlocked_ioctl
= video_ioctl2
,
1018 .release
= vb2_fop_release
,
1019 .poll
= vb2_fop_poll
,
1020 .read
= vb2_fop_read
,
1021 .mmap
= vb2_fop_mmap
,
1024 static const struct v4l2_ioctl_ops tw686x_video_ioctl_ops
= {
1025 .vidioc_querycap
= tw686x_querycap
,
1026 .vidioc_g_fmt_vid_cap
= tw686x_g_fmt_vid_cap
,
1027 .vidioc_s_fmt_vid_cap
= tw686x_s_fmt_vid_cap
,
1028 .vidioc_enum_fmt_vid_cap
= tw686x_enum_fmt_vid_cap
,
1029 .vidioc_try_fmt_vid_cap
= tw686x_try_fmt_vid_cap
,
1031 .vidioc_querystd
= tw686x_querystd
,
1032 .vidioc_g_std
= tw686x_g_std
,
1033 .vidioc_s_std
= tw686x_s_std
,
1035 .vidioc_g_parm
= tw686x_g_parm
,
1036 .vidioc_s_parm
= tw686x_s_parm
,
1038 .vidioc_enum_input
= tw686x_enum_input
,
1039 .vidioc_g_input
= tw686x_g_input
,
1040 .vidioc_s_input
= tw686x_s_input
,
1042 .vidioc_reqbufs
= vb2_ioctl_reqbufs
,
1043 .vidioc_querybuf
= vb2_ioctl_querybuf
,
1044 .vidioc_qbuf
= vb2_ioctl_qbuf
,
1045 .vidioc_dqbuf
= vb2_ioctl_dqbuf
,
1046 .vidioc_create_bufs
= vb2_ioctl_create_bufs
,
1047 .vidioc_streamon
= vb2_ioctl_streamon
,
1048 .vidioc_streamoff
= vb2_ioctl_streamoff
,
1049 .vidioc_prepare_buf
= vb2_ioctl_prepare_buf
,
1051 .vidioc_log_status
= v4l2_ctrl_log_status
,
1052 .vidioc_subscribe_event
= v4l2_ctrl_subscribe_event
,
1053 .vidioc_unsubscribe_event
= v4l2_event_unsubscribe
,
1056 void tw686x_video_irq(struct tw686x_dev
*dev
, unsigned long requests
,
1057 unsigned int pb_status
, unsigned int fifo_status
,
1058 unsigned int *reset_ch
)
1060 struct tw686x_video_channel
*vc
;
1061 unsigned long flags
;
1062 unsigned int ch
, pb
;
1064 for_each_set_bit(ch
, &requests
, max_channels(dev
)) {
1065 vc
= &dev
->video_channels
[ch
];
1068 * This can either be a blue frame (with signal-lost bit set)
1069 * or a good frame (with signal-lost bit clear). If we have just
1070 * got signal, then this channel needs resetting.
1072 if (vc
->no_signal
&& !(fifo_status
& BIT(ch
))) {
1073 v4l2_printk(KERN_DEBUG
, &dev
->v4l2_dev
,
1074 "video%d: signal recovered\n", vc
->num
);
1075 vc
->no_signal
= false;
1076 *reset_ch
|= BIT(ch
);
1080 vc
->no_signal
= !!(fifo_status
& BIT(ch
));
1082 /* Check FIFO errors only if there's signal */
1083 if (!vc
->no_signal
) {
1084 u32 fifo_ov
, fifo_bad
;
1086 fifo_ov
= (fifo_status
>> 24) & BIT(ch
);
1087 fifo_bad
= (fifo_status
>> 16) & BIT(ch
);
1088 if (fifo_ov
|| fifo_bad
) {
1089 /* Mark this channel for reset */
1090 v4l2_printk(KERN_DEBUG
, &dev
->v4l2_dev
,
1091 "video%d: FIFO error\n", vc
->num
);
1092 *reset_ch
|= BIT(ch
);
1098 pb
= !!(pb_status
& BIT(ch
));
1100 /* Mark this channel for reset */
1101 v4l2_printk(KERN_DEBUG
, &dev
->v4l2_dev
,
1102 "video%d: unexpected p-b buffer!\n",
1104 *reset_ch
|= BIT(ch
);
1109 spin_lock_irqsave(&vc
->qlock
, flags
);
1110 tw686x_buf_done(vc
, pb
);
1111 dev
->dma_ops
->buf_refill(vc
, pb
);
1112 spin_unlock_irqrestore(&vc
->qlock
, flags
);
1116 void tw686x_video_free(struct tw686x_dev
*dev
)
1118 unsigned int ch
, pb
;
1120 for (ch
= 0; ch
< max_channels(dev
); ch
++) {
1121 struct tw686x_video_channel
*vc
= &dev
->video_channels
[ch
];
1123 video_unregister_device(vc
->device
);
1125 if (dev
->dma_ops
->free
)
1126 for (pb
= 0; pb
< 2; pb
++)
1127 dev
->dma_ops
->free(vc
, pb
);
1131 int tw686x_video_init(struct tw686x_dev
*dev
)
1133 unsigned int ch
, val
;
1136 if (dev
->dma_mode
== TW686X_DMA_MODE_MEMCPY
)
1137 dev
->dma_ops
= &memcpy_dma_ops
;
1138 else if (dev
->dma_mode
== TW686X_DMA_MODE_CONTIG
)
1139 dev
->dma_ops
= &contig_dma_ops
;
1140 else if (dev
->dma_mode
== TW686X_DMA_MODE_SG
)
1141 dev
->dma_ops
= &sg_dma_ops
;
1145 err
= v4l2_device_register(&dev
->pci_dev
->dev
, &dev
->v4l2_dev
);
1149 if (dev
->dma_ops
->setup
) {
1150 err
= dev
->dma_ops
->setup(dev
);
1155 for (ch
= 0; ch
< max_channels(dev
); ch
++) {
1156 struct tw686x_video_channel
*vc
= &dev
->video_channels
[ch
];
1157 struct video_device
*vdev
;
1159 mutex_init(&vc
->vb_mutex
);
1160 spin_lock_init(&vc
->qlock
);
1161 INIT_LIST_HEAD(&vc
->vidq_queued
);
1166 /* default settings */
1167 err
= tw686x_set_standard(vc
, V4L2_STD_NTSC
);
1171 err
= tw686x_set_format(vc
, formats
[0].fourcc
,
1173 TW686X_VIDEO_HEIGHT(vc
->video_standard
),
1178 tw686x_set_input(vc
, 0);
1179 tw686x_set_framerate(vc
, 30);
1180 reg_write(dev
, VDELAY_LO
[ch
], 0x14);
1181 reg_write(dev
, HACTIVE_LO
[ch
], 0xd0);
1182 reg_write(dev
, VIDEO_SIZE
[ch
], 0);
1184 vc
->vidq
.io_modes
= VB2_READ
| VB2_MMAP
| VB2_DMABUF
;
1185 vc
->vidq
.type
= V4L2_BUF_TYPE_VIDEO_CAPTURE
;
1186 vc
->vidq
.drv_priv
= vc
;
1187 vc
->vidq
.buf_struct_size
= sizeof(struct tw686x_v4l2_buf
);
1188 vc
->vidq
.ops
= &tw686x_video_qops
;
1189 vc
->vidq
.mem_ops
= dev
->dma_ops
->mem_ops
;
1190 vc
->vidq
.timestamp_flags
= V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC
;
1191 vc
->vidq
.min_buffers_needed
= 2;
1192 vc
->vidq
.lock
= &vc
->vb_mutex
;
1193 vc
->vidq
.gfp_flags
= GFP_DMA32
;
1194 vc
->vidq
.dev
= &dev
->pci_dev
->dev
;
1196 err
= vb2_queue_init(&vc
->vidq
);
1198 v4l2_err(&dev
->v4l2_dev
,
1199 "dma%d: cannot init vb2 queue\n", ch
);
1203 err
= v4l2_ctrl_handler_init(&vc
->ctrl_handler
, 4);
1205 v4l2_err(&dev
->v4l2_dev
,
1206 "dma%d: cannot init ctrl handler\n", ch
);
1209 v4l2_ctrl_new_std(&vc
->ctrl_handler
, &ctrl_ops
,
1210 V4L2_CID_BRIGHTNESS
, -128, 127, 1, 0);
1211 v4l2_ctrl_new_std(&vc
->ctrl_handler
, &ctrl_ops
,
1212 V4L2_CID_CONTRAST
, 0, 255, 1, 100);
1213 v4l2_ctrl_new_std(&vc
->ctrl_handler
, &ctrl_ops
,
1214 V4L2_CID_SATURATION
, 0, 255, 1, 128);
1215 v4l2_ctrl_new_std(&vc
->ctrl_handler
, &ctrl_ops
,
1216 V4L2_CID_HUE
, -128, 127, 1, 0);
1217 err
= vc
->ctrl_handler
.error
;
1221 err
= v4l2_ctrl_handler_setup(&vc
->ctrl_handler
);
1225 vdev
= video_device_alloc();
1227 v4l2_err(&dev
->v4l2_dev
,
1228 "dma%d: unable to allocate device\n", ch
);
1233 snprintf(vdev
->name
, sizeof(vdev
->name
), "%s video", dev
->name
);
1234 vdev
->fops
= &tw686x_video_fops
;
1235 vdev
->ioctl_ops
= &tw686x_video_ioctl_ops
;
1236 vdev
->release
= video_device_release
;
1237 vdev
->v4l2_dev
= &dev
->v4l2_dev
;
1238 vdev
->queue
= &vc
->vidq
;
1239 vdev
->tvnorms
= V4L2_STD_525_60
| V4L2_STD_625_50
;
1241 vdev
->lock
= &vc
->vb_mutex
;
1242 vdev
->ctrl_handler
= &vc
->ctrl_handler
;
1244 video_set_drvdata(vdev
, vc
);
1246 err
= video_register_device(vdev
, VFL_TYPE_GRABBER
, -1);
1249 vc
->num
= vdev
->num
;
1252 val
= TW686X_DEF_PHASE_REF
;
1253 for (ch
= 0; ch
< max_channels(dev
); ch
++)
1254 val
|= dev
->dma_ops
->hw_dma_mode
<< (16 + ch
* 2);
1255 reg_write(dev
, PHASE_REF
, val
);
1257 reg_write(dev
, MISC2
[0], 0xe7);
1258 reg_write(dev
, VCTRL1
[0], 0xcc);
1259 reg_write(dev
, LOOP
[0], 0xa5);
1260 if (max_channels(dev
) > 4) {
1261 reg_write(dev
, VCTRL1
[1], 0xcc);
1262 reg_write(dev
, LOOP
[1], 0xa5);
1263 reg_write(dev
, MISC2
[1], 0xe7);
1268 tw686x_video_free(dev
);