1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3  * STK1160 driver
4  *
5  * Copyright (C) 2012 Ezequiel Garcia
6  * <elezegarcia--a.t--gmail.com>
7  *
8  * Based on Easycap driver by R.M. Thomas
9  *	Copyright (C) 2010 R.M. Thomas
10  *	<rmthomas--a.t--sciolus.org>
11  */
12 
13 #include <linux/module.h>
14 #include <linux/usb.h>
15 #include <linux/mm.h>
16 #include <linux/slab.h>
17 
18 #include <linux/videodev2.h>
19 #include <media/v4l2-device.h>
20 #include <media/v4l2-common.h>
21 #include <media/v4l2-ioctl.h>
22 #include <media/v4l2-fh.h>
23 #include <media/v4l2-event.h>
24 #include <media/videobuf2-vmalloc.h>
25 
26 #include <media/i2c/saa7115.h>
27 
28 #include "stk1160.h"
29 #include "stk1160-reg.h"
30 
31 static bool keep_buffers;
32 module_param(keep_buffers, bool, 0644);
33 MODULE_PARM_DESC(keep_buffers, "don't release buffers upon stop streaming");
34 
35 enum stk1160_decimate_mode {
36 	STK1160_DECIMATE_MORE_THAN_HALF,
37 	STK1160_DECIMATE_LESS_THAN_HALF,
38 };
39 
40 struct stk1160_decimate_ctrl {
41 	bool col_en, row_en;
42 	enum stk1160_decimate_mode col_mode, row_mode;
43 	unsigned int col_n, row_n;
44 };
45 
46 /* supported video standards */
47 static struct stk1160_fmt format[] = {
48 	{
49 		.fourcc   = V4L2_PIX_FMT_UYVY,
50 		.depth    = 16,
51 	}
52 };
53 
54 /*
55  * Helper to find the next divisor that results in modulo being zero.
56  * This is required to guarantee valid decimation unit counts.
57  */
58 static unsigned int
59 div_round_integer(unsigned int x, unsigned int y)
60 {
61 	for (;; y++) {
62 		if (x % y == 0)
63 			return x / y;
64 	}
65 }
66 
67 static void stk1160_set_std(struct stk1160 *dev)
68 {
69 	int i;
70 
71 	static struct regval std525[] = {
72 
73 		/* 720x480 */
74 
75 		/* Frame start */
76 		{STK116_CFSPO_STX_L, 0x0000},
77 		{STK116_CFSPO_STX_H, 0x0000},
78 		{STK116_CFSPO_STY_L, 0x0003},
79 		{STK116_CFSPO_STY_H, 0x0000},
80 
81 		/* Frame end */
82 		{STK116_CFEPO_ENX_L, 0x05a0},
83 		{STK116_CFEPO_ENX_H, 0x0005},
84 		{STK116_CFEPO_ENY_L, 0x00f3},
85 		{STK116_CFEPO_ENY_H, 0x0000},
86 
87 		{0xffff, 0xffff}
88 	};
89 
90 	static struct regval std625[] = {
91 
92 		/* 720x576 */
93 
94 		/* TODO: Each line of frame has some junk at the end */
95 		/* Frame start */
96 		{STK116_CFSPO,   0x0000},
97 		{STK116_CFSPO+1, 0x0000},
98 		{STK116_CFSPO+2, 0x0001},
99 		{STK116_CFSPO+3, 0x0000},
100 
101 		/* Frame end */
102 		{STK116_CFEPO,   0x05a0},
103 		{STK116_CFEPO+1, 0x0005},
104 		{STK116_CFEPO+2, 0x0121},
105 		{STK116_CFEPO+3, 0x0001},
106 
107 		{0xffff, 0xffff}
108 	};
109 
110 	if (dev->norm & V4L2_STD_525_60) {
111 		stk1160_dbg("registers to NTSC like standard\n");
112 		for (i = 0; std525[i].reg != 0xffff; i++)
113 			stk1160_write_reg(dev, std525[i].reg, std525[i].val);
114 	} else {
115 		stk1160_dbg("registers to PAL like standard\n");
116 		for (i = 0; std625[i].reg != 0xffff; i++)
117 			stk1160_write_reg(dev, std625[i].reg, std625[i].val);
118 	}
119 
120 }
121 
122 static void stk1160_set_fmt(struct stk1160 *dev,
123 			    struct stk1160_decimate_ctrl *ctrl)
124 {
125 	u32 val = 0;
126 
127 	if (ctrl) {
128 		/*
129 		 * Since the format is UYVY, the device must skip or send
130 		 * a number of rows/columns multiple of four. This way, the
131 		 * colour format is preserved. The STK1160_DEC_UNIT_SIZE bit
132 		 * does exactly this.
133 		 */
134 		val |= STK1160_DEC_UNIT_SIZE;
135 		val |= ctrl->col_en ? STK1160_H_DEC_EN : 0;
136 		val |= ctrl->row_en ? STK1160_V_DEC_EN : 0;
137 		val |= ctrl->col_mode ==
138 			STK1160_DECIMATE_MORE_THAN_HALF ?
139 			STK1160_H_DEC_MODE : 0;
140 		val |= ctrl->row_mode ==
141 			STK1160_DECIMATE_MORE_THAN_HALF ?
142 			STK1160_V_DEC_MODE : 0;
143 
144 		/* Horizontal count units */
145 		stk1160_write_reg(dev, STK1160_DMCTRL_H_UNITS, ctrl->col_n);
146 		/* Vertical count units */
147 		stk1160_write_reg(dev, STK1160_DMCTRL_V_UNITS, ctrl->row_n);
148 
149 		stk1160_dbg("decimate 0x%x, column units %d, row units %d\n",
150 			    val, ctrl->col_n, ctrl->row_n);
151 	}
152 
153 	/* Decimation control */
154 	stk1160_write_reg(dev, STK1160_DMCTRL, val);
155 }
156 
157 /*
158  * Set a new alternate setting.
159  * Returns true is dev->max_pkt_size has changed, false otherwise.
160  */
161 static bool stk1160_set_alternate(struct stk1160 *dev)
162 {
163 	int i, prev_alt = dev->alt;
164 	unsigned int min_pkt_size;
165 	bool new_pkt_size;
166 
167 	/*
168 	 * If we don't set right alternate,
169 	 * then we will get a green screen with junk.
170 	 */
171 	min_pkt_size = STK1160_MIN_PKT_SIZE;
172 
173 	for (i = 0; i < dev->num_alt; i++) {
174 		/* stop when the selected alt setting offers enough bandwidth */
175 		if (dev->alt_max_pkt_size[i] >= min_pkt_size) {
176 			dev->alt = i;
177 			break;
178 		/*
179 		 * otherwise make sure that we end up with the maximum bandwidth
180 		 * because the min_pkt_size equation might be wrong...
181 		 */
182 		} else if (dev->alt_max_pkt_size[i] >
183 			   dev->alt_max_pkt_size[dev->alt])
184 			dev->alt = i;
185 	}
186 
187 	stk1160_dbg("setting alternate %d\n", dev->alt);
188 
189 	if (dev->alt != prev_alt) {
190 		stk1160_dbg("minimum isoc packet size: %u (alt=%d)\n",
191 				min_pkt_size, dev->alt);
192 		stk1160_dbg("setting alt %d with wMaxPacketSize=%u\n",
193 			       dev->alt, dev->alt_max_pkt_size[dev->alt]);
194 		usb_set_interface(dev->udev, 0, dev->alt);
195 	}
196 
197 	new_pkt_size = dev->max_pkt_size != dev->alt_max_pkt_size[dev->alt];
198 	dev->max_pkt_size = dev->alt_max_pkt_size[dev->alt];
199 
200 	return new_pkt_size;
201 }
202 
203 static int stk1160_start_streaming(struct stk1160 *dev)
204 {
205 	bool new_pkt_size;
206 	int rc = 0;
207 	int i;
208 
209 	/* Check device presence */
210 	if (!dev->udev)
211 		return -ENODEV;
212 
213 	if (mutex_lock_interruptible(&dev->v4l_lock))
214 		return -ERESTARTSYS;
215 	/*
216 	 * For some reason it is mandatory to set alternate *first*
217 	 * and only *then* initialize isoc urbs.
218 	 * Someone please explain me why ;)
219 	 */
220 	new_pkt_size = stk1160_set_alternate(dev);
221 
222 	/*
223 	 * We (re)allocate isoc urbs if:
224 	 * there is no allocated isoc urbs, OR
225 	 * a new dev->max_pkt_size is detected
226 	 */
227 	if (!dev->isoc_ctl.num_bufs || new_pkt_size) {
228 		rc = stk1160_alloc_isoc(dev);
229 		if (rc < 0)
230 			goto out_stop_hw;
231 	}
232 
233 	/* submit urbs and enables IRQ */
234 	for (i = 0; i < dev->isoc_ctl.num_bufs; i++) {
235 		struct stk1160_urb *stk_urb = &dev->isoc_ctl.urb_ctl[i];
236 
237 		dma_sync_sgtable_for_device(stk1160_get_dmadev(dev), stk_urb->sgt,
238 					    DMA_FROM_DEVICE);
239 		rc = usb_submit_urb(dev->isoc_ctl.urb_ctl[i].urb, GFP_KERNEL);
240 		if (rc) {
241 			stk1160_err("cannot submit urb[%d] (%d)\n", i, rc);
242 			goto out_uninit;
243 		}
244 	}
245 
246 	/* Start saa711x */
247 	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_stream, 1);
248 
249 	dev->sequence = 0;
250 
251 	/* Start stk1160 */
252 	stk1160_write_reg(dev, STK1160_DCTRL, 0xb3);
253 	stk1160_write_reg(dev, STK1160_DCTRL+3, 0x00);
254 
255 	stk1160_dbg("streaming started\n");
256 
257 	mutex_unlock(&dev->v4l_lock);
258 
259 	return 0;
260 
261 out_uninit:
262 	stk1160_uninit_isoc(dev);
263 out_stop_hw:
264 	usb_set_interface(dev->udev, 0, 0);
265 	stk1160_clear_queue(dev, VB2_BUF_STATE_QUEUED);
266 
267 	mutex_unlock(&dev->v4l_lock);
268 
269 	return rc;
270 }
271 
272 /* Must be called with v4l_lock hold */
273 static void stk1160_stop_hw(struct stk1160 *dev)
274 {
275 	/* If the device is not physically present, there is nothing to do */
276 	if (!dev->udev)
277 		return;
278 
279 	/* set alternate 0 */
280 	dev->alt = 0;
281 	stk1160_dbg("setting alternate %d\n", dev->alt);
282 	usb_set_interface(dev->udev, 0, 0);
283 
284 	/* Stop stk1160 */
285 	stk1160_write_reg(dev, STK1160_DCTRL, 0x00);
286 	stk1160_write_reg(dev, STK1160_DCTRL+3, 0x00);
287 
288 	/* Stop saa711x */
289 	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_stream, 0);
290 }
291 
292 static int stk1160_stop_streaming(struct stk1160 *dev)
293 {
294 	if (mutex_lock_interruptible(&dev->v4l_lock))
295 		return -ERESTARTSYS;
296 
297 	/*
298 	 * Once URBs are cancelled, the URB complete handler
299 	 * won't be running. This is required to safely release the
300 	 * current buffer (dev->isoc_ctl.buf).
301 	 */
302 	stk1160_cancel_isoc(dev);
303 
304 	/*
305 	 * It is possible to keep buffers around using a module parameter.
306 	 * This is intended to avoid memory fragmentation.
307 	 */
308 	if (!keep_buffers)
309 		stk1160_free_isoc(dev);
310 
311 	stk1160_stop_hw(dev);
312 
313 	stk1160_clear_queue(dev, VB2_BUF_STATE_ERROR);
314 
315 	stk1160_dbg("streaming stopped\n");
316 
317 	mutex_unlock(&dev->v4l_lock);
318 
319 	return 0;
320 }
321 
322 static const struct v4l2_file_operations stk1160_fops = {
323 	.owner = THIS_MODULE,
324 	.open = v4l2_fh_open,
325 	.release = vb2_fop_release,
326 	.read = vb2_fop_read,
327 	.poll = vb2_fop_poll,
328 	.mmap = vb2_fop_mmap,
329 	.unlocked_ioctl = video_ioctl2,
330 };
331 
332 /*
333  * vidioc ioctls
334  */
335 static int vidioc_querycap(struct file *file,
336 		void *priv, struct v4l2_capability *cap)
337 {
338 	struct stk1160 *dev = video_drvdata(file);
339 
340 	strscpy(cap->driver, "stk1160", sizeof(cap->driver));
341 	strscpy(cap->card, "stk1160", sizeof(cap->card));
342 	usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
343 	return 0;
344 }
345 
346 static int vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
347 		struct v4l2_fmtdesc *f)
348 {
349 	if (f->index != 0)
350 		return -EINVAL;
351 
352 	f->pixelformat = format[f->index].fourcc;
353 	return 0;
354 }
355 
356 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
357 					struct v4l2_format *f)
358 {
359 	struct stk1160 *dev = video_drvdata(file);
360 
361 	f->fmt.pix.width = dev->width;
362 	f->fmt.pix.height = dev->height;
363 	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
364 	f->fmt.pix.pixelformat = dev->fmt->fourcc;
365 	f->fmt.pix.bytesperline = dev->width * 2;
366 	f->fmt.pix.sizeimage = dev->height * f->fmt.pix.bytesperline;
367 	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
368 
369 	return 0;
370 }
371 
372 static int stk1160_try_fmt(struct stk1160 *dev, struct v4l2_format *f,
373 			    struct stk1160_decimate_ctrl *ctrl)
374 {
375 	unsigned int width, height;
376 	unsigned int base_width, base_height;
377 	unsigned int col_n, row_n;
378 	enum stk1160_decimate_mode col_mode, row_mode;
379 	bool col_en, row_en;
380 
381 	base_width = 720;
382 	base_height = (dev->norm & V4L2_STD_525_60) ? 480 : 576;
383 
384 	/* Minimum width and height is 5% the frame size */
385 	width = clamp_t(unsigned int, f->fmt.pix.width,
386 			base_width / 20, base_width);
387 	height = clamp_t(unsigned int, f->fmt.pix.height,
388 			base_height / 20, base_height);
389 
390 	/* Let's set default no decimation values */
391 	col_n = 0;
392 	row_n = 0;
393 	col_en = false;
394 	row_en = false;
395 	f->fmt.pix.width = base_width;
396 	f->fmt.pix.height = base_height;
397 	row_mode = STK1160_DECIMATE_LESS_THAN_HALF;
398 	col_mode = STK1160_DECIMATE_LESS_THAN_HALF;
399 
400 	if (width < base_width && width > base_width / 2) {
401 		/*
402 		 * The device will send count units for each
403 		 * unit skipped. This means count unit is:
404 		 *
405 		 * n = width / (frame width - width)
406 		 *
407 		 * And the width is:
408 		 *
409 		 * width = (n / n + 1) * frame width
410 		 */
411 		col_n = div_round_integer(width, base_width - width);
412 		if (col_n > 0 && col_n <= 255) {
413 			col_en = true;
414 			col_mode = STK1160_DECIMATE_LESS_THAN_HALF;
415 			f->fmt.pix.width = (base_width * col_n) / (col_n + 1);
416 		}
417 
418 	} else if (width <= base_width / 2) {
419 
420 		/*
421 		 * The device will skip count units for each
422 		 * unit sent. This means count is:
423 		 *
424 		 * n = (frame width / width) - 1
425 		 *
426 		 * And the width is:
427 		 *
428 		 * width = frame width / (n + 1)
429 		 */
430 		col_n = div_round_integer(base_width, width) - 1;
431 		if (col_n > 0 && col_n <= 255) {
432 			col_en = true;
433 			col_mode = STK1160_DECIMATE_MORE_THAN_HALF;
434 			f->fmt.pix.width = base_width / (col_n + 1);
435 		}
436 	}
437 
438 	if (height < base_height && height > base_height / 2) {
439 		row_n = div_round_integer(height, base_height - height);
440 		if (row_n > 0 && row_n <= 255) {
441 			row_en = true;
442 			row_mode = STK1160_DECIMATE_LESS_THAN_HALF;
443 			f->fmt.pix.height = (base_height * row_n) / (row_n + 1);
444 		}
445 
446 	} else if (height <= base_height / 2) {
447 		row_n = div_round_integer(base_height, height) - 1;
448 		if (row_n > 0 && row_n <= 255) {
449 			row_en = true;
450 			row_mode = STK1160_DECIMATE_MORE_THAN_HALF;
451 			f->fmt.pix.height = base_height / (row_n + 1);
452 		}
453 	}
454 
455 	f->fmt.pix.pixelformat = dev->fmt->fourcc;
456 	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
457 	f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
458 	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
459 	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
460 
461 	if (ctrl) {
462 		ctrl->col_en = col_en;
463 		ctrl->col_n = col_n;
464 		ctrl->col_mode = col_mode;
465 		ctrl->row_en = row_en;
466 		ctrl->row_n = row_n;
467 		ctrl->row_mode = row_mode;
468 	}
469 
470 	stk1160_dbg("width %d, height %d\n",
471 		    f->fmt.pix.width, f->fmt.pix.height);
472 	return 0;
473 }
474 
475 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
476 				  struct v4l2_format *f)
477 {
478 	struct stk1160 *dev = video_drvdata(file);
479 
480 	return stk1160_try_fmt(dev, f, NULL);
481 }
482 
483 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
484 					struct v4l2_format *f)
485 {
486 	struct stk1160 *dev = video_drvdata(file);
487 	struct vb2_queue *q = &dev->vb_vidq;
488 	struct stk1160_decimate_ctrl ctrl;
489 	int rc;
490 
491 	if (vb2_is_busy(q))
492 		return -EBUSY;
493 
494 	rc = stk1160_try_fmt(dev, f, &ctrl);
495 	if (rc < 0)
496 		return rc;
497 	dev->width = f->fmt.pix.width;
498 	dev->height = f->fmt.pix.height;
499 	stk1160_set_fmt(dev, &ctrl);
500 
501 	return 0;
502 }
503 
504 static int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *norm)
505 {
506 	struct stk1160 *dev = video_drvdata(file);
507 	v4l2_device_call_all(&dev->v4l2_dev, 0, video, querystd, norm);
508 	return 0;
509 }
510 
511 static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *norm)
512 {
513 	struct stk1160 *dev = video_drvdata(file);
514 
515 	*norm = dev->norm;
516 	return 0;
517 }
518 
519 static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
520 {
521 	struct stk1160 *dev = video_drvdata(file);
522 	struct vb2_queue *q = &dev->vb_vidq;
523 
524 	if (dev->norm == norm)
525 		return 0;
526 
527 	if (vb2_is_busy(q))
528 		return -EBUSY;
529 
530 	/* Check device presence */
531 	if (!dev->udev)
532 		return -ENODEV;
533 
534 	/* We need to set this now, before we call stk1160_set_std */
535 	dev->width = 720;
536 	dev->height = (norm & V4L2_STD_525_60) ? 480 : 576;
537 	dev->norm = norm;
538 
539 	stk1160_set_std(dev);
540 
541 	/* Calling with NULL disables frame decimation */
542 	stk1160_set_fmt(dev, NULL);
543 
544 	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_std,
545 			dev->norm);
546 
547 	return 0;
548 }
549 
550 
551 static int vidioc_enum_input(struct file *file, void *priv,
552 				struct v4l2_input *i)
553 {
554 	struct stk1160 *dev = video_drvdata(file);
555 
556 	if (i->index > STK1160_MAX_INPUT)
557 		return -EINVAL;
558 
559 	/* S-Video special handling */
560 	if (i->index == STK1160_SVIDEO_INPUT)
561 		sprintf(i->name, "S-Video");
562 	else
563 		sprintf(i->name, "Composite%d", i->index);
564 
565 	i->type = V4L2_INPUT_TYPE_CAMERA;
566 	i->std = dev->vdev.tvnorms;
567 	return 0;
568 }
569 
570 static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
571 {
572 	struct stk1160 *dev = video_drvdata(file);
573 	*i = dev->ctl_input;
574 	return 0;
575 }
576 
577 static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
578 {
579 	struct stk1160 *dev = video_drvdata(file);
580 
581 	if (i > STK1160_MAX_INPUT)
582 		return -EINVAL;
583 
584 	dev->ctl_input = i;
585 
586 	stk1160_select_input(dev);
587 
588 	return 0;
589 }
590 
591 #ifdef CONFIG_VIDEO_ADV_DEBUG
592 static int vidioc_g_register(struct file *file, void *priv,
593 			     struct v4l2_dbg_register *reg)
594 {
595 	struct stk1160 *dev = video_drvdata(file);
596 	int rc;
597 	u8 val;
598 
599 	/* Match host */
600 	rc = stk1160_read_reg(dev, reg->reg, &val);
601 	reg->val = val;
602 	reg->size = 1;
603 
604 	return rc;
605 }
606 
607 static int vidioc_s_register(struct file *file, void *priv,
608 			     const struct v4l2_dbg_register *reg)
609 {
610 	struct stk1160 *dev = video_drvdata(file);
611 
612 	/* Match host */
613 	return stk1160_write_reg(dev, reg->reg, reg->val);
614 }
615 #endif
616 
617 static const struct v4l2_ioctl_ops stk1160_ioctl_ops = {
618 	.vidioc_querycap      = vidioc_querycap,
619 	.vidioc_enum_fmt_vid_cap  = vidioc_enum_fmt_vid_cap,
620 	.vidioc_g_fmt_vid_cap     = vidioc_g_fmt_vid_cap,
621 	.vidioc_try_fmt_vid_cap   = vidioc_try_fmt_vid_cap,
622 	.vidioc_s_fmt_vid_cap     = vidioc_s_fmt_vid_cap,
623 	.vidioc_querystd      = vidioc_querystd,
624 	.vidioc_g_std         = vidioc_g_std,
625 	.vidioc_s_std         = vidioc_s_std,
626 	.vidioc_enum_input    = vidioc_enum_input,
627 	.vidioc_g_input       = vidioc_g_input,
628 	.vidioc_s_input       = vidioc_s_input,
629 
630 	/* vb2 takes care of these */
631 	.vidioc_reqbufs       = vb2_ioctl_reqbufs,
632 	.vidioc_querybuf      = vb2_ioctl_querybuf,
633 	.vidioc_qbuf          = vb2_ioctl_qbuf,
634 	.vidioc_dqbuf         = vb2_ioctl_dqbuf,
635 	.vidioc_streamon      = vb2_ioctl_streamon,
636 	.vidioc_streamoff     = vb2_ioctl_streamoff,
637 	.vidioc_expbuf        = vb2_ioctl_expbuf,
638 
639 	.vidioc_log_status  = v4l2_ctrl_log_status,
640 	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
641 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
642 
643 #ifdef CONFIG_VIDEO_ADV_DEBUG
644 	.vidioc_g_register = vidioc_g_register,
645 	.vidioc_s_register = vidioc_s_register,
646 #endif
647 };
648 
649 /********************************************************************/
650 
651 /*
652  * Videobuf2 operations
653  */
654 static int queue_setup(struct vb2_queue *vq,
655 				unsigned int *nbuffers, unsigned int *nplanes,
656 				unsigned int sizes[], struct device *alloc_devs[])
657 {
658 	struct stk1160 *dev = vb2_get_drv_priv(vq);
659 	unsigned long size;
660 
661 	size = dev->width * dev->height * 2;
662 
663 	/*
664 	 * Here we can change the number of buffers being requested.
665 	 * So, we set a minimum and a maximum like this:
666 	 */
667 	*nbuffers = clamp_t(unsigned int, *nbuffers,
668 			STK1160_MIN_VIDEO_BUFFERS, STK1160_MAX_VIDEO_BUFFERS);
669 
670 	if (*nplanes)
671 		return sizes[0] < size ? -EINVAL : 0;
672 
673 	/* This means a packed colorformat */
674 	*nplanes = 1;
675 
676 	sizes[0] = size;
677 
678 	stk1160_dbg("%s: buffer count %d, each %ld bytes\n",
679 		    __func__, *nbuffers, size);
680 
681 	return 0;
682 }
683 
684 static void buffer_queue(struct vb2_buffer *vb)
685 {
686 	unsigned long flags;
687 	struct stk1160 *dev = vb2_get_drv_priv(vb->vb2_queue);
688 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
689 	struct stk1160_buffer *buf =
690 		container_of(vbuf, struct stk1160_buffer, vb);
691 
692 	spin_lock_irqsave(&dev->buf_lock, flags);
693 	if (!dev->udev) {
694 		/*
695 		 * If the device is disconnected return the buffer to userspace
696 		 * directly. The next QBUF call will fail with -ENODEV.
697 		 */
698 		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
699 	} else {
700 
701 		buf->mem = vb2_plane_vaddr(vb, 0);
702 		buf->length = vb2_plane_size(vb, 0);
703 		buf->bytesused = 0;
704 		buf->pos = 0;
705 
706 		/*
707 		 * If buffer length is less from expected then we return
708 		 * the buffer to userspace directly.
709 		 */
710 		if (buf->length < dev->width * dev->height * 2)
711 			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
712 		else
713 			list_add_tail(&buf->list, &dev->avail_bufs);
714 
715 	}
716 	spin_unlock_irqrestore(&dev->buf_lock, flags);
717 }
718 
719 static int start_streaming(struct vb2_queue *vq, unsigned int count)
720 {
721 	struct stk1160 *dev = vb2_get_drv_priv(vq);
722 	return stk1160_start_streaming(dev);
723 }
724 
725 /* abort streaming and wait for last buffer */
726 static void stop_streaming(struct vb2_queue *vq)
727 {
728 	struct stk1160 *dev = vb2_get_drv_priv(vq);
729 	stk1160_stop_streaming(dev);
730 }
731 
732 static const struct vb2_ops stk1160_video_qops = {
733 	.queue_setup		= queue_setup,
734 	.buf_queue		= buffer_queue,
735 	.start_streaming	= start_streaming,
736 	.stop_streaming		= stop_streaming,
737 	.wait_prepare		= vb2_ops_wait_prepare,
738 	.wait_finish		= vb2_ops_wait_finish,
739 };
740 
741 static const struct video_device v4l_template = {
742 	.name = "stk1160",
743 	.tvnorms = V4L2_STD_525_60 | V4L2_STD_625_50,
744 	.fops = &stk1160_fops,
745 	.ioctl_ops = &stk1160_ioctl_ops,
746 	.release = video_device_release_empty,
747 };
748 
749 /********************************************************************/
750 
751 /* Must be called with both v4l_lock and vb_queue_lock hold */
752 void stk1160_clear_queue(struct stk1160 *dev, enum vb2_buffer_state vb2_state)
753 {
754 	struct stk1160_buffer *buf;
755 	unsigned long flags;
756 
757 	/* Release all active buffers */
758 	spin_lock_irqsave(&dev->buf_lock, flags);
759 	while (!list_empty(&dev->avail_bufs)) {
760 		buf = list_first_entry(&dev->avail_bufs,
761 			struct stk1160_buffer, list);
762 		list_del(&buf->list);
763 		vb2_buffer_done(&buf->vb.vb2_buf, vb2_state);
764 		stk1160_dbg("buffer [%p/%d] aborted\n",
765 			    buf, buf->vb.vb2_buf.index);
766 	}
767 
768 	/* It's important to release the current buffer */
769 	if (dev->isoc_ctl.buf) {
770 		buf = dev->isoc_ctl.buf;
771 		dev->isoc_ctl.buf = NULL;
772 
773 		vb2_buffer_done(&buf->vb.vb2_buf, vb2_state);
774 		stk1160_dbg("buffer [%p/%d] aborted\n",
775 			    buf, buf->vb.vb2_buf.index);
776 	}
777 	spin_unlock_irqrestore(&dev->buf_lock, flags);
778 }
779 
780 int stk1160_vb2_setup(struct stk1160 *dev)
781 {
782 	int rc;
783 	struct vb2_queue *q;
784 
785 	q = &dev->vb_vidq;
786 	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
787 	q->io_modes = VB2_READ | VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
788 	q->drv_priv = dev;
789 	q->buf_struct_size = sizeof(struct stk1160_buffer);
790 	q->ops = &stk1160_video_qops;
791 	q->mem_ops = &vb2_vmalloc_memops;
792 	q->lock = &dev->vb_queue_lock;
793 	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
794 
795 	rc = vb2_queue_init(q);
796 	if (rc < 0)
797 		return rc;
798 
799 	/* initialize video dma queue */
800 	INIT_LIST_HEAD(&dev->avail_bufs);
801 
802 	return 0;
803 }
804 
805 int stk1160_video_register(struct stk1160 *dev)
806 {
807 	int rc;
808 
809 	/* Initialize video_device with a template structure */
810 	dev->vdev = v4l_template;
811 	dev->vdev.queue = &dev->vb_vidq;
812 
813 	/*
814 	 * Provide mutexes for v4l2 core and for videobuf2 queue.
815 	 * It will be used to protect *only* v4l2 ioctls.
816 	 */
817 	dev->vdev.lock = &dev->v4l_lock;
818 
819 	/* This will be used to set video_device parent */
820 	dev->vdev.v4l2_dev = &dev->v4l2_dev;
821 	dev->vdev.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
822 				V4L2_CAP_READWRITE;
823 
824 	/* NTSC is default */
825 	dev->norm = V4L2_STD_NTSC_M;
826 	dev->width = 720;
827 	dev->height = 480;
828 
829 	/* set default format */
830 	dev->fmt = &format[0];
831 	stk1160_set_std(dev);
832 
833 	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_std,
834 			dev->norm);
835 
836 	video_set_drvdata(&dev->vdev, dev);
837 	rc = video_register_device(&dev->vdev, VFL_TYPE_VIDEO, -1);
838 	if (rc < 0) {
839 		stk1160_err("video_register_device failed (%d)\n", rc);
840 		return rc;
841 	}
842 
843 	v4l2_info(&dev->v4l2_dev, "V4L2 device registered as %s\n",
844 		  video_device_node_name(&dev->vdev));
845 
846 	return 0;
847 }
848