Implement mpeg2 garbage filter in pvrusb2

From: Mike Isely <isely@pobox.com>

The PVR USB2 hardware seems to like emitting some garbage data before
real stream data appears.  This problem has been present all along,
but with the older 29xxx model series it was benign because the
garbage data in question didn't seem to bother mpeg2 players.  But
with the 24xxx model series, the garbage data includes what looks like
a corrupted mpeg2 packet, which kills mythtv and ffmpeg.  This fix
causes incoming stream data to be discarded until valid mpeg2 data is
seen.

Signed-off-by: Mike Isely <isely@pobox.com>

5 files changed, 253 insertions, 75 deletions

diff --git a/linux/drivers/media/video/pvrusb2/pvrusb2-context.c b/linux/drivers/media/video/pvrusb2/pvrusb2-context.c
index 0f0e14d01..f3fd61f21 100644
--- a/linux/drivers/media/video/pvrusb2/pvrusb2-context.c
+++ b/linux/drivers/media/video/pvrusb2/pvrusb2-context.c
@@ -20,6 +20,7 @@
 
 #include "pvrusb2-context.h"
 #include "pvrusb2-io.h"
+#include "pvrusb2-ioread.h"
 #include "pvrusb2-hdw.h"
 #include "pvrusb2-debug.h"
 #include <linux/errno.h>
@@ -203,6 +204,23 @@ int pvr2_channel_claim_stream(struct pvr2_channel *cp,
 }
 
 
+// This is the marker for the real beginning of a legitimate mpeg2 stream.
+static char stream_sync_key[] = {
+	0x00, 0x00, 0x01, 0xba,
+};
+
+struct pvr2_ioread *pvr2_channel_create_mpeg_stream(
+	struct pvr2_context_stream *sp)
+{
+	struct pvr2_ioread *cp;
+	cp = pvr2_ioread_create();
+	if (!cp) return 0;
+	pvr2_ioread_setup(cp,sp->stream);
+	pvr2_ioread_set_sync_key(cp,stream_sync_key,sizeof(stream_sync_key));
+	return cp;
+}
+
+
 /*
   Stuff for Emacs to see, in order to encourage consistent editing style:
   *** Local Variables: ***
diff --git a/linux/drivers/media/video/pvrusb2/pvrusb2-context.h b/linux/drivers/media/video/pvrusb2/pvrusb2-context.h
index 873622a0f..910086ba2 100644
--- a/linux/drivers/media/video/pvrusb2/pvrusb2-context.h
+++ b/linux/drivers/media/video/pvrusb2/pvrusb2-context.h
@@ -37,6 +37,7 @@ struct pvr2_context;        /* All central state */
 struct pvr2_channel;        /* One I/O pathway to a user */
 struct pvr2_context_stream; /* Wrapper for a stream */
 struct pvr2_crit_reg;       /* Critical region pointer */
+struct pvr2_ioread;         /* Low level stream structure */
 
 struct pvr2_context_stream {
 	struct pvr2_channel *user;
@@ -85,6 +86,8 @@ void pvr2_channel_init(struct pvr2_channel *,struct pvr2_context *);
 void pvr2_channel_done(struct pvr2_channel *);
 int pvr2_channel_claim_stream(struct pvr2_channel *,
 			      struct pvr2_context_stream *);
+struct pvr2_ioread *pvr2_channel_create_mpeg_stream(
+	struct pvr2_context_stream *);
 
 
 #endif /* __PVRUSB2_CONTEXT_H */
diff --git a/linux/drivers/media/video/pvrusb2/pvrusb2-ioread.c b/linux/drivers/media/video/pvrusb2/pvrusb2-ioread.c
index 4182d75b7..f81a0c225 100644
--- a/linux/drivers/media/video/pvrusb2/pvrusb2-ioread.c
+++ b/linux/drivers/media/video/pvrusb2/pvrusb2-ioread.c
@@ -39,10 +39,20 @@
 struct pvr2_ioread {
 	struct pvr2_stream *stream;
 	char *buffer_storage[BUFFER_COUNT];
-	int resid_offs;
-	int enabled;
-	int stream_running;
-	int spigot_open;
+	char *sync_key_ptr;
+	unsigned int sync_key_len;
+	unsigned int sync_buf_offs;
+	unsigned int sync_state;
+	unsigned int sync_trashed_count;
+	int enabled;         // Streaming is on
+	int spigot_open;     // OK to pass data to client
+	int stream_running;  // Passing data to client now
+
+	/* State relevant to current buffer being read */
+	struct pvr2_buffer *c_buf;
+	char *c_data_ptr;
+	unsigned int c_data_len;
+	unsigned int c_data_offs;
 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,16)
 	struct mutex mutex;
 #else
@@ -103,19 +113,59 @@ void pvr2_ioread_destroy(struct pvr2_ioread *cp)
 	if (!cp) return;
 	pvr2_ioread_done(cp);
 	pvr2_trace(PVR2_TRACE_STRUCT,"pvr2_ioread_destroy id=%p",cp);
+	if (cp->sync_key_ptr) {
+		kfree(cp->sync_key_ptr);
+		cp->sync_key_ptr = 0;
+	}
 	kfree(cp);
 }
 
