stk1160-v4l.c

  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
 31static bool keep_buffers;
 32module_param(keep_buffers, bool, 0644);
 33MODULE_PARM_DESC(keep_buffers, "don't release buffers upon stop streaming");
 34
 35enum stk1160_decimate_mode {
 36	STK1160_DECIMATE_MORE_THAN_HALF,
 37	STK1160_DECIMATE_LESS_THAN_HALF,
 38};
 39
 40struct 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 */
 47static 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 */
 58static unsigned int
 59div_round_integer(unsigned int x, unsigned int y)
 60{
 61	for (;; y++) {
 62		if (x % y == 0)
 63			return x / y;
 64	}
 65}
 66
 67static 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
122static 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 */
161static 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
203static 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		rc = usb_submit_urb(dev->isoc_ctl.urb_ctl[i].urb, GFP_KERNEL);
236		if (rc) {
237			stk1160_err("cannot submit urb[%d] (%d)\n", i, rc);
238			goto out_uninit;
239		}
240	}
241
242	/* Start saa711x */
243	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_stream, 1);
244
245	dev->sequence = 0;
246
247	/* Start stk1160 */
248	stk1160_write_reg(dev, STK1160_DCTRL, 0xb3);
249	stk1160_write_reg(dev, STK1160_DCTRL+3, 0x00);
250
251	stk1160_dbg("streaming started\n");
252
253	mutex_unlock(&dev->v4l_lock);
254
255	return 0;
256
257out_uninit:
258	stk1160_uninit_isoc(dev);
259out_stop_hw:
260	usb_set_interface(dev->udev, 0, 0);
261	stk1160_clear_queue(dev, VB2_BUF_STATE_QUEUED);
262
263	mutex_unlock(&dev->v4l_lock);
264
265	return rc;
266}
267
268/* Must be called with v4l_lock hold */
269static void stk1160_stop_hw(struct stk1160 *dev)
270{
271	/* If the device is not physically present, there is nothing to do */
272	if (!dev->udev)
273		return;
274
275	/* set alternate 0 */
276	dev->alt = 0;
277	stk1160_dbg("setting alternate %d\n", dev->alt);
278	usb_set_interface(dev->udev, 0, 0);
279
280	/* Stop stk1160 */
281	stk1160_write_reg(dev, STK1160_DCTRL, 0x00);
282	stk1160_write_reg(dev, STK1160_DCTRL+3, 0x00);
283
284	/* Stop saa711x */
285	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_stream, 0);
286}
287
288static int stk1160_stop_streaming(struct stk1160 *dev)
289{
290	if (mutex_lock_interruptible(&dev->v4l_lock))
291		return -ERESTARTSYS;
292
293	/*
294	 * Once URBs are cancelled, the URB complete handler
295	 * won't be running. This is required to safely release the
296	 * current buffer (dev->isoc_ctl.buf).
297	 */
298	stk1160_cancel_isoc(dev);
299
300	/*
301	 * It is possible to keep buffers around using a module parameter.
302	 * This is intended to avoid memory fragmentation.
303	 */
304	if (!keep_buffers)
305		stk1160_free_isoc(dev);
306
307	stk1160_stop_hw(dev);
308
309	stk1160_clear_queue(dev, VB2_BUF_STATE_ERROR);
310
311	stk1160_dbg("streaming stopped\n");
312
313	mutex_unlock(&dev->v4l_lock);
314
315	return 0;
316}
317
318static const struct v4l2_file_operations stk1160_fops = {
319	.owner = THIS_MODULE,
320	.open = v4l2_fh_open,
321	.release = vb2_fop_release,
322	.read = vb2_fop_read,
323	.poll = vb2_fop_poll,
324	.mmap = vb2_fop_mmap,
325	.unlocked_ioctl = video_ioctl2,
326};
327
328/*
329 * vidioc ioctls
330 */
331static int vidioc_querycap(struct file *file,
332		void *priv, struct v4l2_capability *cap)
333{
334	struct stk1160 *dev = video_drvdata(file);
335
336	strscpy(cap->driver, "stk1160", sizeof(cap->driver));
337	strscpy(cap->card, "stk1160", sizeof(cap->card));
338	usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
339	return 0;
340}
341
342static int vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
343		struct v4l2_fmtdesc *f)
344{
345	if (f->index != 0)
346		return -EINVAL;
347
348	f->pixelformat = format[f->index].fourcc;
349	return 0;
350}
351
352static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
353					struct v4l2_format *f)
354{
355	struct stk1160 *dev = video_drvdata(file);
356
357	f->fmt.pix.width = dev->width;
358	f->fmt.pix.height = dev->height;
359	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
360	f->fmt.pix.pixelformat = dev->fmt->fourcc;
361	f->fmt.pix.bytesperline = dev->width * 2;
362	f->fmt.pix.sizeimage = dev->height * f->fmt.pix.bytesperline;
363	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
364
365	return 0;
366}
367
368static int stk1160_try_fmt(struct stk1160 *dev, struct v4l2_format *f,
369			    struct stk1160_decimate_ctrl *ctrl)
370{
371	unsigned int width, height;
372	unsigned int base_width, base_height;
373	unsigned int col_n, row_n;
374	enum stk1160_decimate_mode col_mode, row_mode;
375	bool col_en, row_en;
376
377	base_width = 720;
378	base_height = (dev->norm & V4L2_STD_525_60) ? 480 : 576;
379
380	/* Minimum width and height is 5% the frame size */
381	width = clamp_t(unsigned int, f->fmt.pix.width,
382			base_width / 20, base_width);
383	height = clamp_t(unsigned int, f->fmt.pix.height,
384			base_height / 20, base_height);
385
386	/* Let's set default no decimation values */
387	col_n = 0;
388	row_n = 0;
389	col_en = false;
390	row_en = false;
391	f->fmt.pix.width = base_width;
392	f->fmt.pix.height = base_height;
393	row_mode = STK1160_DECIMATE_LESS_THAN_HALF;
394	col_mode = STK1160_DECIMATE_LESS_THAN_HALF;
395
396	if (width < base_width && width > base_width / 2) {
397		/*
398		 * The device will send count units for each
399		 * unit skipped. This means count unit is:
400		 *
401		 * n = width / (frame width - width)
402		 *
403		 * And the width is:
404		 *
405		 * width = (n / n + 1) * frame width
406		 */
407		col_n = div_round_integer(width, base_width - width);
408		if (col_n > 0 && col_n <= 255) {
409			col_en = true;
410			col_mode = STK1160_DECIMATE_LESS_THAN_HALF;
411			f->fmt.pix.width = (base_width * col_n) / (col_n + 1);
412		}
413
414	} else if (width <= base_width / 2) {
415
416		/*
417		 * The device will skip count units for each
418		 * unit sent. This means count is:
419		 *
420		 * n = (frame width / width) - 1
421		 *
422		 * And the width is:
423		 *
424		 * width = frame width / (n + 1)
425		 */
426		col_n = div_round_integer(base_width, width) - 1;
427		if (col_n > 0 && col_n <= 255) {
428			col_en = true;
429			col_mode = STK1160_DECIMATE_MORE_THAN_HALF;
430			f->fmt.pix.width = base_width / (col_n + 1);
431		}
432	}
433
434	if (height < base_height && height > base_height / 2) {
435		row_n = div_round_integer(height, base_height - height);
436		if (row_n > 0 && row_n <= 255) {
437			row_en = true;
438			row_mode = STK1160_DECIMATE_LESS_THAN_HALF;
439			f->fmt.pix.height = (base_height * row_n) / (row_n + 1);
440		}
441
442	} else if (height <= base_height / 2) {
443		row_n = div_round_integer(base_height, height) - 1;
444		if (row_n > 0 && row_n <= 255) {
445			row_en = true;
446			row_mode = STK1160_DECIMATE_MORE_THAN_HALF;
447			f->fmt.pix.height = base_height / (row_n + 1);
448		}
449	}
450
451	f->fmt.pix.pixelformat = dev->fmt->fourcc;
452	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
453	f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
454	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
455	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
456
457	if (ctrl) {
458		ctrl->col_en = col_en;
459		ctrl->col_n = col_n;
460		ctrl->col_mode = col_mode;
461		ctrl->row_en = row_en;
462		ctrl->row_n = row_n;
463		ctrl->row_mode = row_mode;
464	}
465
466	stk1160_dbg("width %d, height %d\n",
467		    f->fmt.pix.width, f->fmt.pix.height);
468	return 0;
469}
470
471static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
472				  struct v4l2_format *f)
473{
474	struct stk1160 *dev = video_drvdata(file);
475
476	return stk1160_try_fmt(dev, f, NULL);
477}
478
479static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
480					struct v4l2_format *f)
481{
482	struct stk1160 *dev = video_drvdata(file);
483	struct vb2_queue *q = &dev->vb_vidq;
484	struct stk1160_decimate_ctrl ctrl;
485	int rc;
486
487	if (vb2_is_busy(q))
488		return -EBUSY;
489
490	rc = stk1160_try_fmt(dev, f, &ctrl);
491	if (rc < 0)
492		return rc;
493	dev->width = f->fmt.pix.width;
494	dev->height = f->fmt.pix.height;
495	stk1160_set_fmt(dev, &ctrl);
496
497	return 0;
498}
499
500static int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *norm)
501{
502	struct stk1160 *dev = video_drvdata(file);
503	v4l2_device_call_all(&dev->v4l2_dev, 0, video, querystd, norm);
504	return 0;
505}
506
507static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *norm)
508{
509	struct stk1160 *dev = video_drvdata(file);
510
511	*norm = dev->norm;
512	return 0;
513}
514
515static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
516{
517	struct stk1160 *dev = video_drvdata(file);
518	struct vb2_queue *q = &dev->vb_vidq;
519
520	if (dev->norm == norm)
521		return 0;
522
523	if (vb2_is_busy(q))
524		return -EBUSY;
525
526	/* Check device presence */
527	if (!dev->udev)
528		return -ENODEV;
529
530	/* We need to set this now, before we call stk1160_set_std */
531	dev->width = 720;
532	dev->height = (norm & V4L2_STD_525_60) ? 480 : 576;
533	dev->norm = norm;
534
535	stk1160_set_std(dev);
536
537	/* Calling with NULL disables frame decimation */
538	stk1160_set_fmt(dev, NULL);
539
540	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_std,
541			dev->norm);
542
543	return 0;
544}
545
546
547static int vidioc_enum_input(struct file *file, void *priv,
548				struct v4l2_input *i)
549{
550	struct stk1160 *dev = video_drvdata(file);
551
552	if (i->index > STK1160_MAX_INPUT)
553		return -EINVAL;
554
555	/* S-Video special handling */
556	if (i->index == STK1160_SVIDEO_INPUT)
557		sprintf(i->name, "S-Video");
558	else
559		sprintf(i->name, "Composite%d", i->index);
560
561	i->type = V4L2_INPUT_TYPE_CAMERA;
562	i->std = dev->vdev.tvnorms;
563	return 0;
564}
565
566static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
567{
568	struct stk1160 *dev = video_drvdata(file);
569	*i = dev->ctl_input;
570	return 0;
571}
572
573static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
574{
575	struct stk1160 *dev = video_drvdata(file);
576
577	if (i > STK1160_MAX_INPUT)
578		return -EINVAL;
579
580	dev->ctl_input = i;
581
582	stk1160_select_input(dev);
583
584	return 0;
585}
586
587#ifdef CONFIG_VIDEO_ADV_DEBUG
588static int vidioc_g_register(struct file *file, void *priv,
589			     struct v4l2_dbg_register *reg)
590{
591	struct stk1160 *dev = video_drvdata(file);
592	int rc;
593	u8 val;
594
595	/* Match host */
596	rc = stk1160_read_reg(dev, reg->reg, &val);
597	reg->val = val;
598	reg->size = 1;
599
600	return rc;
601}
602
603static int vidioc_s_register(struct file *file, void *priv,
604			     const struct v4l2_dbg_register *reg)
605{
606	struct stk1160 *dev = video_drvdata(file);
607
608	/* Match host */
609	return stk1160_write_reg(dev, reg->reg, reg->val);
610}
611#endif
612
613static const struct v4l2_ioctl_ops stk1160_ioctl_ops = {
614	.vidioc_querycap      = vidioc_querycap,
615	.vidioc_enum_fmt_vid_cap  = vidioc_enum_fmt_vid_cap,
616	.vidioc_g_fmt_vid_cap     = vidioc_g_fmt_vid_cap,
617	.vidioc_try_fmt_vid_cap   = vidioc_try_fmt_vid_cap,
618	.vidioc_s_fmt_vid_cap     = vidioc_s_fmt_vid_cap,
619	.vidioc_querystd      = vidioc_querystd,
620	.vidioc_g_std         = vidioc_g_std,
621	.vidioc_s_std         = vidioc_s_std,
622	.vidioc_enum_input    = vidioc_enum_input,
623	.vidioc_g_input       = vidioc_g_input,
624	.vidioc_s_input       = vidioc_s_input,
625
626	/* vb2 takes care of these */
627	.vidioc_reqbufs       = vb2_ioctl_reqbufs,
628	.vidioc_querybuf      = vb2_ioctl_querybuf,
629	.vidioc_qbuf          = vb2_ioctl_qbuf,
630	.vidioc_dqbuf         = vb2_ioctl_dqbuf,
631	.vidioc_streamon      = vb2_ioctl_streamon,
632	.vidioc_streamoff     = vb2_ioctl_streamoff,
633	.vidioc_expbuf        = vb2_ioctl_expbuf,
634
635	.vidioc_log_status  = v4l2_ctrl_log_status,
636	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
637	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
638
639#ifdef CONFIG_VIDEO_ADV_DEBUG
640	.vidioc_g_register = vidioc_g_register,
641	.vidioc_s_register = vidioc_s_register,
642#endif
643};
644
645/********************************************************************/
646
647/*
648 * Videobuf2 operations
649 */
650static int queue_setup(struct vb2_queue *vq,
651				unsigned int *nbuffers, unsigned int *nplanes,
652				unsigned int sizes[], struct device *alloc_devs[])
653{
654	struct stk1160 *dev = vb2_get_drv_priv(vq);
655	unsigned long size;
656
657	size = dev->width * dev->height * 2;
658
659	/*
660	 * Here we can change the number of buffers being requested.
661	 * So, we set a minimum and a maximum like this:
662	 */
663	*nbuffers = clamp_t(unsigned int, *nbuffers,
664			STK1160_MIN_VIDEO_BUFFERS, STK1160_MAX_VIDEO_BUFFERS);
665
666	if (*nplanes)
667		return sizes[0] < size ? -EINVAL : 0;
668
669	/* This means a packed colorformat */
670	*nplanes = 1;
671
672	sizes[0] = size;
673
674	stk1160_dbg("%s: buffer count %d, each %ld bytes\n",
675		    __func__, *nbuffers, size);
676
677	return 0;
678}
679
680static void buffer_queue(struct vb2_buffer *vb)
681{
682	unsigned long flags;
683	struct stk1160 *dev = vb2_get_drv_priv(vb->vb2_queue);
684	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
685	struct stk1160_buffer *buf =
686		container_of(vbuf, struct stk1160_buffer, vb);
687
688	spin_lock_irqsave(&dev->buf_lock, flags);
689	if (!dev->udev) {
690		/*
691		 * If the device is disconnected return the buffer to userspace
692		 * directly. The next QBUF call will fail with -ENODEV.
693		 */
694		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
695	} else {
696
697		buf->mem = vb2_plane_vaddr(vb, 0);
698		buf->length = vb2_plane_size(vb, 0);
699		buf->bytesused = 0;
700		buf->pos = 0;
701
702		/*
703		 * If buffer length is less from expected then we return
704		 * the buffer to userspace directly.
705		 */
706		if (buf->length < dev->width * dev->height * 2)
707			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
708		else
709			list_add_tail(&buf->list, &dev->avail_bufs);
710
711	}
712	spin_unlock_irqrestore(&dev->buf_lock, flags);
713}
714
715static int start_streaming(struct vb2_queue *vq, unsigned int count)
716{
717	struct stk1160 *dev = vb2_get_drv_priv(vq);
718	return stk1160_start_streaming(dev);
719}
720
721/* abort streaming and wait for last buffer */
722static void stop_streaming(struct vb2_queue *vq)
723{
724	struct stk1160 *dev = vb2_get_drv_priv(vq);
725	stk1160_stop_streaming(dev);
726}
727
728static const struct vb2_ops stk1160_video_qops = {
729	.queue_setup		= queue_setup,
730	.buf_queue		= buffer_queue,
731	.start_streaming	= start_streaming,
732	.stop_streaming		= stop_streaming,
733};
734
735static const struct video_device v4l_template = {
736	.name = "stk1160",
737	.tvnorms = V4L2_STD_525_60 | V4L2_STD_625_50,
738	.fops = &stk1160_fops,
739	.ioctl_ops = &stk1160_ioctl_ops,
740	.release = video_device_release_empty,
741};
742
743/********************************************************************/
744
745/* Must be called with both v4l_lock and vb_queue_lock hold */
746void stk1160_clear_queue(struct stk1160 *dev, enum vb2_buffer_state vb2_state)
747{
748	struct stk1160_buffer *buf;
749	unsigned long flags;
750
751	/* Release all active buffers */
752	spin_lock_irqsave(&dev->buf_lock, flags);
753	while (!list_empty(&dev->avail_bufs)) {
754		buf = list_first_entry(&dev->avail_bufs,
755			struct stk1160_buffer, list);
756		list_del(&buf->list);
757		vb2_buffer_done(&buf->vb.vb2_buf, vb2_state);
758		stk1160_dbg("buffer [%p/%d] aborted\n",
759			    buf, buf->vb.vb2_buf.index);
760	}
761
762	/* It's important to release the current buffer */
763	if (dev->isoc_ctl.buf) {
764		buf = dev->isoc_ctl.buf;
765		dev->isoc_ctl.buf = NULL;
766
767		vb2_buffer_done(&buf->vb.vb2_buf, vb2_state);
768		stk1160_dbg("buffer [%p/%d] aborted\n",
769			    buf, buf->vb.vb2_buf.index);
770	}
771	spin_unlock_irqrestore(&dev->buf_lock, flags);
772}
773
774int stk1160_vb2_setup(struct stk1160 *dev)
775{
776	int rc;
777	struct vb2_queue *q;
778
779	q = &dev->vb_vidq;
780	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
781	q->io_modes = VB2_READ | VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
782	q->drv_priv = dev;
783	q->buf_struct_size = sizeof(struct stk1160_buffer);
784	q->ops = &stk1160_video_qops;
785	q->mem_ops = &vb2_vmalloc_memops;
786	q->lock = &dev->vb_queue_lock;
787	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
788
789	rc = vb2_queue_init(q);
790	if (rc < 0)
791		return rc;
792
793	/* initialize video dma queue */
794	INIT_LIST_HEAD(&dev->avail_bufs);
795
796	return 0;
797}
798
799int stk1160_video_register(struct stk1160 *dev)
800{
801	int rc;
802
803	/* Initialize video_device with a template structure */
804	dev->vdev = v4l_template;
805	dev->vdev.queue = &dev->vb_vidq;
806
807	/*
808	 * Provide mutexes for v4l2 core and for videobuf2 queue.
809	 * It will be used to protect *only* v4l2 ioctls.
810	 */
811	dev->vdev.lock = &dev->v4l_lock;
812
813	/* This will be used to set video_device parent */
814	dev->vdev.v4l2_dev = &dev->v4l2_dev;
815	dev->vdev.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
816				V4L2_CAP_READWRITE;
817
818	/* NTSC is default */
819	dev->norm = V4L2_STD_NTSC_M;
820	dev->width = 720;
821	dev->height = 480;
822
823	/* set default format */
824	dev->fmt = &format[0];
825	stk1160_set_std(dev);
826
827	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_std,
828			dev->norm);
829
830	video_set_drvdata(&dev->vdev, dev);
831	rc = video_register_device(&dev->vdev, VFL_TYPE_VIDEO, -1);
832	if (rc < 0) {
833		stk1160_err("video_register_device failed (%d)\n", rc);
834		return rc;
835	}
836
837	v4l2_info(&dev->v4l2_dev, "V4L2 device registered as %s\n",
838		  video_device_node_name(&dev->vdev));
839
840	return 0;
841}