From edf8977b62ae86859cccf3d0ea4883333ea4d138 Mon Sep 17 00:00:00 2001
From: Dale Hamel <dale.hamel@srvthe.net>
Date: Tue, 24 Feb 2015 10:31:04 -0500
Subject: [PATCH] Add framescaling support to stk1160

Implements frame scaling support for stk1160 to support format changes instead of a static frame size.

This is effectively a dumb sampling, and could perhaps benefit from being an averaging instead.

This was a requested "TO DO" for this driver, and allows support for userspace programs like Hyperion and Boblight.

Submitted on behalf of the original author, Michael Stegemann <endarno@gmail.com>
---
 drivers/media/usb/stk1160/stk1160-core.c  |   1 +
 drivers/media/usb/stk1160/stk1160-reg.h   |  26 +++++++
 drivers/media/usb/stk1160/stk1160-v4l.c   | 112 +++++++++++++++++++++++-------
 drivers/media/usb/stk1160/stk1160-video.c |  17 ++++-
 drivers/media/usb/stk1160/stk1160.h       |   1 +
 5 files changed, 130 insertions(+), 27 deletions(-)

diff --git a/drivers/media/usb/stk1160/stk1160-core.c b/drivers/media/usb/stk1160/stk1160-core.c
index 03504dc..1881770 100644
--- a/drivers/media/usb/stk1160/stk1160-core.c
+++ b/drivers/media/usb/stk1160/stk1160-core.c
@@ -418,6 +418,7 @@ static void stk1160_disconnect(struct usb_interface *interface)
 	stk1160_ac97_unregister(dev);
 
 	stk1160_clear_queue(dev);
+	vb2_queue_release(&dev->vb_vidq);
 
 	video_unregister_device(&dev->vdev);
 	v4l2_device_disconnect(&dev->v4l2_dev);
diff --git a/drivers/media/usb/stk1160/stk1160-reg.h b/drivers/media/usb/stk1160/stk1160-reg.h
index 3e49da6..b1fa11d 100644
--- a/drivers/media/usb/stk1160/stk1160-reg.h
+++ b/drivers/media/usb/stk1160/stk1160-reg.h
@@ -33,6 +33,32 @@
  */
 #define STK1160_DCTRL			0x100
 
+/*
+ * Decimation Control Register:
+ * Byte 104:
+ * Horizontal Decimation Line Unit Count
+ * Byte 105:
+ * Vertical   Decimation Line Unit Count
+ * Byte 106:
+ * Bit 0 - Horizontal Decimation Control
+ *	 0 Horizontal decimation is disabled.
+ *	 1 Horizontal decimation is enabled.
+ * Bit 1 - Decimates Half or More Column
+ *   0 Decimates less than half from original column,
+ *     -> send count unit (0x105) before each unit skipped.
+ *   1 Decimates half or more from original column,
+ *     -> skip count unit (0x105) before each unit sent.
+ * Bit 2 - Vertical Decimation Control
+ *	 see Bit 0, only vertical
+ * Bit 3 - Vertical Greater or Equal to Half
+ *	 see Bit 1, only vertical
+ * Bit 4 - Decimation Unit
+ *	0 Decimation will work with 2 rows or columns per unit.
+ *	1 Decimation will work with 4 rows or columns per unit.
+ */
+#define STK1160_DMCTRL			0x104
+
+
 /* Capture Frame Start Position */
 #define STK116_CFSPO			0x110
 #define STK116_CFSPO_STX_L		0x110
diff --git a/drivers/media/usb/stk1160/stk1160-v4l.c b/drivers/media/usb/stk1160/stk1160-v4l.c
index 65a326c..94dafeb 100644
--- a/drivers/media/usb/stk1160/stk1160-v4l.c
+++ b/drivers/media/usb/stk1160/stk1160-v4l.c
@@ -106,6 +106,76 @@ static void stk1160_set_std(struct stk1160 *dev)
 
 }
 
