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