+void pvr2_ioread_set_sync_key(struct pvr2_ioread *cp,
+			      const char *sync_key_ptr,
+			      unsigned int sync_key_len)
+{
+	if (!cp) return;
+
+	if (!sync_key_ptr) sync_key_len = 0;
+	if ((sync_key_len == cp->sync_key_len) &&
+	    ((!sync_key_len) ||
+	     (!memcmp(sync_key_ptr,cp->sync_key_ptr,sync_key_len)))) return;
+
+	if (sync_key_len != cp->sync_key_len) {
+		if (cp->sync_key_ptr) {
+			kfree(cp->sync_key_ptr);
+			cp->sync_key_ptr = 0;
+		}
+		cp->sync_key_len = 0;
+		if (sync_key_len) {
+			cp->sync_key_ptr = kmalloc(sync_key_len,GFP_KERNEL);
+			if (cp->sync_key_ptr) {
+				cp->sync_key_len = sync_key_len;
+			}
+		}
+	}
+	if (!cp->sync_key_len) return;
+	memcpy(cp->sync_key_ptr,sync_key_ptr,cp->sync_key_len);
+}
+
 static void pvr2_ioread_stop(struct pvr2_ioread *cp)
 {
 	if (!(cp->enabled)) return;
 	pvr2_trace(PVR2_TRACE_START_STOP,
 		   "/*---TRACE_READ---*/ pvr2_ioread_stop id=%p",cp);
 	pvr2_stream_kill(cp->stream);
-	cp->resid_offs = 0;
+	cp->c_buf = 0;
+	cp->c_data_ptr = 0;
+	cp->c_data_len = 0;
+	cp->c_data_offs = 0;
 	cp->enabled = 0;
 	cp->stream_running = 0;
 	cp->spigot_open = 0;
+	if (cp->sync_state) {
+		pvr2_trace(PVR2_TRACE_DATA_FLOW,
+			   "/*---TRACE_READ---*/ sync_state <== 0");
+		cp->sync_state = 0;
+	}
 }
 
 static int pvr2_ioread_start(struct pvr2_ioread *cp)
@@ -126,7 +176,6 @@ static int pvr2_ioread_start(struct pvr2_ioread *cp)
 	if (!(cp->stream)) return 0;
 	pvr2_trace(PVR2_TRACE_START_STOP,
 		   "/*---TRACE_READ---*/ pvr2_ioread_start id=%p",cp);
-	cp->resid_offs = 0;
 	while ((bp = pvr2_stream_get_idle_buffer(cp->stream)) != 0) {
 		stat = pvr2_buffer_queue(bp);
 		if (stat < 0) {
@@ -140,7 +189,18 @@ static int pvr2_ioread_start(struct pvr2_ioread *cp)
 		}
 	}
 	cp->enabled = !0;
+	cp->c_buf = 0;
+	cp->c_data_ptr = 0;
+	cp->c_data_len = 0;
+	cp->c_data_offs = 0;
 	cp->stream_running = 0;
+	if (cp->sync_key_len) {
+		pvr2_trace(PVR2_TRACE_DATA_FLOW,
+			   "/*---TRACE_READ---*/ sync_state <== 1");
+		cp->sync_state = 1;
+		cp->sync_trashed_count = 0;
+		cp->sync_buf_offs = 0;
+	}
 	cp->spigot_open = 0;
 	return 0;
 }
