reworked omxdevice and improved clock stretching for transfer mode

6 files changed, 310 insertions, 180 deletions

diff --git a/HISTORY b/HISTORY
index 2c601ed..f9b5875 100644
--- a/HISTORY
+++ b/HISTORY
@@ -2,6 +2,7 @@ VDR Plugin 'rpihddevice' Revision History
 -----------------------------------------
 
 - new:
+  - reworked omxdevice and improved clock stretching for transfer mode
   - added Hungarian translations (thanks to Füley István)
   - updated Finnish translations and fixed tr() usage (thanks to Rolf Ahrenberg)
   - use fast deinterlacer for HD streams
diff --git a/Makefile b/Makefile
index 547d7ab..fa3ca1c 100644
--- a/Makefile
+++ b/Makefile
@@ -69,6 +69,16 @@ ifeq ($(DEBUG), 1)
     DEFINES += -DDEBUG
 endif
 
+DEBUG_LATENCY ?= 0
+ifeq ($(DEBUG_LATENCY), 1)
+    DEFINES += -DDEBUG_LATENCY
+endif
+
+DEBUG_BUFFERS ?= 0
+ifeq ($(DEBUG_BUFFERS), 1)
+    DEFINES += -DDEBUG_BUFFERS
+endif
+
 # ffmpeg/libav configuration
 ifdef EXT_LIBAV
 	LIBAV_PKGCFG = $(shell PKG_CONFIG_PATH=$(EXT_LIBAV)/lib/pkgconfig pkg-config $(1))
diff --git a/omx.c b/omx.c
index c7ff045..54fc16e 100644
--- a/omx.c
+++ b/omx.c
@@ -18,7 +18,7 @@ extern "C" {
 
 #include "bcm_host.h"
 
-#define OMX_PRE_ROLL 50
+#define OMX_PRE_ROLL 0
 
 #define OMX_INIT_STRUCT(a) \
 	memset(&(a), 0, sizeof(a)); \
@@ -457,7 +457,7 @@ int cOmx::Init(void)
 	ilclient_change_component_state(m_comp[eAudioRender], OMX_StateIdle);
 
 	SetClockLatencyTarget();
-	SetBufferStallThreshold(1500);
+	SetBufferStallThreshold(20000);
 	SetClockReference(cOmx::eClockRefVideo);
 
 	FlushVideo();
@@ -660,19 +660,19 @@ void cOmx::SetCurrentReferenceTime(uint64_t pts)
 	}
 }
 
-unsigned int cOmx::GetMediaTime(void)
+unsigned int cOmx::GetAudioLatency(void)
 {
 	unsigned int ret = 0;
 
-	OMX_TIME_CONFIG_TIMESTAMPTYPE timestamp;
-	OMX_INIT_STRUCT(timestamp);
-	timestamp.nPortIndex = OMX_ALL;
+	OMX_PARAM_U32TYPE u32;
+	OMX_INIT_STRUCT(u32);
+	u32.nPortIndex = 100;
 
-	if (OMX_GetConfig(ILC_GET_HANDLE(m_comp[eClock]),
-		OMX_IndexConfigTimeCurrentMediaTime, &timestamp) != OMX_ErrorNone)
-		ELOG("failed get current clock reference!");
+	if (OMX_GetConfig(ILC_GET_HANDLE(m_comp[eAudioRender]),
+		OMX_IndexConfigAudioRenderingLatency, &u32) != OMX_ErrorNone)
+		ELOG("failed get audio render latency!");
 	else
-		ret = timestamp.nTimestamp.nLowPart;
+		ret = u32.nU32;
 
 	return ret;
 }
@@ -797,24 +797,6 @@ void cOmx::SetMute(bool mute)
 		ELOG("failed to set mute state!");
 }
 
-void cOmx::SendEos(void)
-{
-#if 0
-	OMX_BUFFERHEADERTYPE *buf = ilclient_get_input_buffer(m_comp[eVideoDecoder], 130, 1);
-	if (buf == NULL)
-		return;
-
-	buf->nFilledLen = 0;
-	buf->nFlags = OMX_BUFFERFLAG_TIME_UNKNOWN | OMX_BUFFERFLAG_EOS;
-
-	if (OMX_EmptyThisBuffer(ILC_GET_HANDLE(m_comp[eVideoDecoder]), buf) != OMX_ErrorNone)
-		ELOG("failed to send empty packet to video decoder!");
-
-	if (!m_eosEvent->Wait(10000))
-		ELOG("time out waiting for EOS event!");
-#endif
-}
-
 void cOmx::StopVideo(void)
 {
 	// put video decoder into idle
@@ -861,22 +843,6 @@ void cOmx::StopAudio(void)
 	m_spareAudioBuffers = 0;
 }
 