+static void stk1160_try_fmt(struct stk1160 *dev, struct v4l2_format *f, bool try)
+{
+	int base_width, base_height;
+
+	if (dev->norm & V4L2_STD_525_60){
+		base_width = 720;
+		base_height = 480;
+	} else {
+		base_width = 720;
+		base_height = 576;
+	}
+
+	if (f->fmt.pix.width <= (base_width / 3)
+	    || f->fmt.pix.height <= (base_height / 3)){
+		f->fmt.pix.width = base_width / 3;
+		f->fmt.pix.height = base_height / 3;
+		if (!try){
+			dev->decimate = 3;
+		}
+	} else if ((f->fmt.pix.width >= base_width >> 1
+			 && f->fmt.pix.width < base_width)
+			 ||((f->fmt.pix.height >= base_height >> 1
+			 && f->fmt.pix.height < base_height))){
+		f->fmt.pix.width = base_width >> 1;
+		f->fmt.pix.height = base_height >> 1;
+		if (!try){
+			dev->decimate = 2;
+		}
+	} else {
+		f->fmt.pix.width = base_width;
+		f->fmt.pix.height = base_height;
+		if (!try){
+			dev->decimate = 0;
+		}
+	}
+}
+
+static void stk1160_set_fmt(struct stk1160 *dev)
+{
+	if (dev->norm & V4L2_STD_525_60){
+		dev->width = 720;
+		dev->height = 480;
+	} else {
+		dev->width = 720;
+		dev->height = 576;
+	}
+
+	switch (dev->decimate){
+		case 0:
+			stk1160_write_reg(dev, STK1160_DMCTRL,   0x00);
+			stk1160_write_reg(dev, STK1160_DMCTRL+1, 0x00);
+			stk1160_write_reg(dev, STK1160_DMCTRL+2, 0x00);
+			break;
+		case 2:
+			stk1160_write_reg(dev, STK1160_DMCTRL,   0x01);
+			stk1160_write_reg(dev, STK1160_DMCTRL+1, 0x01);
+			stk1160_write_reg(dev, STK1160_DMCTRL+2, 0x1f);
+			dev->width = dev->width >> 1;
+			dev->height = dev->height >> 1;
+			break;
+		case 3:
+			stk1160_write_reg(dev, STK1160_DMCTRL,   0x02);
+			stk1160_write_reg(dev, STK1160_DMCTRL+1, 0x02);
+			stk1160_write_reg(dev, STK1160_DMCTRL+2, 0x1f);
+			dev->width = dev->width / 3;
+			dev->height = dev->height / 3;
+			break;
+	}
+}
+
 /*
  * Set a new alternate setting.
  * Returns true is dev->max_pkt_size has changed, false otherwise.
@@ -321,17 +391,10 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 {
 	struct stk1160 *dev = video_drvdata(file);
 
-	/*
-	 * User can't choose size at his own will,
-	 * so we just return him the current size chosen
-	 * at standard selection.
-	 * TODO: Implement frame scaling?
-	 */
+	stk1160_try_fmt(dev, f, true);
 
-	f->fmt.pix.pixelformat = dev->fmt->fourcc;
-	f->fmt.pix.width = dev->width;
-	f->fmt.pix.height = dev->height;
 	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	f->fmt.pix.pixelformat = dev->fmt->fourcc;
 	f->fmt.pix.bytesperline = dev->width * 2;
 	f->fmt.pix.sizeimage = dev->height * f->fmt.pix.bytesperline;
 	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
@@ -348,9 +411,14 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	if (vb2_is_busy(q))
 		return -EBUSY;
 
-	vidioc_try_fmt_vid_cap(file, priv, f);
+	stk1160_try_fmt(dev, f, false);
+	stk1160_set_fmt(dev);
 