@@ -206,6 +266,116 @@ int pvr2_ioread_get_enabled(struct pvr2_ioread *cp)
 	return cp->enabled != 0;
 }
 
+int pvr2_ioread_get_buffer(struct pvr2_ioread *cp)
+{
+	int stat;
+
+	while (cp->c_data_len <= cp->c_data_offs) {
+		if (cp->c_buf) {
+			// Flush out current buffer first.
+			stat = pvr2_buffer_queue(cp->c_buf);
+			if (stat < 0) {
+				// Streaming error...
+				pvr2_trace(PVR2_TRACE_DATA_FLOW,
+					   "/*---TRACE_READ---*/"
+					   " pvr2_ioread_read id=%p"
+					   " queue_error=%d",
+					   cp,stat);
+				pvr2_ioread_stop(cp);
+				return 0;
+			}
+			cp->c_buf = 0;
+			cp->c_data_ptr = 0;
+			cp->c_data_len = 0;
+			cp->c_data_offs = 0;
+		}
+		// Now get a freshly filled buffer.
+		cp->c_buf = pvr2_stream_get_ready_buffer(cp->stream);
+		if (!cp->c_buf) break; // Nothing ready; done.
+		cp->c_data_len = pvr2_buffer_get_count(cp->c_buf);
+		if (!cp->c_data_len) {
+			// Nothing transferred.  Was there an error?
+			stat = pvr2_buffer_get_status(cp->c_buf);
+			if (stat < 0) {
+				// Streaming error...
+				pvr2_trace(PVR2_TRACE_DATA_FLOW,
+					   "/*---TRACE_READ---*/"
+					   " pvr2_ioread_read id=%p"
+					   " buffer_error=%d",
+					   cp,stat);
+				pvr2_ioread_stop(cp);
+				// Give up.
+				return 0;
+			}
+			// Start over...
+			continue;
+		}
+		cp->c_data_offs = 0;
+		cp->c_data_ptr = cp->buffer_storage[
+			pvr2_buffer_get_id(cp->c_buf)];
+	}
+	return !0;
+}
+
+void pvr2_ioread_filter(struct pvr2_ioread *cp)
+{
+	unsigned int idx;
+	if (!cp->enabled) return;
+	if (cp->sync_state != 1) return;
+
+	// Search the stream for our synchronization key.  This is made
+	// complicated by the fact that in order to be honest with
+	// ourselves here we must search across buffer boundaries...
+	mutex_lock(&cp->mutex); while (1) {
+		// Ensure we have a buffer
+		if (!pvr2_ioread_get_buffer(cp)) break;
+		if (!cp->c_data_len) break;
+
+		// Now walk the buffer contents until we match the key or
+		// run out of buffer data.
+		for (idx = cp->c_data_offs; idx < cp->c_data_len; idx++) {
+			if (cp->sync_buf_offs >= cp->sync_key_len) break;
+			if (cp->c_data_ptr[idx] ==
+			    cp->sync_key_ptr[cp->sync_buf_offs]) {
+				// Found the next key byte
+				(cp->sync_buf_offs)++;
+			} else {
+				// Whoops, mismatched.  Start key over...
+				cp->sync_buf_offs = 0;
+			}
+		}
+
+		// Consume what we've walked through
+		cp->c_data_offs += idx;
+		cp->sync_trashed_count += idx;
+
+		// If we've found the key, then update state and get out.
+		if (cp->sync_buf_offs >= cp->sync_key_len) {
+			cp->sync_trashed_count -= cp->sync_key_len;
+			pvr2_trace(PVR2_TRACE_DATA_FLOW,
+				   "/*---TRACE_READ---*/"
+				   " sync_state <== 2 (skipped %u bytes)",
+				   cp->sync_trashed_count);
+			cp->sync_state = 2;
+			cp->sync_buf_offs = 0;
+			break;
+		}
+
+		if (cp->c_data_offs < cp->c_data_len) {
+			// Sanity check - should NEVER get here
+			pvr2_trace(PVR2_TRACE_ERROR_LEGS,
+				   "ERROR: pvr2_ioread filter sync problem"
+				   " len=%u offs=%u",
+				   cp->c_data_len,cp->c_data_offs);
+			// Get out so we don't get stuck in an infinite
+			// loop.
+			break;
+		}
+
+		continue; // (for clarity)
+	} mutex_unlock(&cp->mutex);
+}
+
 int pvr2_ioread_avail(struct pvr2_ioread *cp)
 {
 	int ret;
@@ -214,6 +384,11 @@ int pvr2_ioread_avail(struct pvr2_ioread *cp)
 		return -EIO;
 	}
 