-void cOmx::SetVideoDataUnitType(eDataUnitType dataUnitType)
-{
-	OMX_PARAM_DATAUNITTYPE dataUnit;
-	OMX_INIT_STRUCT(dataUnit);
-	dataUnit.nPortIndex = 130;
-
-	dataUnit.eEncapsulationType = OMX_DataEncapsulationElementaryStream;
-	dataUnit.eUnitType = dataUnitType == eCodedPicture ?
-				OMX_DataUnitCodedPicture : OMX_DataUnitArbitraryStreamSection;
-
-	if (OMX_SetParameter(ILC_GET_HANDLE(m_comp[eVideoDecoder]),
-			OMX_IndexParamBrcmDataUnit, &dataUnit) != OMX_ErrorNone)
-		ELOG("failed to set video decoder data unit type!");
-
-}
-
 void cOmx::SetVideoErrorConcealment(bool startWithValidFrame)
 {
 	OMX_PARAM_BRCMVIDEODECODEERRORCONCEALMENTTYPE ectype;
@@ -919,7 +885,7 @@ void cOmx::FlushVideo(bool flushRender)
 	m_setVideoDiscontinuity = true;
 }
 
-int cOmx::SetVideoCodec(cVideoCodec::eCodec codec, eDataUnitType dataUnit)
+int cOmx::SetVideoCodec(cVideoCodec::eCodec codec)
 {
 	if (ilclient_change_component_state(m_comp[eVideoDecoder], OMX_StateIdle) != 0)
 		ELOG("failed to set video decoder to idle state!");
@@ -953,7 +919,6 @@ int cOmx::SetVideoCodec(cVideoCodec::eCodec codec, eDataUnitType dataUnit)
 
 	// start with valid frames only if codec is MPEG2
 	SetVideoErrorConcealment(codec == cVideoCodec::eMPEG2);
-	SetVideoDataUnitType(dataUnit);
 	SetVideoDecoderExtraBuffers(3);
 
 	if (ilclient_enable_port_buffers(m_comp[eVideoDecoder], 130, NULL, NULL, NULL) != 0)
@@ -1103,8 +1068,8 @@ int cOmx::SetupAudioRender(cAudioCodec::eCodec outputFormat, int channels,
 			OMX_IndexParamPortDefinition, &param) != OMX_ErrorNone)
 		ELOG("failed to get audio render port parameters!");
 