-	/* We don't support any format changes */
+	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	f->fmt.pix.pixelformat = dev->fmt->fourcc;
+	f->fmt.pix.bytesperline = dev->width * 2;
+	f->fmt.pix.sizeimage = dev->height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 
 	return 0;
 }
@@ -389,23 +457,18 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 	dev->norm = norm;
 
 	/* This is taken from saa7115 video decoder */
-	if (dev->norm & V4L2_STD_525_60) {
-		dev->width = 720;
-		dev->height = 480;
-	} else if (dev->norm & V4L2_STD_625_50) {
-		dev->width = 720;
-		dev->height = 576;
+	if (dev->norm & V4L2_STD_525_60 || dev->norm & V4L2_STD_625_50) {
+		stk1160_set_std(dev);
+		stk1160_set_fmt(dev);
+
+		v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_std,
+				dev->norm);
+
+		return 0;
 	} else {
 		stk1160_err("invalid standard\n");
 		return -EINVAL;
 	}
-
-	stk1160_set_std(dev);
-
-	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_std,
-			dev->norm);
-
-	return 0;
 }
 
 
@@ -671,6 +734,7 @@ int stk1160_video_register(struct stk1160 *dev)
 	dev->norm = V4L2_STD_NTSC_M;
 	dev->width = 720;
 	dev->height = 480;
+	dev->decimate = 0;
 
 	/* set default format */
 	dev->fmt = &format[0];
diff --git a/drivers/media/usb/stk1160/stk1160-video.c b/drivers/media/usb/stk1160/stk1160-video.c
index 39f1aae..5a55028 100644
--- a/drivers/media/usb/stk1160/stk1160-video.c
+++ b/drivers/media/usb/stk1160/stk1160-video.c
@@ -84,7 +84,6 @@ struct stk1160_buffer *stk1160_next_buffer(struct stk1160 *dev)
 	if (!list_empty(&dev->avail_bufs)) {
 		buf = list_first_entry(&dev->avail_bufs,
 				struct stk1160_buffer, list);
-		list_del(&buf->list);
 	}
 	spin_unlock_irqrestore(&dev->buf_lock, flags);
 
@@ -95,7 +94,7 @@ static inline
 void stk1160_buffer_done(struct stk1160 *dev)
 {
 	struct stk1160_buffer *buf = dev->isoc_ctl.buf;
-
+	unsigned long flags = 0;
 	dev->field_count++;
 
 	buf->vb.v4l2_buf.sequence = dev->field_count >> 1;
@@ -104,7 +103,19 @@ void stk1160_buffer_done(struct stk1160 *dev)
 	v4l2_get_timestamp(&buf->vb.v4l2_buf.timestamp);
 
 	vb2_set_plane_payload(&buf->vb, 0, buf->bytesused);
-	vb2_buffer_done(&buf->vb, VB2_BUF_STATE_DONE);
+
+	spin_lock_irqsave(&dev->buf_lock, flags);
+	if (buf->bytesused == vb2_plane_size(&buf->vb, 0))
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_DONE);
+	else
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+
+	if (!list_empty(&dev->avail_bufs)) {
+		list_del(&buf->list);
+	}
+
+	spin_unlock_irqrestore(&dev->buf_lock, flags);
+
 
 	dev->isoc_ctl.buf = NULL;
 }
diff --git a/drivers/media/usb/stk1160/stk1160.h b/drivers/media/usb/stk1160/stk1160.h
index abdea48..895ed35 100644
--- a/drivers/media/usb/stk1160/stk1160.h
+++ b/drivers/media/usb/stk1160/stk1160.h
@@ -150,6 +150,7 @@ struct stk1160 {
 	unsigned int ctl_input;	  /* selected input */
 	v4l2_std_id norm;	  /* current norm */
 	struct stk1160_fmt *fmt;  /* selected format */
+	int decimate;
 
 	unsigned int field_count; /* not sure ??? */
 	enum v4l2_field field;    /* also not sure :/ */
-- 
2.1.1