+	if (cp->sync_state == 1) {
+		pvr2_ioread_filter(cp);
+		if (cp->sync_state == 1) return -EAGAIN;
+	}
+
 	ret = 0;
 	if (cp->stream_running) {
 		if (!pvr2_stream_get_ready_count(cp->stream)) {
@@ -243,7 +418,6 @@ int pvr2_ioread_read(struct pvr2_ioread *cp,void __user *buf,unsigned int cnt)
 	unsigned int bcnt;
 	const char *src;
 	int stat;
-	struct pvr2_buffer *bp;
 	int ret = 0;
 	unsigned int req_cnt = cnt;
 
@@ -264,75 +438,57 @@ int pvr2_ioread_read(struct pvr2_ioread *cp,void __user *buf,unsigned int cnt)
 		// Suck data out of the buffers and copy to the user
 		copied_cnt = 0;
 		if (!buf) cnt = 0;
-		while (cnt) {
-			bp = pvr2_stream_get_ready_buffer(cp->stream);
-			if (!bp) break; // Nothing ready; done
-
-			bcnt = pvr2_buffer_get_count(bp);
-			if (!bcnt) {
-				// Nothing transferred.  Was there an
-				// error?
-				stat = pvr2_buffer_get_status(bp);
-				if (stat < 0) {
-					// Streaming error...
-					pvr2_trace(PVR2_TRACE_DATA_FLOW,
-						   "/*---TRACE_READ---*/"
-						   " pvr2_ioread_read id=%p"
-						   " buffer_error=%d",
-						   cp,stat);
-					pvr2_ioread_stop(cp);
-					ret = -EIO;
-					break;
-				}
-				src = 0;
-			} else {
-				// Calculate buffer offset and count
-				src = cp->buffer_storage[
-					pvr2_buffer_get_id(bp)];
-				if (cp->resid_offs > bcnt) {
-					cp->resid_offs = bcnt;
-				}
-				src += cp->resid_offs;
-				bcnt -= cp->resid_offs;
+		while (1) {
+			if (!pvr2_ioread_get_buffer(cp)) {
+				ret = -EIO;
+				break;
 			}
-			if (bcnt > cnt) {
-				// Can't read the entire buffer this time
-				// so remember how far in we're going to
-				// get so we can resume it later
-				bcnt = cnt;
-				cp->resid_offs += bcnt;
+
+			if (!cnt) break;
+
+			if (cp->sync_state == 2) {
+				// We're repeating the sync key data into
+				// the stream.
+				src = cp->sync_key_ptr + cp->sync_buf_offs;
+				bcnt = cp->sync_key_len - cp->sync_buf_offs;
 			} else {
-				// We will be able to get the whole thing,
-				// so clear the residual offset
-				cp->resid_offs = 0;
+				// Normal buffer copy
+				src = cp->c_data_ptr + cp->c_data_offs;
+				bcnt = cp->c_data_len - cp->c_data_offs;
 			}
-			if (bcnt) {
-				if (copy_to_user(buf,src,bcnt)) {
-					// User supplied a bad pointer?
-					// Give up - this *will* cause data
-					// to be lost.
-					ret = -EFAULT;
-					break;
-				}
-				cnt -= bcnt;
-				buf += bcnt;
-				copied_cnt += bcnt;
-			}
-			if (cp->resid_offs) break; // If there's a residual
-						   // offset, get out
-			// Queue the buffer now that we've consumed it.
-			stat = pvr2_buffer_queue(bp);
-			if (stat < 0) {
-				// Streaming error...
-				pvr2_trace(PVR2_TRACE_DATA_FLOW,
-					   "/*---TRACE_READ---*/"
-					   " pvr2_ioread_read id=%p"
-					   " queue_error=%d",
-					   cp,stat);
-				pvr2_ioread_stop(cp);
-				ret = stat;
+
+			if (!bcnt) break;
+
+			// Don't run past user's buffer
+			if (bcnt > cnt) bcnt = cnt;
+
+			if (copy_to_user(buf,src,bcnt)) {
+				// User supplied a bad pointer?
+				// Give up - this *will* cause data
+				// to be lost.
+				ret = -EFAULT;
 				break;
 			}
+			cnt -= bcnt;
+			buf += bcnt;
+			copied_cnt += bcnt;
+
+			if (cp->sync_state == 2) {
+				// Update offset inside sync key that we're
+				// repeating back out.
+				cp->sync_buf_offs += bcnt;
+				if (cp->sync_buf_offs >= cp->sync_key_len) {
+					// Consumed entire key; switch mode
+					// to normal.
+					pvr2_trace(PVR2_TRACE_DATA_FLOW,
+						   "/*---TRACE_READ---*/"
+						   " sync_state <== 0");
+					cp->sync_state = 0;
+				}
+			} else {
+				// Update buffer offset.
+				cp->c_data_offs += bcnt;
+			}
 		}
 
 	} while (0); mutex_unlock(&cp->mutex);
diff --git a/linux/drivers/media/video/pvrusb2/pvrusb2-ioread.h b/linux/drivers/media/video/pvrusb2/pvrusb2-ioread.h
index e6205f123..6b002597f 100644
--- a/linux/drivers/media/video/pvrusb2/pvrusb2-ioread.h
+++ b/linux/drivers/media/video/pvrusb2/pvrusb2-ioread.h
@@ -29,7 +29,10 @@ struct pvr2_ioread *pvr2_ioread_create(void);
 void pvr2_ioread_destroy(struct pvr2_ioread *);
 int pvr2_ioread_setup(struct pvr2_ioread *,struct pvr2_stream *);
 struct pvr2_stream *pvr2_ioread_get_stream(struct pvr2_ioread *);
-int pvr2_ioread_set_enabled(struct pvr2_ioread *,int);
+void pvr2_ioread_set_sync_key(struct pvr2_ioread *,
+			      const char *sync_key_ptr,
+			      unsigned int sync_key_len);
+int pvr2_ioread_set_enabled(struct pvr2_ioread *,int fl);
 int pvr2_ioread_get_enabled(struct pvr2_ioread *);
 int pvr2_ioread_read(struct pvr2_ioread *,void __user *buf,unsigned int cnt);
 int pvr2_ioread_avail(struct pvr2_ioread *);
diff --git a/linux/drivers/media/video/pvrusb2/pvrusb2-v4l2.c b/linux/drivers/media/video/pvrusb2/pvrusb2-v4l2.c
index 017d30ad0..44423ff1a 100644
--- a/linux/drivers/media/video/pvrusb2/pvrusb2-v4l2.c
+++ b/linux/drivers/media/video/pvrusb2/pvrusb2-v4l2.c
@@ -1018,7 +1018,6 @@ static void pvr2_v4l2_notify(struct pvr2_v4l2_fh *fhp)
 	wake_up(&fhp->wait_data);
 }
 
-
 static int pvr2_v4l2_iosetup(struct pvr2_v4l2_fh *fh)
 {
 	int ret;
@@ -1034,7 +1033,7 @@ static int pvr2_v4l2_iosetup(struct pvr2_v4l2_fh *fh)
 		return ret;
 	}
 
-	fh->rhp = pvr2_ioread_create();
+	fh->rhp = pvr2_channel_create_mpeg_stream(fh->dev_info->stream);
 	if (!fh->rhp) {
 		pvr2_channel_claim_stream(&fh->channel,0);
 		return -ENOMEM;
@@ -1042,7 +1041,6 @@ static int pvr2_v4l2_iosetup(struct pvr2_v4l2_fh *fh)
 
 	hdw = fh->channel.mc_head->hdw;
 	sp = fh->dev_info->stream->stream;
-	pvr2_ioread_setup(fh->rhp,sp);
 	pvr2_stream_set_callback(sp,(pvr2_stream_callback)pvr2_v4l2_notify,fh);
 	pvr2_hdw_set_stream_type(hdw,fh->dev_info->config);
 	pvr2_hdw_set_streaming(hdw,!0);