-	param.nBufferSize = KILOBYTE(160);
-	param.nBufferCountActual = 4;
+	param.nBufferSize = KILOBYTE(16);
+	param.nBufferCountActual = 256;
 	m_freeAudioBuffers = true;
 
 	if (OMX_SetParameter(ILC_GET_HANDLE(m_comp[eAudioRender]),
@@ -1117,7 +1082,7 @@ int cOmx::SetupAudioRender(cAudioCodec::eCodec outputFormat, int channels,
 	ilclient_change_component_state(m_comp[eAudioRender], OMX_StateExecuting);
 
 	if (ilclient_setup_tunnel(&m_tun[eClockToAudioRender], 0, 0) != 0)
-		ELOG("failed to setup up tunnel from clock to video scheduler!");
+		ELOG("failed to setup up tunnel from clock to audio render!");
 
 	return 0;
 }
@@ -1183,9 +1148,16 @@ OMX_BUFFERHEADERTYPE* cOmx::GetAudioBuffer(uint64_t pts)
 
 	if (buf)
 	{
+		buf->nFilledLen = 0;
+		buf->nOffset = 0;
+		buf->nFlags = 0;
+
+		if (m_setAudioStartTime)
+			buf->nFlags |= OMX_BUFFERFLAG_STARTTIME;
+		else if (!pts)
+			buf->nFlags |= OMX_BUFFERFLAG_TIME_UNKNOWN;
+
 		cOmx::PtsToTicks(pts, buf->nTimeStamp);
-		buf->nFlags = pts ? 0 : OMX_BUFFERFLAG_TIME_UNKNOWN;
-		buf->nFlags |= m_setAudioStartTime ? OMX_BUFFERFLAG_STARTTIME : 0;
 		m_setAudioStartTime = false;
 	}
 	else
@@ -1212,11 +1184,19 @@ OMX_BUFFERHEADERTYPE* cOmx::GetVideoBuffer(uint64_t pts)
 
 	if (buf)
 	{
-		cOmx::PtsToTicks(pts, buf->nTimeStamp);
-		buf->nFlags = pts ? 0 : OMX_BUFFERFLAG_TIME_UNKNOWN;
-		buf->nFlags |= m_setVideoStartTime ? OMX_BUFFERFLAG_STARTTIME : 0;
-		buf->nFlags |= m_setVideoDiscontinuity ? OMX_BUFFERFLAG_DISCONTINUITY : 0;
+		buf->nFilledLen = 0;
+		buf->nOffset = 0;
+		buf->nFlags = 0;
 
+		if (m_setVideoStartTime)
+			buf->nFlags |= OMX_BUFFERFLAG_STARTTIME;
+		else if (!pts)
+			buf->nFlags |= OMX_BUFFERFLAG_TIME_UNKNOWN;
+
+		if (m_setVideoDiscontinuity)
+			buf->nFlags |= OMX_BUFFERFLAG_DISCONTINUITY;
+
+		cOmx::PtsToTicks(pts, buf->nTimeStamp);
 		m_setVideoStartTime = false;
 		m_setVideoDiscontinuity = false;
 	}
@@ -1227,14 +1207,53 @@ OMX_BUFFERHEADERTYPE* cOmx::GetVideoBuffer(uint64_t pts)
 	return buf;
 }
 
+#ifdef DEBUG_BUFFERS
+void cOmx::DumpBuffer(OMX_BUFFERHEADERTYPE *buf, const char *prefix)
+{
+	DLOG("%s: TS=%8x%08x, LEN=%5d/%5d: %02x %02x %02x %02x... "
+			"FLAGS: %s%s%s%s%s%s%s%s%s%s%s%s%s%s",
+		prefix,
+		buf->nTimeStamp.nHighPart, buf->nTimeStamp.nLowPart,
+		buf->nFilledLen, buf->nAllocLen,
+		buf->pBuffer[0], buf->pBuffer[1], buf->pBuffer[2], buf->pBuffer[3],
+		buf->nFlags & OMX_BUFFERFLAG_EOS             ? "EOS "           : "",
+		buf->nFlags & OMX_BUFFERFLAG_STARTTIME       ? "STARTTIME "     : "",
+		buf->nFlags & OMX_BUFFERFLAG_DECODEONLY      ? "DECODEONLY "    : "",
+		buf->nFlags & OMX_BUFFERFLAG_DATACORRUPT     ? "DATACORRUPT "   : "",
+		buf->nFlags & OMX_BUFFERFLAG_ENDOFFRAME      ? "ENDOFFRAME "    : "",
+		buf->nFlags & OMX_BUFFERFLAG_SYNCFRAME       ? "SYNCFRAME "     : "",
+		buf->nFlags & OMX_BUFFERFLAG_EXTRADATA       ? "EXTRADATA "     : "",
+		buf->nFlags & OMX_BUFFERFLAG_CODECCONFIG     ? "CODECCONFIG "   : "",
+		buf->nFlags & OMX_BUFFERFLAG_TIME_UNKNOWN    ? "TIME_UNKNOWN "  : "",
+		buf->nFlags & OMX_BUFFERFLAG_CAPTURE_PREVIEW ? "CAPTURE_PREV "  : "",
+		buf->nFlags & OMX_BUFFERFLAG_ENDOFNAL        ? "ENDOFNAL "      : "",
+		buf->nFlags & OMX_BUFFERFLAG_FRAGMENTLIST    ? "FRAGMENTLIST "  : "",
+		buf->nFlags & OMX_BUFFERFLAG_DISCONTINUITY   ? "DISCONTINUITY " : "",
+		buf->nFlags & OMX_BUFFERFLAG_CODECSIDEINFO   ? "CODECSIDEINFO " : ""
+	);
+}
+#endif
+
 bool cOmx::EmptyAudioBuffer(OMX_BUFFERHEADERTYPE *buf)
 {
 	if (!buf)
 		return false;
 
+#ifdef DEBUG_BUFFERS
+	DumpBuffer(buf, "A");
+#endif
+
 	if (OMX_EmptyThisBuffer(ILC_GET_HANDLE(m_comp[eAudioRender]), buf)
 			!= OMX_ErrorNone)
 	{
+		ELOG("failed to empty OMX audio buffer");
+
+		if (buf->nFlags & OMX_BUFFERFLAG_STARTTIME)
+			m_setAudioStartTime = true;
+
+		if (buf->nFlags & OMX_BUFFERFLAG_DISCONTINUITY)
+			m_setVideoDiscontinuity = true;
+
 		buf->nFilledLen = 0;
 		buf->pAppPrivate = m_spareAudioBuffers;
 		m_spareAudioBuffers = buf;
@@ -1249,9 +1268,18 @@ bool cOmx::EmptyVideoBuffer(OMX_BUFFERHEADERTYPE *buf)
 	if (!buf)
 		return false;
 
+#ifdef DEBUG_BUFFERS
+	DumpBuffer(buf, "V");
+#endif
+
 	if (OMX_EmptyThisBuffer(ILC_GET_HANDLE(m_comp[eVideoDecoder]), buf)
 			!= OMX_ErrorNone)
 	{
+		ELOG("failed to empty OMX video buffer");
+
+		if (buf->nFlags & OMX_BUFFERFLAG_STARTTIME)
+			m_setVideoStartTime = true;
+
 		buf->nFilledLen = 0;
 		buf->pAppPrivate = m_spareVideoBuffers;
 		m_spareVideoBuffers = buf;
diff --git a/omx.h b/omx.h
index 4c9f73a..62cd8a3 100644
--- a/omx.h
+++ b/omx.h
@@ -53,7 +53,7 @@ public:
 	void SetClockScale(OMX_S32 scale);
 	bool IsClockFreezed(void) { return m_clockScale == 0; }
 	void SetCurrentReferenceTime(uint64_t pts);
-	unsigned int GetMediaTime(void);
+	unsigned int GetAudioLatency(void);
 
 	enum eClockReference {
 		eClockRefAudio,
@@ -65,24 +65,16 @@ public:
 	void SetClockLatencyTarget(void);
 	void SetVolume(int vol);
 	void SetMute(bool mute);
-	void SendEos(void);
 	void StopVideo(void);
 	void StopAudio(void);
 
-	enum eDataUnitType {
-		eCodedPicture,
-		eArbitraryStreamSection
-	};
-
-	void SetVideoDataUnitType(eDataUnitType dataUnitType);
 	void SetVideoErrorConcealment(bool startWithValidFrame);
 	void SetVideoDecoderExtraBuffers(int extraBuffers);
 
 	void FlushAudio(void);
 	void FlushVideo(bool flushRender = false);
 
-	int SetVideoCodec(cVideoCodec::eCodec codec,
-			eDataUnitType dataUnit = eArbitraryStreamSection);
+	int SetVideoCodec(cVideoCodec::eCodec codec);
 	int SetupAudioRender(cAudioCodec::eCodec outputFormat,
 			int channels, cRpiAudioPort::ePort audioPort,
 			int samplingRate = 0, int frameSize = 0);
@@ -106,6 +98,10 @@ private:
 
 	static const char* errStr(int err);
 
+#ifdef DEBUG_BUFFERS
+	static void DumpBuffer(OMX_BUFFERHEADERTYPE *buf, const char *prefix = "");
+#endif
+
 	enum eOmxComponent {
 		eClock = 0,
 		eVideoDecoder,
diff --git a/omxdevice.c b/omxdevice.c
index f1ac178..3e25121 100644
--- a/omxdevice.c
+++ b/omxdevice.c
@@ -17,22 +17,19 @@
 
 #include <string.h>
 
-// latency target for transfer mode in PTS ticks (90kHz) -> 500ms
-#define LATENCY_TARGET 45000LL
-// latency window for validation where closed loop will be active (+/- 4s)
-#define LATENCY_WINDOW 360000LL
-
 #define S(x) ((int)(floor(x * pow(2, 16))))
 
 // trick speeds as defined in vdr/dvbplayer.c
-int cOmxDevice::s_speeds[2][8] = {
+const int cOmxDevice::s_playbackSpeeds[eNumDirections][eNumPlaybackSpeeds] = {
 	{ S(0.0f), S( 0.125f), S( 0.25f), S( 0.5f), S( 1.0f), S( 2.0f), S( 4.0f), S( 12.0f) },
 	{ S(0.0f), S(-0.125f), S(-0.25f), S(-0.5f), S(-1.0f), S(-2.0f), S(-4.0f), S(-12.0f) }
 };
 
-// speed correction factors for live mode, taken from omxplayer
-int cOmxDevice::s_speedCorrections[5] = {
-	S(0.990f), S(0.999f), S(1.000f), S(1.001), S(1.010)
+// speed correction factors for live mode
+// HDMI specification allows a tolerance of 1000ppm, however on the Raspberry Pi
+// it's limited to 175ppm to avoid audio drops one some A/V receivers
+const int cOmxDevice::s_liveSpeeds[eNumLiveSpeeds] = {
+	S(0.999f), S(0.99985f), S(1.000f), S(1.00015), S(1.001)
 };
 
 const uchar cOmxDevice::PesVideoHeader[14] = {
@@ -46,7 +43,8 @@ cOmxDevice::cOmxDevice(void (*onPrimaryDevice)(void)) :
 	m_audio(new cRpiAudioDecoder(m_omx)),
 	m_mutex(new cMutex()),
 	m_videoCodec(cVideoCodec::eInvalid),
-	m_speed(eNormal),
+	m_liveSpeed(eNoCorrection),
+	m_playbackSpeed(eNormal),
 	m_direction(eForward),
 	m_hasVideo(false),
 	m_hasAudio(false),
@@ -55,7 +53,11 @@ cOmxDevice::cOmxDevice(void (*onPrimaryDevice)(void)) :
 	m_trickRequest(0),
 	m_audioPts(0),
 	m_videoPts(0),
-	m_latency(0)
+	m_audioId(0),
+	m_latencySamples(0),
+	m_latencyTarget(0),
+	m_posMaxCorrections(0),
+	m_negMaxCorrections(0)
 {
 }
 
@@ -92,7 +94,6 @@ int cOmxDevice::Init(void)
 int cOmxDevice::DeInit(void)
 {
 	cRpiSetup::SetVideoSetupChangedCallback(0);
-
 	if (m_audio->DeInit() < 0)
 	{
 		ELOG("failed to deinitialize audio!");
@@ -158,6 +159,15 @@ bool cOmxDevice::SetPlayMode(ePlayMode PlayMode)
 	switch (PlayMode)
 	{
 	case pmNone:
+		FlushStreams(true);
+		if (m_hasVideo)
+			m_omx->StopVideo();
+
+		m_hasAudio = false;
+		m_hasVideo = false;
+		m_videoCodec = cVideoCodec::eInvalid;
+		break;
+
 		if (m_hasAudio)
 		{
 			m_audio->Reset();
@@ -180,7 +190,7 @@ bool cOmxDevice::SetPlayMode(ePlayMode PlayMode)
 	case pmAudioOnly:
 	case pmAudioOnlyBlack:
 	case pmVideoOnly:
-		m_speed = eNormal;
+		m_playbackSpeed = eNormal;
 		m_direction = eForward;
 		break;
 
@@ -198,6 +208,7 @@ void cOmxDevice::StillPicture(const uchar *Data, int Length)
 		cDevice::StillPicture(Data, Length);
 	else
 	{
+		DBG("StillPicture()");
 		int pesLength = 0;
 		uchar *pesPacket = 0;
 
@@ -213,7 +224,6 @@ void cOmxDevice::StillPicture(const uchar *Data, int Length)
 
 			memcpy(pesPacket, PesVideoHeader, sizeof(PesVideoHeader));
 			memcpy(pesPacket + sizeof(PesVideoHeader), Data, Length);
-			PesSetPts(pesPacket, 1);
 		}
 		else
 			codec = ParseVideoCodec(Data + PesPayloadOffset(Data),
@@ -223,34 +233,31 @@ void cOmxDevice::StillPicture(const uchar *Data, int Length)
 			return;
 
 		m_mutex->Lock();
-
-		// manually restart clock and wait for video only
-		m_omx->StopClock();
-		m_omx->SetClockScale(s_speeds[eForward][eNormal]);
-		m_omx->StartClock(true, false);
+		m_playbackSpeed = eNormal;
+		m_direction = eForward;
 
 		// to get a picture displayed, PlayVideo() needs to be called
-		// 4x for MPEG2 and 13x for H264... ?
-		int repeat = codec == cVideoCodec::eMPEG2 ? 4 : 13;
-
+		// 4x for MPEG2 and 10x for H264... ?
+		int repeat = codec == cVideoCodec::eMPEG2 ? 4 : 10;
 		while (repeat--)
 		{
 			int length = pesPacket ? pesLength : Length;
 			const uchar *data = pesPacket ? pesPacket : Data;
 
-			// play every single PES packet, rise EOS flag on last
+			// play every single PES packet, rise ENDOFFRAME flag on last
 			while (PesLongEnough(length))
 			{
 				int pktLen = PesHasLength(data) ? PesLength(data) : length;
 
 				// skip non-video packets as they may occur in PES recordings
 				if ((data[3] & 0xf0) == 0xe0)
-					PlayVideo(data, pktLen, !repeat && (pktLen == length));
+					PlayVideo(data, pktLen, pktLen == length);
 
 				data += pktLen;
 				length -= pktLen;
 			}
 		}
+		SubmitEOS();
 		m_mutex->Unlock();
 
 		if (pesPacket)
@@ -260,24 +267,21 @@ void cOmxDevice::StillPicture(const uchar *Data, int Length)
 
 int cOmxDevice::PlayAudio(const uchar *Data, int Length, uchar Id)
 {
-	if (m_skipAudio)
-		return Length;
-
 	m_mutex->Lock();
 
 	if (!m_hasAudio)
 	{
 		m_hasAudio = true;
+		m_audioId = Id;
 		m_omx->SetClockReference(cOmx::eClockRefAudio);
 
-		// actually, clock should be restarted anyway, but if video is already
-		// present, decoder will get stuck after clock restart and raises a
-		// buffer stall
 		if (!m_hasVideo)
 		{
-			FlushStreams();
-			m_omx->SetClockScale(s_speeds[m_direction][m_speed]);
+			DBG("audio first");
+			m_omx->SetClockScale(s_playbackSpeeds[m_direction][m_playbackSpeed]);
 			m_omx->StartClock(m_hasVideo, m_hasAudio);
+			if (Transferring())
+				ResetLatency();
 		}
 	}
 
@@ -293,7 +297,14 @@ int cOmxDevice::PlayAudio(const uchar *Data, int Length, uchar Id)
 	}
 
 	if (Transferring() && pts)
+	{
+		if (m_audioId != Id)
+		{
+			ResetLatency();
+			m_audioId = Id;
+		}
 		UpdateLatency(pts);
+	}
 
 	int ret = Length;
 	int length = Length - PesPayloadOffset(Data);
@@ -314,12 +325,11 @@ int cOmxDevice::PlayAudio(const uchar *Data, int Length, uchar Id)
 		if (!m_audio->WriteData(data, length, pts))
 			ret = 0;
 	}
-
 	m_mutex->Unlock();
 	return ret;
 }
 
-int cOmxDevice::PlayVideo(const uchar *Data, int Length, bool EndOfStream)
+int cOmxDevice::PlayVideo(const uchar *Data, int Length, bool EndOfFrame)
 {
 	m_mutex->Lock();
 	int ret = Length;
@@ -346,7 +356,7 @@ int cOmxDevice::PlayVideo(const uchar *Data, int Length, bool EndOfStream)
 		if (cRpiSetup::IsVideoCodecSupported(codec))
 		{
 			videoRestart = true;
-			m_omx->SetVideoCodec(codec, cOmx::eArbitraryStreamSection);
+			m_omx->SetVideoCodec(codec);
 			DLOG("set video codec to %s", cVideoCodec::Str(codec));
 		}
 		else
@@ -355,10 +365,17 @@ int cOmxDevice::PlayVideo(const uchar *Data, int Length, bool EndOfStream)
 
 	if (videoRestart)
 	{
+		if (!m_hasAudio)
+		{
+			DBG("video first");
+			m_omx->SetClockReference(cOmx::eClockRefVideo);
+		}
+
 		m_hasVideo = true;
-		FlushStreams();
-		m_omx->SetClockScale(s_speeds[m_direction][m_speed]);
+		m_omx->SetClockScale(s_playbackSpeeds[m_direction][m_playbackSpeed]);
 		m_omx->StartClock(m_hasVideo, m_hasAudio);
+		if (Transferring())
+			ResetLatency();
 	}
 
 	if (m_hasVideo)
@@ -366,13 +383,8 @@ int cOmxDevice::PlayVideo(const uchar *Data, int Length, bool EndOfStream)
 		int64_t pts = PesHasPts(Data) ? PesGetPts(Data) : 0;
 
 		// keep track of direction in case of trick speed
-		if (m_trickRequest && pts)
-		{
-			if (m_videoPts)
-				PtsTracker(PtsDiff(m_videoPts, pts));
-
-			m_videoPts = pts;
-		}
+		if (m_trickRequest && pts && m_videoPts)
+			PtsTracker(PtsDiff(m_videoPts, pts));
 
 		if (!m_hasAudio && Transferring() && pts)
 			UpdateLatency(pts);
@@ -393,8 +405,8 @@ int cOmxDevice::PlayVideo(const uchar *Data, int Length, bool EndOfStream)
 				Length -= buf->nFilledLen;
 				Data += buf->nFilledLen;
 
-				if (EndOfStream && !Length)
-					buf->nFlags |= OMX_BUFFERFLAG_EOS;
+				if (EndOfFrame && !Length)
+					buf->nFlags |= OMX_BUFFERFLAG_ENDOFFRAME;
 
 				if (!m_omx->EmptyVideoBuffer(buf))
 				{
@@ -411,11 +423,20 @@ int cOmxDevice::PlayVideo(const uchar *Data, int Length, bool EndOfStream)
 			pts = 0;
 		}
 	}
-
 	m_mutex->Unlock();
 	return ret;
 }
 
+bool cOmxDevice::SubmitEOS(void)
+{
+	DBG("SubmitEOS()");
+	OMX_BUFFERHEADERTYPE *buf = m_omx->GetVideoBuffer(0);
+	if (buf)
+		buf->nFlags = /*OMX_BUFFERFLAG_ENDOFFRAME | */ OMX_BUFFERFLAG_EOS;
+
+	return m_omx->EmptyVideoBuffer(buf);
+}
+
 int64_t cOmxDevice::GetSTC(void)
 {
 	return m_omx->GetSTC();
@@ -475,8 +496,8 @@ void cOmxDevice::Clear(void)
 	m_mutex->Lock();
 
 	FlushStreams();
-	m_omx->SetClockScale(s_speeds[m_direction][m_speed]);
-	m_omx->StartClock(m_hasVideo, m_hasAudio);
+	m_hasAudio = false;
+	m_hasVideo = false;
 
 	m_mutex->Unlock();
 	cDevice::Clear();
@@ -487,10 +508,9 @@ void cOmxDevice::Play(void)
 	DBG("Play()");
 	m_mutex->Lock();
 
-	m_speed = eNormal;
+	m_playbackSpeed = eNormal;
 	m_direction = eForward;
-	m_omx->SetClockScale(s_speeds[m_direction][m_speed]);
-	m_skipAudio = false;
+	m_omx->SetClockScale(s_playbackSpeeds[m_direction][m_playbackSpeed]);
 
 	m_mutex->Unlock();
 	cDevice::Play();
@@ -501,7 +521,7 @@ void cOmxDevice::Freeze(void)
 	DBG("Freeze()");
 	m_mutex->Lock();
 
-	m_omx->SetClockScale(s_speeds[eForward][ePause]);
+	m_omx->SetClockScale(s_playbackSpeeds[eForward][ePause]);
 
 	m_mutex->Unlock();
 	cDevice::Freeze();
@@ -534,9 +554,9 @@ void cOmxDevice::TrickSpeed(int Speed)
 
 void cOmxDevice::ApplyTrickSpeed(int trickSpeed, bool forward)
 {
-	bool flush = HasIBPTrickSpeed();
+	m_direction = forward ? eForward : eBackward;
+	m_playbackSpeed =
 
-	m_speed =
 		// slow forward
 		trickSpeed ==  8 ? eSlowest :
 		trickSpeed ==  4 ? eSlower  :
@@ -552,42 +572,17 @@ void cOmxDevice::ApplyTrickSpeed(int trickSpeed, bool forward)
 		trickSpeed == 48 ? eSlower  :
 		trickSpeed == 24 ? eSlow    : eNormal;
 
-	m_direction = forward ? eForward : eBackward;
-
-	// we only need to flush when IBP trick mode has changed,
-	// for the other transitions VDR will call Clear() if necessary
-	flush ^= HasIBPTrickSpeed();
-
-	// if there is video to play, we're going to skip audio
-	// but first, we need to flush audio
-	if (!m_skipAudio && m_hasVideo && !HasIBPTrickSpeed())
-	{
-		m_audio->Reset();
-		m_omx->FlushAudio();
-		m_skipAudio = true;
-	}
-
-	if (flush)
-		FlushStreams();
-
-	m_omx->SetClockScale(s_speeds[m_direction][m_speed]);
-
-	if (flush)
-		m_omx->StartClock(m_hasVideo, !m_skipAudio);
+	m_omx->SetClockScale(s_playbackSpeeds[m_direction][m_playbackSpeed]);
 
 	DBG("ApplyTrickSpeed(%s, %s)",
-			SpeedStr(m_speed), DirectionStr(m_direction));
-}
-
-bool cOmxDevice::HasIBPTrickSpeed(void)
-{
-	// IBP trick speed only supported at first fast forward speed or
-	// for audio only recordings at every speed
-	return m_direction == eForward && (m_speed <= eFast || !m_hasVideo);
+			PlaybackSpeedStr(m_playbackSpeed), DirectionStr(m_direction));
+	return;
 }
 
 void cOmxDevice::PtsTracker(int64_t ptsDiff)
 {
+	DBG("PtsTracker(%lld)", ptsDiff);
+
 	if (ptsDiff < 0)
 		--m_playDirection;
 	else if (ptsDiff > 0)
@@ -600,26 +595,108 @@ void cOmxDevice::PtsTracker(int64_t ptsDiff)
 	}
 }
 
+bool cOmxDevice::HasIBPTrickSpeed(void)
+{
+	return !m_hasVideo;
+}
+
 void cOmxDevice::UpdateLatency(int64_t pts)
 {
-	// calculate and validate latency
-	uint64_t latency = pts - m_omx->GetSTC();
-	if (abs(latency > LATENCY_WINDOW))
+	if (!pts || !m_omx->IsClockRunning())
+		return;
+
+	int64_t stc = m_omx->GetSTC();
+	if (!stc || pts <= stc)
+		return;
+
+	for (int i = LATENCY_FILTER_SIZE - 1; i > 0; i--)
+		m_latency[i] = m_latency[i - 1];
+	m_latency[0] = (pts - stc) / 90;
+
+	if (m_latencySamples < LATENCY_FILTER_SIZE - 1)
+	{
+		m_latencySamples++;
 		return;
+	}
+
+#ifdef DEBUG_LATENCY
+	eLiveSpeed oldSpeed = m_liveSpeed;
+#endif
+	int average = 0;
+
+	for (int i = 0; i < LATENCY_FILTER_SIZE; i++)
+		average += m_latency[i];
+	average = average / LATENCY_FILTER_SIZE;
+
+	if (!m_latencyTarget)
+		m_latencyTarget = 1.4f * average;
+
+	if (average > 2.0f * m_latencyTarget)
+	{
+		if (m_liveSpeed < ePosMaxCorrection)
+		{
+			m_liveSpeed = ePosMaxCorrection;
+			m_posMaxCorrections++;
+			DLOG("latency too big, speeding up...");
+		}
+	}
+	else if (average < 0.5f * m_latencyTarget)
+	{
+		if (m_liveSpeed > eNegMaxCorrection)
+		{
+			m_liveSpeed = eNegMaxCorrection;
+			m_negMaxCorrections++;
+			DLOG("latency too small, slowing down...");
+		}
+	}
+	else if (average > 1.1f * m_latencyTarget)
+	{
+		if (m_liveSpeed < ePosMaxCorrection)
+			m_liveSpeed = ePosCorrection;
+	}
+	else if (average < 0.9f * m_latencyTarget)
+	{
+		if (m_liveSpeed > eNegMaxCorrection)
+			m_liveSpeed = eNegCorrection;
+	}
+	else if (average > m_latencyTarget)
+	{
+		if (m_liveSpeed < eNoCorrection)
+			m_liveSpeed = eNoCorrection;
+	}
+	else if (average < m_latencyTarget)
+	{
+		if (m_liveSpeed > eNoCorrection)
+			m_liveSpeed = eNoCorrection;
+	} else
+		m_liveSpeed = eNoCorrection;
 
-	m_latency = (7 * m_latency + latency) >> 3;
-	eSpeedCorrection corr = eNoCorrection;
+	m_omx->SetClockScale(s_liveSpeeds[m_liveSpeed]);
 
-	if (m_latency < 0.5f * LATENCY_TARGET)
-		corr = eNegMaxCorrection;
-	else if (m_latency < 0.9f * LATENCY_TARGET)
-		corr = eNegCorrection;
-	else if (m_latency > 2.0f * LATENCY_TARGET)
-		corr = ePosMaxCorrection;
-	else if (m_latency > 1.1f * LATENCY_TARGET)
-		corr = ePosCorrection;
+#ifdef DEBUG_LATENCY
+	if (oldSpeed != m_liveSpeed)
+	{
+		DLOG("%s%s latency = %4dms, target = %4dms, corr = %s, "
+				"max neg/pos corr = %d/%d",
+				m_hasAudio ? "A" : "-",  m_hasVideo ? "V" : "-",
+				average, m_latencyTarget,
+				m_liveSpeed == eNegMaxCorrection ? "--|  " :
+				m_liveSpeed == eNegCorrection    ? " -|  " :
+				m_liveSpeed == eNoCorrection     ? "  |  " :
+				m_liveSpeed == ePosCorrection    ? "  |+ " :
+				m_liveSpeed == ePosMaxCorrection ? "  |++" : "  ?  ",
+				m_negMaxCorrections, m_posMaxCorrections);
+	}
+#endif
+}
 
-	m_omx->SetClockScale(s_speedCorrections[corr]);
+void cOmxDevice::ResetLatency(void)
+{
+	m_latencySamples = - LATENCY_FILTER_PREROLL;
+	m_latencyTarget = 0;
+	m_liveSpeed = eNoCorrection;
+	m_posMaxCorrections = 0;
+	m_negMaxCorrections = 0;
 }
 
 void cOmxDevice::HandleBufferStall()
@@ -628,7 +705,7 @@ void cOmxDevice::HandleBufferStall()
 	m_mutex->Lock();
 
 	FlushStreams();
-	m_omx->SetClockScale(s_speeds[m_direction][m_speed]);
+	m_omx->SetClockScale(s_playbackSpeeds[m_direction][m_playbackSpeed]);
 	m_omx->StartClock(m_hasVideo, m_hasAudio);
 
 	m_mutex->Unlock();
@@ -637,11 +714,14 @@ void cOmxDevice::HandleBufferStall()
 void cOmxDevice::HandleEndOfStream()
 {
 	DBG("HandleEndOfStream()");
+	m_mutex->Lock();
 
 	// flush pipes and restart clock after still image
 	FlushStreams();
-	m_omx->SetClockScale(s_speeds[eForward][ePause]);
+	m_omx->SetClockScale(s_playbackSpeeds[m_direction][m_playbackSpeed]);
 	m_omx->StartClock(m_hasVideo, m_hasAudio);
+
+	m_mutex->Unlock();
 }
 
 void cOmxDevice::HandleStreamStart()
@@ -701,8 +781,6 @@ void cOmxDevice::FlushStreams(bool flushVideoRender)
 		m_audio->Reset();
 		m_omx->FlushAudio();
 	}
-
-	m_omx->SetCurrentReferenceTime(0);
 }
 
 void cOmxDevice::SetVolumeDevice(int Volume)
diff --git a/omxdevice.h b/omxdevice.h
index 35ba648..6547631 100644
--- a/omxdevice.h
+++ b/omxdevice.h
@@ -47,7 +47,7 @@ public:
 	virtual int PlayVideo(const uchar *Data, int Length)
 		{ return PlayVideo(Data, Length, false); }
 
-	virtual int PlayVideo(const uchar *Data, int Length, bool EndOfStream);
+	virtual int PlayVideo(const uchar *Data, int Length, bool EndOfFrame);
 
 	virtual int64_t GetSTC(void);
 
@@ -74,7 +74,8 @@ protected:
 
 	enum eDirection {
 		eForward,
-		eBackward
+		eBackward,
+		eNumDirections
 	};
 
 	static const char* DirectionStr(eDirection dir) {
@@ -82,7 +83,7 @@ protected:
 				dir == eBackward ? "backward" : "unknown";
 	}
 
-	enum eSpeed {
+	enum ePlaybackSpeed {
 		ePause,
 		eSlowest,
 		eSlower,
@@ -90,10 +91,11 @@ protected:
 		eNormal,
 		eFast,
 		eFaster,
-		eFastest
+		eFastest,
+		eNumPlaybackSpeeds
 	};
 
-	static const char* SpeedStr(eSpeed speed) {
+	static const char* PlaybackSpeedStr(ePlaybackSpeed speed) {
 		return 	speed == ePause   ? "pause"   :
 				speed == eSlowest ? "slowest" :
 				speed == eSlower  ? "slower"  :
@@ -104,15 +106,16 @@ protected:
 				speed == eFastest ? "fastest" : "unknown";
 	}
 
-	enum eSpeedCorrection {
+	enum eLiveSpeed {
 		eNegMaxCorrection,
 		eNegCorrection,
 		eNoCorrection,
 		ePosCorrection,
 		ePosMaxCorrection,
+		eNumLiveSpeeds
 	};
 
-	static const char* SpeedCorrectionStr(eSpeedCorrection corr) {
+	static const char* LiveSpeedStr(eLiveSpeed corr) {
 		return	corr == eNegMaxCorrection ? "max negative" :
 				corr == eNegCorrection    ? "negative"     :
 				corr == eNoCorrection     ? "no"           :
@@ -120,8 +123,8 @@ protected:
 				corr == ePosMaxCorrection ? "max positive" : "unknown";
 	}
 
-	static int s_speeds[2][8];
-	static int s_speedCorrections[5];
+	static const int s_playbackSpeeds[eNumDirections][eNumPlaybackSpeeds];
+	static const int s_liveSpeeds[eNumLiveSpeeds];
 
 	static const uchar PesVideoHeader[14];
 
@@ -148,11 +151,13 @@ private:
 	void HandleVideoSetupChanged();
 
 	void FlushStreams(bool flushVideoRender = false);
+	bool SubmitEOS(void);
 
 	void ApplyTrickSpeed(int trickSpeed, bool forward);
 	void PtsTracker(int64_t ptsDiff);
 
 	void UpdateLatency(int64_t pts);
+	void ResetLatency(void);
 
 	cOmx			 *m_omx;
 	cRpiAudioDecoder *m_audio;
@@ -160,8 +165,9 @@ private:
 
 	cVideoCodec::eCodec	m_videoCodec;
 
-	eSpeed		m_speed;
-	eDirection	m_direction;
+	eLiveSpeed          m_liveSpeed;
+	ePlaybackSpeed      m_playbackSpeed;
+	eDirection          m_direction;
 
 	bool	m_hasVideo;
 	bool	m_hasAudio;
@@ -172,7 +178,18 @@ private:
 
 	int64_t	m_audioPts;
 	int64_t	m_videoPts;
-	int64_t m_latency;
+
+	uchar   m_audioId;
+
+#define LATENCY_FILTER_SIZE 8
+#define LATENCY_FILTER_PREROLL 16
+
+	int     m_latency[LATENCY_FILTER_SIZE];
+	int     m_latencySamples;
+	int     m_latencyTarget;
+
+	int     m_posMaxCorrections;
+	int     m_negMaxCorrections;
 };
 
 #endif