mpegplayer: Get A-V synchronized. Improve frame sync and dropping logic and take advantage of decoder's ability to assist. Straighten out some threading problems. Clean it up a bit. Added some plugin API functions and decided it was a good time to do a good sorting on them so your next update from this build should be a full replacement.

git-svn-id: svn://svn.rockbox.org/rockbox/trunk@13094 a1c6a512-1295-4272-9138-f99709370657

3 files changed, 1116 insertions, 518 deletions

diff --git a/apps/plugin.c b/apps/plugin.c
index e3e6dd4c37..276bf1b078 100644
--- a/apps/plugin.c
+++ b/apps/plugin.c
@@ -39,7 +39,7 @@
 
 #ifdef HAVE_LCD_BITMAP
 #include "scrollbar.h"
-#include "peakmeter.h" 
+#include "peakmeter.h"
 #include "bmp.h"
 #include "bidi.h"
 #endif
@@ -222,7 +222,7 @@ static const struct plugin_api rockbox_api = {
     PREFIX(readdir),
     PREFIX(mkdir),
     PREFIX(rmdir),
-    
+
     /* dir, cached */
 #ifdef HAVE_DIRCACHE
     opendir_cached,
@@ -233,6 +233,9 @@ static const struct plugin_api rockbox_api = {
     /* kernel/ system */
     PREFIX(sleep),
     yield,
+#ifdef HAVE_PRIORITY_SCHEDULING
+    priority_yield,
+#endif
     &current_tick,
     default_event_handler,
     default_event_handler_ex,
@@ -306,6 +309,9 @@ static const struct plugin_api rockbox_api = {
     utf8seek,
 
     /* sound */
+#if CONFIG_CODEC == SWCODEC && defined(HAVE_RECORDING)
+    sound_default,
+#endif
     sound_set,
     set_sound,
 
@@ -321,17 +327,40 @@ static const struct plugin_api rockbox_api = {
 #endif
 #endif
 #if CONFIG_CODEC == SWCODEC
+    &audio_master_sampr_list[0],
+    &hw_freq_sampr[0],
+#ifndef SIMULATOR
+    pcm_apply_settings,
+#endif
     pcm_play_data,
     pcm_play_stop,
     pcm_set_frequency,
     pcm_is_playing,
+    pcm_is_paused,
     pcm_play_pause,
-#endif
-#if CONFIG_CODEC == SWCODEC
+    pcm_get_bytes_waiting,
     pcm_calculate_peaks,
+#ifdef HAVE_RECORDING
+    &rec_freq_sampr[0],
+#ifndef SIMULATOR
+    pcm_init_recording,
+    pcm_close_recording,
+    pcm_record_data,
+    pcm_record_more,
+    pcm_stop_recording,
+    pcm_calculate_rec_peaks,
+    audio_set_recording_gain,
+    audio_set_output_source,
+    rec_set_source,
+#endif
+#endif /* HAVE_RECORDING */
+
 #endif
 
     /* playback control */
+    playlist_amount,
+    playlist_resume,
+    playlist_start,
     PREFIX(audio_play),
     audio_stop,
     audio_pause,
@@ -340,7 +369,6 @@ static const struct plugin_api rockbox_api = {
     audio_prev,
     audio_ff_rewind,
     audio_next_track,
-    playlist_amount,
     audio_status,
     audio_has_changed_track,
     audio_current_track,
@@ -414,6 +442,9 @@ static const struct plugin_api rockbox_api = {
     plugin_get_buffer,
     plugin_get_audio_buffer,
     plugin_tsr,
+#ifdef IRAM_STEAL
+    plugin_iram_init,
+#endif
 #if defined(DEBUG) || defined(SIMULATOR)
     debugf,
 #endif
@@ -421,6 +452,7 @@ static const struct plugin_api rockbox_api = {
     _logf,
 #endif
     &global_settings,
+    &global_status,
     mp3info,
     count_mp3_frames,
     create_xing_header,
@@ -444,53 +476,16 @@ static const struct plugin_api rockbox_api = {
 #if LCD_DEPTH > 1
     lcd_get_backdrop,
 #endif
+
     /* new stuff at the end, sort into place next time
        the API gets incompatible */
 
-/* Keep these at the bottom till fully proven */
-#if CONFIG_CODEC == SWCODEC
-    &audio_master_sampr_list[0],
-    &hw_freq_sampr[0],
-#ifndef SIMULATOR
-    pcm_apply_settings,
-#endif
-#ifdef HAVE_RECORDING
-    &rec_freq_sampr[0],
-#ifndef SIMULATOR
-    pcm_init_recording,
-    pcm_close_recording,
-    pcm_record_data,
-    pcm_stop_recording,
-    pcm_calculate_rec_peaks,
-    audio_set_recording_gain,
-    audio_set_output_source,
-    rec_set_source,
-#endif
-#endif /* HAVE_RECORDING */
-#endif /* CONFIG_CODEC == SWCODEC */
-
-#ifdef IRAM_STEAL
-    plugin_iram_init,
-#endif
-
-#if CONFIG_CODEC == SWCODEC && defined(HAVE_RECORDING) && !defined(SIMULATOR)
-    sound_default,
-    pcm_record_more,
-#endif
-
 #ifdef IRIVER_H100_SERIES
     /* Routines for the iriver_flash -plugin. */
     detect_original_firmware,
     detect_flashed_ramimage,
     detect_flashed_romimage,
 #endif
-    playlist_resume,
-    playlist_start,
-    &global_status,
-
-#if CONFIG_CODEC == SWCODEC
-    pcm_get_bytes_waiting,
-#endif
 };
 
 int plugin_load(const char* plugin, void* parameter)
@@ -516,7 +511,7 @@ int plugin_load(const char* plugin, void* parameter)
     if (!p)
         p = plugin;
     action_signalscreenchange();
-    
+
     if (pfn_tsr_exit != NULL) /* if we have a resident old plugin: */
     {
         if (pfn_tsr_exit(!strcmp(current_plugin,p)) == false )
@@ -527,7 +522,7 @@ int plugin_load(const char* plugin, void* parameter)
         pfn_tsr_exit = NULL;
         plugin_loaded = false;
     }
-    
+
     gui_syncsplash(0, str(LANG_WAIT));
     strcpy(current_plugin,p);
 
@@ -590,7 +585,7 @@ int plugin_load(const char* plugin, void* parameter)
     xm = lcd_getxmargin();
     ym = lcd_getymargin();
     lcd_setmargins(0,0);
-    
+
 #if defined HAVE_LCD_BITMAP && LCD_DEPTH > 1
     old_backdrop = lcd_get_backdrop();
 #endif
@@ -609,7 +604,7 @@ int plugin_load(const char* plugin, void* parameter)
 
     rc = hdr->entry_point((struct plugin_api*) &rockbox_api, parameter);
     /* explicitly casting the pointer here to avoid touching every plugin. */
-    
+
     action_signalscreenchange();
     button_clear_queue();
 
diff --git a/apps/plugin.h b/apps/plugin.h
index ccc8d1c3af..51421fffb4 100644
--- a/apps/plugin.h
+++ b/apps/plugin.h
@@ -29,7 +29,7 @@
 #define MEM 2
 #endif
 
-#include <stdbool.h>                  
+#include <stdbool.h>
 #include <sys/types.h>
 #include <stdarg.h>
 #include <stdio.h>
@@ -110,12 +110,12 @@
 #define PLUGIN_MAGIC 0x526F634B /* RocK */
 
 /* increase this every time the api struct changes */
-#define PLUGIN_API_VERSION 51
+#define PLUGIN_API_VERSION 52
 
 /* update this to latest version if a change to the api struct breaks
    backwards compatibility (and please take the opportunity to sort in any
    new function which are "waiting" at the end of the function table) */
-#define PLUGIN_MIN_API_VERSION 51
+#define PLUGIN_MIN_API_VERSION 52
 
 /* plugin return codes */
 enum plugin_status {
@@ -322,6 +322,9 @@ struct plugin_api {
     /* kernel/ system */
     void (*PREFIX(sleep))(int ticks);
     void (*yield)(void);
+#ifdef HAVE_PRIORITY_SCHEDULING
+    void (*priority_yield)(void);
+#endif
     long* current_tick;
     long (*default_event_handler)(long event);
     long (*default_event_handler_ex)(long event, void (*callback)(void *), void *parameter);
@@ -401,6 +404,9 @@ struct plugin_api {
     int (*utf8seek)(const unsigned char* utf8, int offset);
 
     /* sound */
+#if CONFIG_CODEC == SWCODEC && defined(HAVE_RECORDING)
+    int (*sound_default)(int setting);
+#endif
     void (*sound_set)(int setting, int value);
     bool (*set_sound)(const unsigned char * string,
                       int* variable, int setting);
@@ -416,18 +422,42 @@ struct plugin_api {
 #endif
 #endif /* !SIMULATOR */
 #if CONFIG_CODEC == SWCODEC
+    const unsigned long *audio_master_sampr_list;
+    const unsigned long *hw_freq_sampr;
+#ifndef SIMULATOR
+    void (*pcm_apply_settings)(void);
+#endif
     void (*pcm_play_data)(pcm_more_callback_type get_more,
             unsigned char* start, size_t size);
     void (*pcm_play_stop)(void);
     void (*pcm_set_frequency)(unsigned int frequency);
     bool (*pcm_is_playing)(void);
+    bool (*pcm_is_paused)(void);
     void (*pcm_play_pause)(bool play);
-#endif
-#if CONFIG_CODEC == SWCODEC
+    size_t (*pcm_get_bytes_waiting)(void);
     void (*pcm_calculate_peaks)(int *left, int *right);
+#ifdef HAVE_RECORDING
+    const unsigned long *rec_freq_sampr;
+#ifndef SIMULATOR
+    void (*pcm_init_recording)(void);
+    void (*pcm_close_recording)(void);
+    void (*pcm_record_data)(pcm_more_callback_type2 more_ready,
+                            void *start, size_t size);
+    void (*pcm_record_more)(void *start, size_t size);
+    void (*pcm_stop_recording)(void);
+    void (*pcm_calculate_rec_peaks)(int *left, int *right);
+    void (*audio_set_recording_gain)(int left, int right, int type);
+    void (*audio_set_output_source)(int monitor);
+    void (*rec_set_source)(int source, unsigned flags);
+#endif
+#endif /* HAVE_RECORDING */
+
 #endif
 
     /* playback control */
+    int (*playlist_amount)(void);
+    int (*playlist_resume)(void);
+    int (*playlist_start)(int start_index, int offset);
     void (*PREFIX(audio_play))(long offset);
     void (*audio_stop)(void);
     void (*audio_pause)(void);
@@ -436,7 +466,6 @@ struct plugin_api {
     void (*audio_prev)(void);
     void (*audio_ff_rewind)(long newtime);
     struct mp3entry* (*audio_next_track)(void);
-    int (*playlist_amount)(void);
     int (*audio_status)(void);
     bool (*audio_has_changed_track)(void);
     struct mp3entry* (*audio_current_track)(void);
@@ -519,6 +548,10 @@ struct plugin_api {
     void* (*plugin_get_buffer)(int* buffer_size);
     void* (*plugin_get_audio_buffer)(int* buffer_size);
     void (*plugin_tsr)(bool (*exit_callback)(bool reenter));
+#ifdef IRAM_STEAL
+    void (*plugin_iram_init)(char *iramstart, char *iramcopy, size_t iram_size,
+                             char *iedata, size_t iedata_size);
+#endif
 #if defined(DEBUG) || defined(SIMULATOR)
     void (*debugf)(const char *fmt, ...) ATTRIBUTE_PRINTF(1, 2);
 #endif
@@ -526,6 +559,7 @@ struct plugin_api {
     void (*logf)(const char *fmt, ...) ATTRIBUTE_PRINTF(1, 2);
 #endif
     struct user_settings* global_settings;
+    struct system_status *global_status;
     bool (*mp3info)(struct mp3entry *entry, const char *filename, bool v1first);
     int (*count_mp3_frames)(int fd, int startpos, int filesize,
                             void (*progressfunc)(int));
@@ -560,52 +594,12 @@ struct plugin_api {
     /* new stuff at the end, sort into place next time
        the API gets incompatible */
 
-/* Keep these at the bottom till fully proven */
-#if CONFIG_CODEC == SWCODEC
-    const unsigned long *audio_master_sampr_list;
-    const unsigned long *hw_freq_sampr;
-#ifndef SIMULATOR
-    void (*pcm_apply_settings)(void);
-#endif
-#ifdef HAVE_RECORDING
-    const unsigned long *rec_freq_sampr;
-#ifndef SIMULATOR
-    void (*pcm_init_recording)(void);
-    void (*pcm_close_recording)(void);
-    void (*pcm_record_data)(pcm_more_callback_type2 more_ready,
-                            void *start, size_t size);
-    void (*pcm_stop_recording)(void);
-    void (*pcm_calculate_rec_peaks)(int *left, int *right);
-    void (*audio_set_recording_gain)(int left, int right, int type);
-    void (*audio_set_output_source)(int monitor);
-    void (*rec_set_source)(int source, unsigned flags);
-#endif
-#endif /* HAVE_RECORDING */
-#endif /* CONFIG_CODEC == SWCODEC */
-
-#ifdef IRAM_STEAL
-    void (*plugin_iram_init)(char *iramstart, char *iramcopy, size_t iram_size,
-                             char *iedata, size_t iedata_size);
-#endif
-
-#if CONFIG_CODEC == SWCODEC && defined(HAVE_RECORDING) && !defined(SIMULATOR)
-    int (*sound_default)(int setting);
-    void (*pcm_record_more)(void *start, size_t size);
-#endif
-    
 #ifdef IRIVER_H100_SERIES
     /* Routines for the iriver_flash -plugin. */
     bool (*detect_original_firmware)(void);
     bool (*detect_flashed_ramimage)(void);
     bool (*detect_flashed_romimage)(void);
 #endif
-    int (*playlist_resume)(void);
-    int (*playlist_start)(int start_index, int offset);
-    struct system_status *global_status;
-
-#if CONFIG_CODEC == SWCODEC
-    size_t (*pcm_get_bytes_waiting)(void);
-#endif
 };
 
 /* plugin header */
@@ -660,7 +654,7 @@ void plugin_iram_init(char *iramstart, char *iramcopy, size_t iram_size,
                       char *iedata, size_t iedata_size);
 #endif
 
-/* plugin_tsr, 
+/* plugin_tsr,
     callback returns true to allow the new plugin to load,
     reenter means the currently running plugin is being reloaded */
 void plugin_tsr(bool (*exit_callback)(bool reenter));
diff --git a/apps/plugins/mpegplayer/mpegplayer.c b/apps/plugins/mpegplayer/mpegplayer.c
index a2e22ed2bd..4fd9311a66 100644
--- a/apps/plugins/mpegplayer/mpegplayer.c
+++ b/apps/plugins/mpegplayer/mpegplayer.c
@@ -5,7 +5,7 @@
  *
  * Copyright (C) 2000-2003 Michel Lespinasse <walken@zoy.org>
  * Copyright (C) 1999-2000 Aaron Holtzman <aholtzma@ess.engr.uvic.ca>
- * 
+ *
  * m2psd: MPEG 2 Program Stream Demultiplexer
  * Copyright (C) 2003 Eric Smith <eric@brouhaha.com>
  *
@@ -108,6 +108,10 @@ FPS     | 27Mhz   | 100Hz          | 44.1KHz   | 48KHz
 #include "mpeg2.h"
 #include "mpeg_settings.h"
 #include "video_out.h"
+#ifndef ATTR_ALIGN
+#define ATTR_ALIGN(a) __attribute__((aligned (a)))
+#endif
+#include "mpeg2_internal.h"
 #include "../../codecs/libmad/mad.h"
 
 PLUGIN_HEADER
@@ -167,20 +171,58 @@ struct plugin_api* rb;
 static mpeg2dec_t * mpeg2dec;
 static int total_offset = 0;
 
+/* Utility */
+
+#define FF_I_TYPE 1 // Intra
+#define FF_P_TYPE 2 // Predicted
+#define FF_B_TYPE 3 // Bi-dir predicted
+#define FF_S_TYPE 4 // S(GMC)-VOP MPEG4
+#define FF_SI_TYPE 5
+#define FF_SP_TYPE 6
+
+/* Atomically add one long value to another - not core safe atm if ever needed */
+static inline void locked_add_long(volatile long *value, long amount)
+{
+#if defined (CPU_ARM)
+    /* Disable the fiq - this cuts an instruction out over using the
+       system functions */
+    long cpsr, x;
+    asm volatile (
+        "mrs    %[sr], cpsr           \r\n"
+        "orr    %[sr], %[sr], #0x40   \r\n"
+        "msr    cpsr_c, %[sr]         \r\n"
+        "ldr    %[x], [%[value]]      \r\n"
+        "add    %[x], %[x], %[amount] \r\n"
+        "str    %[x], [%[value]]      \r\n"
+        "bic    %[sr], %[sr], #0x40   \r\n"
+        "msr    cpsr_c, %[sr]         \r\n"
+        : [sr]"=&r"(cpsr), [x]"=&r"(x)
+        : [value]"r"(value), [amount]"r"(amount)
+        );
+#elif defined (CPU_COLDFIRE)
+    add_l(amount, value);
+#else
+    /* Don't know what this is so can't lock it */
+    *value += amount;
+#endif
+}
+
 /* Streams */
 typedef struct
 {
-   uint8_t* curr_packet;        /* Current stream packet beginning */
-   uint8_t* curr_packet_end;    /* Current stream packet end */
+    uint8_t* curr_packet;        /* Current stream packet beginning */
+    uint8_t* curr_packet_end;    /* Current stream packet end */
 
-   uint8_t* prev_packet;        /* Previous stream packet beginning */
-   uint8_t* next_packet;        /* Next stream packet beginning */
+    uint8_t* prev_packet;        /* Previous stream packet beginning */
+    uint8_t* next_packet;        /* Next stream packet beginning */
 
-   size_t guard_bytes;          /* Number of bytes in guardbuf used */
-   size_t buffer_remaining;     /* How much data is left in the buffer */ 
-   uint32_t first_pts;
-   uint32_t curr_pts;
-   int id;
+    size_t guard_bytes;          /* Number of bytes in guardbuf used */
+    size_t buffer_remaining;     /* How much data is left in the buffer */
+    uint32_t curr_pts;           /* Current presentation timestamp */
+    uint32_t curr_time;          /* Current time in samples */
+    uint32_t tagged;             /* curr_pts is valid */
+
+    int id;
 } Stream;
 
 static Stream audio_str IBSS_ATTR;
@@ -204,27 +246,35 @@ static struct thread_entry* audiothread_id;
 static struct thread_entry* videothread_id;
 
 /* Status */
-#define STREAM_PLAYING 0
-#define STREAM_DONE 1
-#define STREAM_PAUSING 2
-#define STREAM_BUFFERING 3
-#define STREAM_ERROR 4
-#define PLEASE_STOP 5
-#define PLEASE_PAUSE 6
+enum
+{
+    STREAM_PLAYING = 0,
+    STREAM_DONE,
+    STREAM_PAUSING,
+    STREAM_BUFFERING,
+    STREAM_ERROR,
+    PLEASE_STOP,
+    PLEASE_PAUSE,
+    THREAD_TERMINATED,
+};
 
 int audiostatus IBSS_ATTR;
 int videostatus IBSS_ATTR;
 
 /* Various buffers */
 /* TODO: Can we reduce the PCM buffer size? */
-#define PCMBUFFER_SIZE (512*1024)
-#define AUDIOBUFFER_SIZE (32*1024)
-#define LIBMPEG2BUFFER_SIZE (2*1024*1024)
-
-/* TODO: Is 32KB enough?  */
-#define MPEG_GUARDBUF_SIZE (32*1024)
+#define PCMBUFFER_SIZE              ((512*1024)-PCMBUFFER_GUARD_SIZE)
+#define PCMBUFFER_GUARD_SIZE        (1152*4 + sizeof (struct pcm_frame_header))
+#define MPA_MAX_FRAME_SIZE          1441 /* Largest frame with a padding byte */
+#define MPABUF_SIZE                 (64*1024 + ALIGN_UP(MPA_MAX_FRAME_SIZE + 2*MAD_BUFFER_GUARD, 4))
+#define LIBMPEG2BUFFER_SIZE         (2*1024*1024)
+
+/* 65536+6 is required since each PES has a 6 byte header with a 16 bit packet length field  */
+#define MPEG_GUARDBUF_SIZE (64*1024+16) /* Keep a bit extra */
 #define MPEG_LOW_WATERMARK (1024*1024)
 
+static void pcm_playback_play_pause(bool play);
+
 static void button_loop(void)
 {
     bool result;
@@ -257,7 +307,7 @@ static void button_loop(void)
 #endif
             vol = rb->global_settings->volume;
             minvol = rb->sound_min(SOUND_VOLUME);
-            
+
             if (vol > minvol) {
                 vol--;
                 rb->sound_set(SOUND_VOLUME, vol);
@@ -266,7 +316,7 @@ static void button_loop(void)
             break;
 
         case MPEG_MENU:
-            rb->pcm_play_pause(false);
+            pcm_playback_play_pause(false);
             if (videostatus != STREAM_DONE) {
                 videostatus=PLEASE_PAUSE;
 
@@ -291,7 +341,7 @@ static void button_loop(void)
                 if (videostatus != STREAM_DONE) videostatus = PLEASE_STOP;
             } else {
                 if (videostatus != STREAM_DONE) videostatus = STREAM_PLAYING;
-                rb->pcm_play_pause(true);
+                pcm_playback_play_pause(true);
             }
             break;
 
@@ -302,7 +352,7 @@ static void button_loop(void)
 
         case MPEG_PAUSE:
             if (videostatus != STREAM_DONE) videostatus=PLEASE_PAUSE;
-            rb->pcm_play_pause(false);
+            pcm_playback_play_pause(false);
 
             button = BUTTON_NONE;
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
@@ -318,7 +368,7 @@ static void button_loop(void)
             } while (button != MPEG_PAUSE);
 
             if (videostatus != STREAM_DONE) videostatus = STREAM_PLAYING;
-            rb->pcm_play_pause(true);
+            pcm_playback_play_pause(true);
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
             rb->cpu_boost(true);
 #endif
@@ -358,188 +408,276 @@ static void init_mad(void* mad_frame_overlap)
 }
 
 /* MPEG related headers */
-uint8_t packet_start_code_prefix [3] = { 0x00, 0x00, 0x01 };
-uint8_t end_code [4] =                 { 0x00, 0x00, 0x01, 0xb9 };
-uint8_t pack_start_code [4] =          { 0x00, 0x00, 0x01, 0xba };
-uint8_t system_header_start_code [4] = { 0x00, 0x00, 0x01, 0xbb };
 
-/* This function demux the streams and give the next stream data pointer */
+/* Macros for comparing memory bytes to a series of constant bytes in an
+   efficient manner - evaluate to true if corresponding bytes match */
+#if defined (CPU_ARM)
+/* ARM must load 32-bit values at addres % 4 == 0 offsets but this data
+   isn't aligned nescessarily, so just byte compare */
+#define CMP_3_CONST(_a, _b) \
+    ({                                      \
+        int _x;                             \
+        asm volatile (                      \
+            "ldrb   %[x], [%[a], #0]  \r\n" \
+            "eors   %[x], %[x], %[b0] \r\n" \
+            "ldreqb %[x], [%[a], #1]  \r\n" \
+            "eoreqs %[x], %[x], %[b1] \r\n" \
+            "ldreqb %[x], [%[a], #2]  \r\n" \
+            "eoreqs %[x], %[x], %[b2] \r\n" \
+            : [x]"=&r"(_x)                  \
+            : [a]"r"(_a),                   \
+              [b0]"i"((_b)       >> 24),    \
+              [b1]"i"((_b) << 8  >> 24),    \
+              [b2]"i"((_b) << 16 >> 24)     \
+        );                                  \
+        _x == 0;                            \
+    })
+#define CMP_4_CONST(_a, _b) \
+    ({                                      \
+        int _x;                             \
+        asm volatile (                      \
+            "ldrb   %[x], [%[a], #0]  \r\n" \
+            "eors   %[x], %[x], %[b0] \r\n" \
+            "ldreqb %[x], [%[a], #1]  \r\n" \
+            "eoreqs %[x], %[x], %[b1] \r\n" \
+            "ldreqb %[x], [%[a], #2]  \r\n" \
+            "eoreqs %[x], %[x], %[b2] \r\n" \
+            "ldreqb %[x], [%[a], #3]  \r\n" \
+            "eoreqs %[x], %[x], %[b3] \r\n" \
+            : [x]"=&r"(_x)                  \
+            : [a]"r"(_a),                   \
+              [b0]"i"((_b)       >> 24),    \
+              [b1]"i"((_b) <<  8 >> 24),    \
+              [b2]"i"((_b) << 16 >> 24),    \
+              [b3]"i"((_b) << 24 >> 24)     \
+        );                                  \
+        _x == 0;                            \
+    })
+#elif defined (CPU_COLDFIRE)
+/* Coldfire can just load a 32 bit value at any offset but ASM is not the best way
+   to integrate this with the C code */
+#define CMP_3_CONST(a, b) \
+    (((*(uint32_t *)(a) >> 8) ^ ((uint32_t)(b) >> 8)) == 0)
+#define CMP_4_CONST(a, b) \
+    ((*(uint32_t *)(a) ^ (b)) == 0)
+#else
+/* Don't know what this is - use bytewise comparisons */
+#define CMP_3_CONST(a, b) \
+    (( ((a)[0] ^ ((b)       >> 24)) | \
+       ((a)[1] ^ ((b) <<  8 >> 24)) | \
+       ((a)[2] ^ ((b) << 16 >> 24)) ) == 0)
+#define CMP_4_CONST(a, b) \
+    (( ((a)[0] ^ ((b)       >> 24)) | \
+       ((a)[1] ^ ((b) <<  8 >> 24)) | \
+       ((a)[2] ^ ((b) << 16 >> 24)) | \
+       ((a)[3] ^ ((b) << 24 >> 24)) ) == 0)
+#endif
+
+/* Codes for various header byte sequences - MSB represents lowest memory
+   address */
+#define PACKET_START_CODE_PREFIX    0x00000100ul
+#define END_CODE                    0x000001b9ul
+#define PACK_START_CODE             0x000001baul
+#define SYSTEM_HEADER_START_CODE    0x000001bbul
+
+/* p = base pointer, b0 - b4 = byte offsets from p */
+#define TS_FROM_HEADER(p, b0, b1, b2, b3, b4) \
+    ((uint32_t)(((p)[b0] >> 1 << 30) | \
+                ((p)[b1]      << 22) | \
+                ((p)[b2] >> 1 << 15) | \
+                ((p)[b3]      <<  7) | \
+                ((p)[b4] >> 1      )))
+
+/* This function demuxes the streams and gives the next stream data pointer */
 static void get_next_data( Stream* str )
 {
     uint8_t *p;
     uint8_t *header;
     int stream;
-    
-    static int mpeg1_skip_table[16] = {
-        0, 0, 4, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
-    };
 
-    if (str->curr_packet_end == NULL) {
+    static int mpeg1_skip_table[16] =
+        { 0, 0, 4, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+    if (str->curr_packet_end == NULL)
+    {
         /* What does this do? */
-        while( (p = disk_buf) == NULL )
+        while ((p = disk_buf) == NULL)
         {
             rb->lcd_putsxy(0,LCD_HEIGHT-10,"FREEZE!");
             rb->lcd_update();
-            rb->sleep(100);
+            rb->sleep(HZ);
         }
-    } else {
+    }
+    else
+    {
         p = str->curr_packet_end;
     }
 
-    for( ;; )
+    while (1)
     {
         int length, bytes;
-        
-            if( p >= disk_buf_end )
-            {
-                p = disk_buf + (p - disk_buf_end);
-            }
+
+        if (p >= disk_buf_end)
+        {
+            p = disk_buf + (p - disk_buf_end);
+        }
 
         /* Pack header, skip it */
-        if( rb->memcmp (p, pack_start_code, sizeof (pack_start_code)) == 0 )
+        if (CMP_4_CONST(p, PACK_START_CODE))
         {
-            if ((p[4] & 0xc0) == 0x40) { /* mpeg-2 */
+            if ((p[4] & 0xc0) == 0x40)      /* mpeg-2 */
+            {
                 p += 14 + (p[13] & 7);
-            } else if ((p[4] & 0xf0) == 0x20) { /* mpeg-1 */
+            }
+            else if ((p[4] & 0xf0) == 0x20) /* mpeg-1 */
+            {
                 p += 12;
-            } else {
+            }
+            else
+            {
                 rb->splash( 30, "Weird Pack header!" );
                 p += 5;
             }
             /*rb->splash( 30, "Pack header" );*/
         }
 
-        /* System header, parse and skip it */
-        if( rb->memcmp (p, system_header_start_code, sizeof (system_header_start_code)) == 0 )
+        /* System header, parse and skip it - four bytes */
+        if (CMP_4_CONST(p, SYSTEM_HEADER_START_CODE))
         {
             int header_length;
 
             p += 4;  /*skip start code*/
-            header_length = (*(p++)) << 8;
-            header_length += *(p++);
+            header_length = *p++ << 8;
+            header_length += *p++;
 
             p += header_length;
             /*rb->splash( 30, "System header" );*/
         }
-        
+
         /* Packet header, parse it */
-        if( rb->memcmp (p, packet_start_code_prefix, sizeof (packet_start_code_prefix)) != 0 )
+        if (!CMP_3_CONST(p, PACKET_START_CODE_PREFIX))
         {
             /* Problem */
             //rb->splash( HZ*3, "missing packet start code prefix : %X%X at %X", *p, *(p+2), p-disk_buf );
             str->curr_packet_end = str->curr_packet = NULL;
             return;
             //++p;
-            break;
-        }        
-        
+            //break;
+        }
+
         /* We retrieve basic infos */
-        stream = *(p+3);
-        length = (*(p+4)) << 8;
-        length += *(p+5);
-        
+        stream = p[3];
+        length = (p[4] << 8) | p[5];
+
         /*rb->splash( 100, "Stream : %X", stream );*/
         if (stream != str->id)
         {
             /* End of stream ? */
-            if( stream == 0xB9 )
+            if (stream == 0xB9)
             {
                 str->curr_packet_end = str->curr_packet = NULL;
                 return;
             }
-            
-            /* It's not the packet we're looking for, skip it */            
-            p += length+6;
+
+            /* It's not the packet we're looking for, skip it */
+            p += length + 6;
             continue;
         }
-        
+
         /* Ok, it's our packet */
-        str->curr_packet_end = p + length+6;    
+        str->curr_packet_end = p + length+6;
         header = p;
-        if ((header[6] & 0xc0) == 0x80) {    /* mpeg2 */
-        
-            length = 9 + header[8];
-            /* header points to the mpeg2 pes header */
-            if (header[7] & 0x80) {
-                uint32_t pts, dts;
 
-                pts = (((header[9] >> 1) << 30) |
-                       (header[10] << 22) | ((header[11] >> 1) << 15) |
-                       (header[12] << 7) | (header[13] >> 1));
+        if ((header[6] & 0xc0) == 0x80) /* mpeg2 */
+        {
+            length = 9 + header[8];
 
-                if (str->first_pts==0)
-                    str->first_pts = pts;
+            /* header points to the mpeg2 pes header */
+            if (header[7] & 0x80)
+            {
+                uint32_t pts = TS_FROM_HEADER(header, 9, 10, 11, 12, 13);
 
-                str->curr_pts = pts;
+                if (stream >= 0xe0)
+                {
+                    uint32_t dts = (header[7] & 0x40) == 0 ?
+                        pts : TS_FROM_HEADER(header, 14, 15, 16, 17, 18);
 
-                dts = (!(header[7] & 0x40) ? pts :
-                       ((uint32_t)(((header[14] >> 1) << 30) |
-                    (header[15] << 22) |
-                    ((header[16] >> 1) << 15) |
-                    (header[17] << 7) | (header[18] >> 1))));
-                
-                if( stream >= 0xe0 )
                     mpeg2_tag_picture (mpeg2dec, pts, dts);
+                }
+                else
+                {
+                    str->curr_pts = pts;
+                    str->tagged = 1;
+                }
             }
-        } else {    /* mpeg1 */
+        }
+        else                            /* mpeg1 */
+        {
             int len_skip;
             uint8_t * ptsbuf;
 
             length = 7;
-            while (header[length - 1] == 0xff) 
+
+            while (header[length - 1] == 0xff)
             {
                 length++;
-                if (length > 23) 
+                if (length > 23)
                 {
                     rb->splash( 30, "Too much stuffing" );
                     DEBUGF("Too much stuffing" );
                     break;
                 }
             }
-            if ((header[length - 1] & 0xc0) == 0x40) 
+
+            if ((header[length - 1] & 0xc0) == 0x40)
             {
                 length += 2;
             }
+
             len_skip = length;
             length += mpeg1_skip_table[header[length - 1] >> 4];
 
             /* header points to the mpeg1 pes header */
             ptsbuf = header + len_skip;
+
             if ((ptsbuf[-1] & 0xe0) == 0x20)
             {
-                uint32_t pts, dts;
-
-                pts = (((ptsbuf[-1] >> 1) << 30) |
-                       (ptsbuf[0] << 22) | ((ptsbuf[1] >> 1) << 15) |
-                       (ptsbuf[2] << 7) | (ptsbuf[3] >> 1));
-
-                if (str->first_pts==0)
-                    str->first_pts = pts;
+                uint32_t pts = TS_FROM_HEADER(ptsbuf, -1, 0, 1, 2, 3);
 
-                str->curr_pts = pts;
-
-                dts = (((ptsbuf[-1] & 0xf0) != 0x30) ? pts :
-                       ((uint32_t)(((ptsbuf[4] >> 1) << 30) |
-                    (ptsbuf[5] << 22) | ((ptsbuf[6] >> 1) << 15) |
-                    (ptsbuf[7] << 7) | (ptsbuf[18] >> 1))));
+                if (stream >= 0xe0)
+                {
+                    uint32_t dts = (ptsbuf[-1] & 0xf0) != 0x30 ?
+                        pts : TS_FROM_HEADER(ptsbuf, 4, 5, 6, 7, 18);
 
-                if( stream >= 0xe0 )    
                     mpeg2_tag_picture (mpeg2dec, pts, dts);
+                }
+                else
+                {
+                    str->curr_pts = pts;
+                    str->tagged = 1;
+                }
             }
         }
 
         p += length;
         bytes = 6 + (header[4] << 8) + header[5] - length;
-        if (bytes > 0) {
-            str->curr_packet_end = p+bytes;
+
+        if (bytes > 0)
+        {
+            str->curr_packet_end = p + bytes;
             //DEBUGF("prev = %d, curr = %d\n",str->prev_packet,str->curr_packet);
 
-            if (str->curr_packet != NULL) {
-                if (str->curr_packet < str->prev_packet) {
-                    str->buffer_remaining -= (disk_buf_end - str->prev_packet) + (str->curr_packet - disk_buf);
+            if (str->curr_packet != NULL)
+            {
+                if (str->curr_packet < str->prev_packet)
+                {
+                    str->buffer_remaining -= (disk_buf_end - str->prev_packet) +
+                                             (str->curr_packet - disk_buf);
                     str->buffer_remaining -= str->guard_bytes;
                     str->guard_bytes = 0;
-                } else {
+                }
+                else
+                {
                     str->buffer_remaining -= (str->curr_packet - str->prev_packet);
                 }
 
@@ -548,265 +686,534 @@ static void get_next_data( Stream* str )
 
             str->curr_packet = p;
 
-            if( str->curr_packet_end > disk_buf_end )
+            if (str->curr_packet_end > disk_buf_end)
             {
-                str->guard_bytes = str->curr_packet_end-disk_buf_end;
-                rb->memcpy(disk_buf_end,disk_buf,str->guard_bytes);
+                str->guard_bytes = str->curr_packet_end - disk_buf_end;
+                rb->memcpy(disk_buf_end, disk_buf, str->guard_bytes);
             }
-            return ;
+
+            return;
         }
-                
+
         break;
-    }    
+    } /* end while */
 }
 
+/* Our clock rate in ticks/second - this won't be a constant for long */
+#define CLOCK_RATE 44100
+
+/* For simple lowpass filtering of sync variables */
+#define AVERAGE(var, x, count) (((var) * (count-1) + (x)) / (count))
+/* Convert 90kHz PTS/DTS ticks to our clock ticks */
+#define TS_TO_TICKS(pts) ((uint64_t)CLOCK_RATE*(pts) / 90000)
+/* Convert 27MHz ticks to our clock ticks */
+#define TIME_TO_TICKS(stamp) ((uint64_t)CLOCK_RATE*(stamp) / 27000000)
+
+/** MPEG audio stream buffer */
 uint8_t* mpa_buffer;
-size_t mpa_buffer_size;
 
-static volatile int madpcm_playing IBSS_ATTR;
-static volatile int16_t* pcm_buffer IBSS_ATTR;
-static volatile size_t pcm_buffer_size IBSS_ATTR;
+static bool init_mpabuf(void)
+{
+    mpa_buffer = mpeg2_malloc(MPABUF_SIZE,-2);
+    return mpa_buffer != NULL;
+}
 
-static volatile size_t pcmbuf_len IBSS_ATTR;
-static volatile int16_t* pcmbuf_end IBSS_ATTR;
-static volatile int16_t* pcmbuf_head IBSS_ATTR;
-static volatile int16_t* pcmbuf_tail IBSS_ATTR;
+#define PTS_QUEUE_LEN  (1 << 4) /* 16 should be way more than sufficient -
+                                   if not, the case is handled */
+#define PTS_QUEUE_MASK (PTS_QUEUE_LEN-1)
+struct pts_queue_slot
+{
+    uint32_t pts;   /* Time stamp for packet          */
+    ssize_t  size;  /* Number of bytes left in packet */
+} pts_queue[PTS_QUEUE_MASK+1];
+
+ /* This starts out wr == rd but will never be emptied to zero during
+    streaming again in order to support initializing the first packet's
+    pts value without a special case */
+static unsigned pts_queue_rd;
+static unsigned pts_queue_wr;
+
+/* Resets the pts queue - call when starting and seeking */
+static void pts_queue_reset(void)
+{
+    pts_queue_rd = pts_queue_wr;
+}
+
+/* Increments the queue head postion - should be used to preincrement */
+static bool pts_queue_add_head(void)
+{
+    if (pts_queue_wr - pts_queue_rd >= PTS_QUEUE_LEN-1)
+        return false;
+
+    pts_queue_wr++;
+    return true;
+}
+
+/* Increments the queue tail position - leaves one slot as current */
+static bool pts_queue_remove_tail(void)
+{
+    if (pts_queue_wr - pts_queue_rd <= 1u)
+        return false;
+
+    pts_queue_rd++;
+    return true;
+}
+
+/* Returns the "head" at the index just behind the write index */
+static struct pts_queue_slot * pts_queue_head(void)
+{
+    return &pts_queue[(pts_queue_wr - 1) & PTS_QUEUE_MASK];
+}
 
-static volatile uint32_t samplesplayed IBSS_ATTR;
-static volatile int delay IBSS_ATTR;
+/* Returns a pointer to the current tail */
+static struct pts_queue_slot * pts_queue_tail(void)
+{
+    return &pts_queue[pts_queue_rd & PTS_QUEUE_MASK];
+}
 
-static void init_pcmbuf(void)
+struct pcm_frame_header     /* Header added to pcm data every time a decoded
+                               mpa frame is sent out */
+{
+    uint32_t size;          /* size of this frame - including header */
+    uint32_t time;          /* timestamp for this frame - derived from PTS */
+    unsigned char data[];   /* open array of audio data */
+};
+
+#define PCMBUF_PLAY_ALL         1l          /* Forces buffer to play back all data */
+#define PCMBUF_PLAY_NONE        LONG_MAX    /* Keeps buffer from playing any data */
+static volatile ssize_t         pcmbuf_used      IBSS_ATTR;
+static volatile ssize_t         pcmbuf_threshold IBSS_ATTR;
+static struct pcm_frame_header *pcm_buffer       IBSS_ATTR;
+static struct pcm_frame_header *pcmbuf_end       IBSS_ATTR;
+static struct pcm_frame_header * volatile pcmbuf_head IBSS_ATTR;
+static struct pcm_frame_header * volatile pcmbuf_tail IBSS_ATTR;
+
+static volatile uint32_t samplesplayed IBSS_ATTR; /* Our base clock */
+static          uint32_t samplestart   IBSS_ATTR; /* Clock at playback start */
+static volatile int32_t  sampleadjust  IBSS_ATTR; /* Clock drift adjustment */
+
+static bool init_pcmbuf(void)
 {
+    pcm_buffer = mpeg2_malloc(PCMBUFFER_SIZE + PCMBUFFER_GUARD_SIZE, -2);
+
+    if (pcm_buffer == NULL)
+        return false;
+
     pcmbuf_head = pcm_buffer;
-    pcmbuf_len = 0;
     pcmbuf_tail = pcm_buffer;
-    pcmbuf_end = pcm_buffer + pcm_buffer_size / sizeof(int16_t);
-    madpcm_playing = 0;
+    pcmbuf_end  = SKIPBYTES(pcm_buffer, PCMBUFFER_SIZE);
+    pcmbuf_used = 0;
+
+    return true;
+}
+
+/* Advance a PCM buffer pointer by size bytes circularly */
+static inline void pcm_advance_buffer(struct pcm_frame_header * volatile *p,
+                                      size_t size)
+{
+    *p = SKIPBYTES(*p, size);
+    if (*p >= pcmbuf_end)
+        *p = pcm_buffer;
 }
 
 static void get_more(unsigned char** start, size_t* size)
 {
-   if (pcmbuf_len < 32*1024) {
-      *start = NULL;
-      *size = 0;
-      madpcm_playing = 0;
-      pcmbuf_len = 0;
-   } else {
-      *start = (unsigned char*)(pcmbuf_tail);
-      *size = 32*1024;
-      pcmbuf_tail += (32*1024)/sizeof(int16_t);
-      pcmbuf_len -= 32*1024;
-      if (pcmbuf_tail >= pcmbuf_end) { pcmbuf_tail = pcm_buffer; }
-
-      /* Update master clock */
-      samplesplayed += (32*1024)/4;
-   }
+    /* 25ms @ 44.1kHz */
+    static unsigned char silence[4412] __attribute__((aligned (4))) = { 0 };
+    size_t sz;
+
+    if (pcmbuf_used >= pcmbuf_threshold)
+    {
+        uint32_t time = pcmbuf_tail->time;
+        sz = pcmbuf_tail->size;
+
+        *start = (unsigned char *)pcmbuf_tail->data;
+
+        pcm_advance_buffer(&pcmbuf_tail, sz);
+
+        pcmbuf_used -= sz;
+
+        sz -= sizeof (*pcmbuf_tail);
+
+        *size = sz;
+
+        /* Drift the clock towards the audio timestamp values */
+        sampleadjust = AVERAGE(sampleadjust, (int32_t)(time - samplesplayed), 8);
+
+        /* Update master clock */
+        samplesplayed += sz >> 2;
+        return;
+    }
+
+    /* Keep clock going at all times */
+    sz = sizeof (silence);
+    *start = silence;
+    *size  = sz;
+
+    samplesplayed += sz >> 2;
+
+    if (pcmbuf_used < 0)
+        pcmbuf_used = 0;
+}
+
+/* Flushes the buffer - clock keeps counting */
+static void pcm_playback_flush(void)
+{
+    bool was_playing = rb->pcm_is_playing();
+
+    if (was_playing)
+        rb->pcm_play_stop();
+
+    pcmbuf_used = 0;
+    pcmbuf_head = pcmbuf_tail;
+
+    if (was_playing)
+        rb->pcm_play_data(get_more, NULL, 0);
+}
+
+/* Seek the reference clock to the specified time - next audio data ready to
+   go to DMA should be on the buffer with the same time index or else the PCM
+   buffer should be empty */
+static void pcm_playback_seek_time(uint32_t time)
+{
+    bool was_playing = rb->pcm_is_playing();
+
+    if (was_playing)
+        rb->pcm_play_stop();
+
+    samplesplayed = time;
+    samplestart   = time;
+    sampleadjust  = 0;
+
+    if (was_playing)
+        rb->pcm_play_data(get_more, NULL, 0);
+}
+
+/* Start pcm playback with the reference clock set to the specified time */
+static void pcm_playback_play(uint32_t time)
+{
+    pcm_playback_seek_time(time);
+
+    if (!rb->pcm_is_playing())
+        rb->pcm_play_data(get_more, NULL, 0);
+}
+
+/* Pauses playback - and the clock */
+static void pcm_playback_play_pause(bool play)
+{
+    rb->pcm_play_pause(play);
+}
+
+/* Stops all playback and resets the clock */
+static void pcm_playback_stop(void)
+{
+    if (rb->pcm_is_playing())
+        rb->pcm_play_stop();
+
+    pcm_playback_flush();
+
+    sampleadjust  =
+    samplestart   =
+    samplesplayed = 0;
+}
+
+static uint32_t get_stream_time(void)
+{
+    return samplesplayed + sampleadjust - (rb->pcm_get_bytes_waiting() >> 2);
+}
+
+static uint32_t get_playback_time(void)
+{
+    return samplesplayed + sampleadjust -
+            samplestart - (rb->pcm_get_bytes_waiting() >> 2);
 }
 
-int line;
+static inline int32_t clip_sample(int32_t sample)
+{
+    if ((int16_t)sample != sample)
+        sample = 0x7fff ^ (sample >> 31);
+
+    return sample;
+}
 
+static void audio_thread(void) __attribute__((noreturn));
 static void audio_thread(void)
 {
-    int32_t* left;
-    int32_t* right;
-    int32_t sample;
-    int i;
-    size_t n = 0;
-    size_t len;
-    int file_end = 0;  /* A count of the errors in each frame */
-    int framelength;
-    int found_avdelay = 0;
-    int avdelay = 0;   /* Number of audio samples difference between first audio and video PTS values. */
-    int64_t apts_samples;
-    uint32_t samplesdecoded = 0;
+    uint8_t *mpabuf = mpa_buffer;
+    ssize_t mpabuf_used = 0;
+    int mad_errors = 0;  /* A count of the errors in each frame */
+    struct pts_queue_slot *pts;
 
     /* We need this here to init the EMAC for Coldfire targets */
     mad_synth_init(&synth);
 
-    init_pcmbuf();
+    /* Init pts queue */
+    pts_queue_reset();
+    pts = pts_queue_tail();
+
+    /* Keep buffer from playing */
+    pcmbuf_threshold = PCMBUF_PLAY_NONE;
+
+    /* Start clock */
+    pcm_playback_play(0);
+
+    /* Get first packet */
+    get_next_data(&audio_str);
+
+    if (audio_str.curr_packet == NULL)
+        goto done;
 
     /* This is the decoding loop. */
-    for (;;) {
-        button_loop();
+    while (1)
+    {
+        int mad_stat;
+        size_t len;
 
-        if (!found_avdelay) {
-            if ((audio_str.first_pts != 0) && (video_str.first_pts != 0)) {
-                avdelay = audio_str.first_pts - video_str.first_pts;
-                avdelay *= 44100;
-                avdelay /= 90000;
-                found_avdelay = 1;
-                DEBUGF("First Audio PTS = %u, First Video PTS=%u, A-V=%d samples\n",(unsigned int)audio_str.first_pts,(unsigned int)video_str.first_pts,avdelay);
-            }
-        }
+        button_loop();
 
-        if (audiostatus == PLEASE_STOP) {
+        if (audiostatus == PLEASE_STOP)
            goto done;
+
+        /** Buffering **/
+        if (mpabuf_used >= MPA_MAX_FRAME_SIZE + MAD_BUFFER_GUARD)
+        {
+            /* Above low watermark - do nothing */
         }
+        else if (audio_str.curr_packet != NULL)
+        {
+            /* Get data from next audio packet */
+            len = audio_str.curr_packet_end - audio_str.curr_packet;
 
-        if (n < 1500) {  /* TODO: What is the maximum size of an MPEG audio frame? */
-            get_next_data( &audio_str );
-            if (audio_str.curr_packet == NULL) {
-                /* Wait for audio to finish */
-                while (pcmbuf_len > 0) { rb->sleep(HZ/10); }
-                goto done;
-            }
+            if (audio_str.tagged)
+            {
+                struct pts_queue_slot *stamp = pts;
 
-            len = audio_str.curr_packet_end - audio_str.curr_packet;
-            if (n + len > mpa_buffer_size) { 
-                rb->splash( 30, "Audio buffer overflow" );
-                DEBUGF("Audio buffer overflow" );
-                audiostatus=STREAM_DONE;
-                /* Wait to be killed */
-                for (;;) { rb->sleep(HZ); }
+                if (pts_queue_add_head())
+                {
+                    stamp = pts_queue_head();
+                    stamp->pts = TS_TO_TICKS(audio_str.curr_pts);
+                    /* pts->size should have been zeroed when slot was freed */
+                }
+                /* else queue full - just count up from the last to make it look
+                   like more data in the same packet */
+                stamp->size += len;
+                audio_str.tagged = 0;
             }
-            rb->memcpy(mpa_buffer+n,audio_str.curr_packet,len);
-            n += len;
+            else
+            {
+                /* Add to the one just behind the head - this may be the tail or
+                   the previouly added head - whether or not we'll ever reach this
+                   is quite in question since audio always seems to have every
+                   packet timestamped */
+                pts_queue_head()->size += len;
+            }
+
+            /* Slide any remainder over to beginning - avoid function call overhead if
+               no data remaining as well */
+            if (mpabuf > mpa_buffer && mpabuf_used > 0)
+                rb->memmove(mpa_buffer, mpabuf, mpabuf_used);
+
+            /* Splice this packet onto any remainder */
+            rb->memcpy(mpa_buffer + mpabuf_used, audio_str.curr_packet, len);
+
+            mpabuf_used += len;
+            mpabuf = mpa_buffer;
+
+            /* Move stream position to the next packet */
+            get_next_data(&audio_str);
         }
-            
-        /* Lock buffers */
-        if (stream.error == 0) {
-            mad_stream_buffer(&stream, mpa_buffer, n);
+        else if (mpabuf_used <= 0)
+        {
+            /* Used up remainder of mpa buffer so quit */
+            break;
         }
 
-        if (mad_frame_decode(&frame, &stream)) {
-            DEBUGF("Audio stream error - %d\n",stream.error);
+        /** Decoding **/
+        if (stream.error == 0)
+            mad_stream_buffer(&stream, mpabuf, mpabuf_used);
+
+        mad_stat = mad_frame_decode(&frame, &stream);
+
+        /* Next mad stream buffer is the next frame postion */
+        mpabuf = (uint8_t *)stream.next_frame;
+
+        /* Adjust sizes by the frame size */
+        len = stream.next_frame - stream.this_frame;
+        mpabuf_used -= len;
+        pts->size -= len;
+
+        if (pts->size <= 0)
+        {
+            /* Carry any overshoot to the next size since we're technically
+               -pts->size bytes into it already. If size is negative an audio
+               frame was split accross packets. Old has to be saved before
+               moving the tail. */
+            struct pts_queue_slot *old = pts;
+
+            if (pts_queue_remove_tail())
+            {
+                pts = pts_queue_tail();
+                pts->size += old->size;
+            }
+
+            /* Zero the size of the last slot to simplify stamping code and ensure
+               that it is never negative if the tail didn't move */
+            old->size = 0;
+        }
+
+        if (mad_stat != 0)
+        {
+            DEBUGF("Audio stream error - %d\n", stream.error);
+
             if (stream.error == MAD_FLAG_INCOMPLETE
-                || stream.error == MAD_ERROR_BUFLEN) {
+                || stream.error == MAD_ERROR_BUFLEN)
+            {
                 /* This makes the codec support partially corrupted files */
-                if (file_end == 30)
+                if (mad_errors >= 30)
                     break;
 
-#if 0
-                /* The mpa.c version: */
-                if (stream.next_frame)
-                    inputbuffer = stream.next_frame;
-                else
-                    inputbuffer++;
-#endif
-
                 stream.error = 0;
-                file_end++;
+                mad_errors++;
+                rb->priority_yield();
                 continue;
-            } else if (MAD_RECOVERABLE(stream.error)) {
+            }
+            else if (MAD_RECOVERABLE(stream.error))
+            {
+                rb->priority_yield();
                 continue;
-            } else {
+            }
+            else
+            {
                 /* Some other unrecoverable error */
                 DEBUGF("Unrecoverable error\n");
-                break;
             }
+
             break;
         }
 
-        file_end = 0;
+        mad_errors = 0; /* Clear errors */
 
+        /* Generate the pcm samples */
         mad_synth_frame(&synth, &frame);
 
-        /* TODO: Don't memmove so much... */
-        if (stream.next_frame) {
-            len = stream.next_frame - mpa_buffer;
-            rb->memmove(mpa_buffer,stream.next_frame,n-len);
-            n -= len;
-        } else {
+        if (stream.next_frame == NULL)
+        {
             /* What to do here? */
             DEBUGF("/* What to do here? */\n");
-            goto done;
+            break;
         }
-#if 0
-        /* The mpa.c version: */
-        if (stream.next_frame)
-            inputbuffer = stream.next_frame;
-        else
-            inputbuffer = inputbuffer_end;
-#endif
 
-        framelength = synth.pcm.length; 
-        samplesdecoded += framelength;
+        /** Output **/
 
-        if (found_avdelay) {
-            apts_samples = (audio_str.curr_pts-audio_str.first_pts);
-            apts_samples *= 44100;
-            apts_samples /= 90000;
-            delay = (int)(avdelay+apts_samples-samplesdecoded);
-            //DEBUGF("delay=%d\n",delay);
-        }
+        /* TODO: Output through core dsp. We'll still use our own PCM buffer
+           since the core pcm buffer has no timestamping or clock facilities */
+
+        /* Add a frame of audio to the pcm buffer. Maximum is 1152 samples. */
+        if (synth.pcm.length > 0)
+        {
+            int16_t *audio_data = (int16_t *)pcmbuf_head->data;
+            size_t size = sizeof (*pcmbuf_head) + synth.pcm.length*4;
+            size_t wait_for = size + 32*1024;
+
+            /* Leave at least 32KB free (this will be the currently
+               playing chunk) */
+            while (pcmbuf_used + wait_for > PCMBUFFER_SIZE)
+            {
+                if (audiostatus == PLEASE_STOP)
+                    goto done;
+                rb->priority_yield();
+            }
 
-        if (framelength > 0) {
-            /* Leave at least 32KB free (this will be the currently playing chunk) */
-            while (pcmbuf_len + framelength*4 + 32*1024 > pcm_buffer_size) { rb->yield(); }
+            /* TODO: This part will be replaced with dsp calls soon */
+            if (MAD_NCHANNELS(&frame.header) == 2)
+            {
+                int32_t *left = &synth.pcm.samples[0][0];
+                int32_t *right = &synth.pcm.samples[1][0];
+                int i = synth.pcm.length;
 
-            if (MAD_NCHANNELS(&frame.header) == 2) {
-                left = &synth.pcm.samples[0][0];
-                right = &synth.pcm.samples[1][0];
-                for (i = 0 ; i < framelength; i++) {
+                do
+                {
                     /* libmad outputs s3.28 */
-                    sample = *(left++) >> 13;
-                    if (sample > 32767)
-                        sample = 32767;
-                    else if (sample < -32768)
-                        sample = -32768;
-                    *(pcmbuf_head++) = sample;
-
-                    sample = *(right++) >> 13;
-                    if (sample > 32767)
-                        sample = 32767;
-                    else if (sample < -32768)
-                        sample = -32768;
-                    *(pcmbuf_head++) = sample;
-
-                    if (pcmbuf_head >= pcmbuf_end) { pcmbuf_head = pcm_buffer; }
+                    *audio_data++ = clip_sample(*left++ >> 13);
+                    *audio_data++ = clip_sample(*right++ >> 13);
                 }
-            } else { /* mono */
-                left = &synth.pcm.samples[0][0];
-                for (i = 0 ; i < framelength; i++) {
-                    sample = *(left++) >> 13;
-
-                    if (sample > 32767)
-                        sample = 32767;
-                    else if (sample < -32768)
-                        sample = -32768;
-
-                    *(pcmbuf_head++) = sample;
-                    *(pcmbuf_head++) = sample;
-                    if (pcmbuf_head >= pcmbuf_end) { pcmbuf_head = pcm_buffer; }
+                while (--i > 0);
+            }
+            else  /* mono */
+            {
+                int32_t *mono = &synth.pcm.samples[0][0];
+                int i = synth.pcm.length;
+
+                do
+                {
+                    int32_t s = clip_sample(*mono++ >> 13);
+                    *audio_data++ = s;
+                    *audio_data++ = s;
                 }
+                while (--i > 0);
             }
+            /**/
 
-            /* TODO: Disable interrupts for Coldfire? */
+            pcmbuf_head->time = pts->pts;
+            pcmbuf_head->size = size;
 
-            /* pcmbuf_len is also modified by the FIQ handler (in
-            get_more), so we disable the FIQ TODO: Add sempahore so we
-            don't change whilst get_more is running. */
+            /* As long as we're on this timestamp, the time is just incremented
+               by the number of samples */
+            pts->pts += synth.pcm.length;
 
-#ifdef CPU_ARM
-            disable_fiq();
-#endif
-            pcmbuf_len += framelength*4;
-#ifdef CPU_ARM
-            enable_fiq();
-#endif
-            if ((!madpcm_playing) && (pcmbuf_len > 64*1024)) {
-                madpcm_playing = 1;
-                rb->pcm_play_data(get_more,NULL,0);
+            pcm_advance_buffer(&pcmbuf_head, size);
+
+            if (pcmbuf_threshold != PCMBUF_PLAY_ALL && pcmbuf_used >= 64*1024)
+            {
+                /* We've reached our size treshold so start playing back the
+                   audio in the buffer and set the buffer to play all data */
                 audiostatus = STREAM_PLAYING;
+                pcmbuf_threshold = PCMBUF_PLAY_ALL;
+                pcm_playback_seek_time(pcmbuf_tail->time);
             }
+
+            /* Make this data available to DMA */
+            locked_add_long(&pcmbuf_used, size);
         }
+
         rb->yield();
-    }
+    } /* end decoding loop */
 
 done:
-    rb->pcm_play_stop();
-    audiostatus=STREAM_DONE;
+    /* Force any residue to play if audio ended before reaching the
+       threshold */
+    if (pcmbuf_threshold != PCMBUF_PLAY_ALL && pcmbuf_used > 0)
+    {
+        pcm_playback_play(pcmbuf_tail->time);
+        pcmbuf_threshold = PCMBUF_PLAY_ALL;
+    }
 
-    for (;;) {  
+    if (rb->pcm_is_playing() && !rb->pcm_is_paused())
+    {
+        /* Wait for audio to finish */
+        while (pcmbuf_used > 0)
+            rb->sleep(HZ/10);
+    }
+
+    audiostatus = STREAM_DONE;
+
+    /* Process events until finished */
+    while (audiostatus != PLEASE_STOP)
+    {
         button_loop();
         rb->sleep(HZ/4);
     }
+
+    /* Don't really terminate but just signal that it's ok for this thread
+       to be killed */
+    audiostatus = THREAD_TERMINATED;
+
+    while (1)
+        rb->yield();
 }
 
 /* End of libmad stuff */
 
-static int64_t eta IBSS_ATTR;
-
 /* TODO: Running in the main thread, libmad needs 8.25KB of stack.
    The codec thread uses a 9KB stack.  So we can probable reduce this a
    little, but leave at 9KB for now to be safe. */
@@ -820,21 +1227,29 @@ static uint32_t video_stack[VIDEO_STACKSIZE / sizeof(uint32_t)] IBSS_ATTR;
 
 static void video_thread(void)
 {
+    /* Time dither to maximally spread frame drops in time to best
+       approximate the appearance of a lower frame rate */
+    static const uint32_t time_dither[8] =
+        { 1, 4, 7, 2, 5, 8, 3, 6 };
     const mpeg2_info_t * info;
     mpeg2_state_t state;
     char str[80];
-    int skipped = 0;
-    int skipcount = 0;
-    int frame = 0;
-    int lasttick;
-    int64_t eta2;
-    int64_t s;
-    int64_t fps;
-
-    rb->sleep(HZ/5);
-    mpeg2dec = mpeg2_init ();
-
-    if (mpeg2dec == NULL) {
+    int dither_index = 0;
+    uint32_t curr_time = 0;
+    uint32_t eta_audio = UINT_MAX, eta_video = 0;
+    int32_t eta_early = 0, eta_late = 0;
+    int frame_drop_level = 0;
+    int drop_frame = 0;
+    int num_skipped = 0;
+    int num_drawn = 0;
+    /* Used to decide when to display FPS */
+    unsigned long last_showfps = *rb->current_tick - HZ;
+    /* Used to decide whether or not to force a frame update */
+    unsigned long last_render = last_showfps;
+
+    mpeg2dec = mpeg2_init();
+    if (mpeg2dec == NULL)
+    {
         videostatus = STREAM_ERROR;
         rb->splash(0, "mpeg2_init failed");
         /* Commit suicide */
@@ -846,111 +1261,291 @@ static void video_thread(void)
     rb->lcd_clear_display();
     rb->lcd_update();
 
-    /* Used to decide when to display FPS */
-    lasttick = *rb->current_tick - HZ;
-
     /* Request the first packet data */
     get_next_data( &video_str );
     mpeg2_buffer (mpeg2dec, video_str.curr_packet, video_str.curr_packet_end);
     total_offset += video_str.curr_packet_end - video_str.curr_packet;
 
     info = mpeg2_info (mpeg2dec);
-    while (1) {
-        if (videostatus == PLEASE_STOP) {
-            goto done;
-        } else if (videostatus == PLEASE_PAUSE) {
+
+    /* Wait if the audio thread is buffering - i.e. before
+       the first frames are decoded */
+    while (audiostatus == STREAM_BUFFERING)
+        rb->priority_yield();
+
+    while (1)
+    {
+        if (videostatus == PLEASE_STOP)
+        {
+            break;
+        }
+        else if (videostatus == PLEASE_PAUSE)
+        {
             videostatus = STREAM_PAUSING;
-            while (videostatus == STREAM_PAUSING) { rb->sleep(HZ/10); }
+            while (videostatus == STREAM_PAUSING)
+                rb->sleep(HZ/10);
         }
+
         state = mpeg2_parse (mpeg2dec);
         rb->yield();
 
-        switch (state) {
+        switch (state)
+        {
         case STATE_BUFFER:
             /* Request next packet data */
             get_next_data( &video_str );
             mpeg2_buffer (mpeg2dec, video_str.curr_packet, video_str.curr_packet_end);
             total_offset += video_str.curr_packet_end - video_str.curr_packet;
             info = mpeg2_info (mpeg2dec);
-            if (video_str.curr_packet == NULL) {
+
+            if (video_str.curr_packet == NULL)
+            {
                 /* No more data. */
                 goto done;
             }
             continue;
 
         case STATE_SEQUENCE:
+            /* New GOP, inform output of any changes */
             vo_setup(info->sequence);
-            mpeg2_skip (mpeg2dec, false);
-
             break;
+
         case STATE_PICTURE:
+        {
+            /* A new picture is available - see if we should draw it */
+            uint32_t period; /* Frame period in clock ticks */
+            int32_t offset;  /* Tick adjustment to keep sync */
+            int type;        /* Frame type: I/P/B/D */
+
+            /* No limiting => no dropping so simply make sure skipping is off
+               in the decoder and the frame will be drawn */
+            if (!settings.limitfps)
+                goto picture_done;
+
+            period = TIME_TO_TICKS(info->sequence->frame_period);
+
+            /* Get presentation times in audio samples - quite accurate
+               enough */
+            curr_time = (info->current_picture->flags & PIC_FLAG_TAGS) ?
+                TS_TO_TICKS(info->current_picture->tag) : (curr_time + period);
+
+            eta_video = curr_time;
+            eta_audio = get_stream_time();
+
+            /* How early/late are we? > 0 = late, < 0  early */
+            offset = eta_audio - eta_video;
+
+            if (!settings.skipframes)
+            {
+                /* Make no effort to determine whether this frame should be
+                   drawn or not since no action can be taken to correct the
+                   situation. We'll just wait if we're early and correct for
+                   lateness as much as possible. */
+                if (offset < 0)
+                    offset = 0;
+
+                eta_late = AVERAGE(eta_late, offset, 4);
+                offset = eta_late;
+
+                if ((uint32_t)offset > eta_video)
+                    offset = eta_video;
+
+                eta_video -= offset;
+                goto picture_done;
+            }
+
+            /** Possibly skip this frame **/
+
+            /* Frameskipping has the following order of preference:
+             *
+             * Frame Type  Who      Notes/Rationale
+             * B           decoder  arbitrarily drop - no decode or draw
+             * Any         renderer arbitrarily drop - will be I/D/P
+             * P           decoder  must wait for I/D-frame - choppy
+             * I/D         decoder  must wait for I/D-frame - choppy
+             *
+             * If a frame can be drawn and it has been at least 1/2 second,
+             * the image will be updated no matter how late it is just to
+             * avoid looking stuck.
+             */
+
+            /* If we're late, set the eta to play the frame early so
+               we may catch up. If early, especially because of a drop,
+               mitigate a "snap" by moving back gradually. */
+            if (offset >= 0) /* late or on time */
+            {
+                eta_early = 0; /* Not early now :( */
+
+                eta_late = AVERAGE(eta_late, offset, 4);
+                offset = eta_late;
+
+                if ((uint32_t)offset > eta_video)
+                    offset = eta_video;
+
+                eta_video -= offset;
+            }
+            else
+            {
+                eta_late = 0; /* Not late now :) */
+
+                if (offset > eta_early)
+                {
+                    /* Just dropped a frame and we're now early or we're
+                       coming back from being early */
+                    eta_early = offset;
+                    if ((uint32_t)-offset > eta_video)
+                        offset = -eta_video;
+
+                    eta_video += offset;
+                }
+                else
+                {
+                    /* Just early with an offset, do exponential drift back */
+                    if (eta_early != 0)
+                    {
+                        eta_early = AVERAGE(eta_early, 0, 8);
+                        eta_video = ((uint32_t)-eta_early > eta_video) ?
+                            0 : (eta_video + eta_early);
+                    }
+
+                    offset = eta_early;
+                }
+            }
+
+            /* If we are more than 167ms behind schedule and it's
+               been less than 1/2 second since the last frame, skip frame. */
+            dither_index = (dither_index + 1) & 7;
+            offset += time_dither[dither_index]*period >> 3;
+
+            type = info->current_picture->flags & PIC_MASK_CODING_TYPE;
+
+            if (frame_drop_level > 1 || offset > CLOCK_RATE*167/1000)
+            {
+                /* Things are running a bit late or all frames are being
+                   dropped until a key frame */
+
+                if (frame_drop_level > 1 && (type == I_TYPE || type == D_TYPE))
+                    frame_drop_level = 0; /* This frame can be drawn */
+
+                if (frame_drop_level <= 1 && offset > CLOCK_RATE*250/1000)
+                {
+                    /* Things are very, very late. Resort to stronger measures
+                       to keep sync by dropping a I/D/P frame. Drawing cannot
+                       take place again until the next key frame. */
+                    if (type == I_TYPE || type == D_TYPE || type == P_TYPE)
+                        frame_drop_level = 2;
+                }
+
+                drop_frame = 1 << frame_drop_level;
+
+                if (*rb->current_tick - last_render >= HZ/2)
+                {
+                    /* Timeout has expired and this frame will be drawn if it
+                       is available. This may reverse the decision to drop a
+                       key frame above. */
+                    if (frame_drop_level <= 1 || type == I_TYPE || type == D_TYPE)
+                    {
+                        drop_frame = 0;
+                    }
+                }
+                else if (type == B_TYPE)
+                {
+                    /* We want to drop something, so this B frame won't even be
+                       decoded. Drawing can happen on the next frame if so
+                       desired. Drop level is not affected. */
+                    drop_frame |= 1 << 1;
+                }
+
+                if (drop_frame != 0)
+                {
+                    num_skipped++;
+                    eta_early = INT32_MIN; /* Indicate a dropped frame */
+                }
+            }
+
+        picture_done:
+            mpeg2_skip(mpeg2dec, drop_frame >> 1);
             break;
+            }
         case STATE_SLICE:
         case STATE_END:
         case STATE_INVALID_END:
             /* draw current picture */
-            if (info->display_fbuf) {
-                /* Wait if the audio thread is buffering - i.e. before
-                   the first frames are decoded */
-                while (audiostatus == STREAM_BUFFERING) {
-                    rb->sleep(1);
+            if (!info->display_fbuf)
+                break;
+
+            if (settings.limitfps)
+            {
+                /* Framerate regulation is turned on - drop it if STATE_PICTURE said
+                   or wait for it if it's too early to draw it. */
+                if (drop_frame != 0)
+                {
+                    drop_frame = 0;
+                    break;
                 }
-                eta += (info->sequence->frame_period);
 
-                /*  Convert eta (in 27MHz ticks) into audio samples */
-                eta2 =(eta * 44100) / 27000000;
+                /* Wait until audio catches up */
+                while (eta_video > eta_audio)
+                {
+                    rb->priority_yield();
 
-                eta2 -= delay;
+                    /* Make sure not to get stuck waiting here forever */
+                    if (videostatus != STREAM_PLAYING)
+                        goto rendering_finished;
 
-                s = samplesplayed - (rb->pcm_get_bytes_waiting() >> 2);
-                if (settings.limitfps) {
-                    if (eta2 > s) {
-                        rb->sleep(4); //((eta2-s)*HZ)/44100);
-                    }              
+                    eta_audio = get_stream_time();
                 }
 
-                /* If we are more than 3/20 second behind schedule (and 
-                   more than 3/20 second into the decoding), skip frame.
-                   But don't skip more than 10 consecutive frames.  */
-                if (settings.skipframes && (s > ((3*44100)/20)) && 
-                   (eta2 < (s - ((3*44100)/20))) && (skipcount < 10)) {
-                    skipped++;
-                    skipcount++;
-                } else {
-                    vo_draw_frame(info->display_fbuf->buf);
-                    skipcount = 0;
-                }
+                /* Record last frame time */
+                last_render = *rb->current_tick;
+            }
 
-                /* Calculate fps */
-                frame++;
-                if (settings.showfps && (*rb->current_tick-lasttick>=HZ)) {
-                    fps=frame;
-                    fps*=441000;
-                    fps/=s;
-                    rb->snprintf(str,sizeof(str),"%d.%d %d %d",
-                                 (int)(fps/10),(int)(fps%10),skipped,delay);
-                    rb->lcd_putsxy(0,0,str);
-                    rb->lcd_update_rect(0,0,LCD_WIDTH,8);
-        
-                    lasttick = *rb->current_tick;
-                }
+            vo_draw_frame(info->display_fbuf->buf);
+            num_drawn++;
+
+            if (!settings.showfps)
+                break;
+
+            /* Calculate and display fps */
+            if (TIME_AFTER(*rb->current_tick, last_showfps + HZ))
+            {
+                uint32_t clock_ticks = get_playback_time();
+                int fps = 0;
+
+                if (clock_ticks != 0)
+                    fps = num_drawn*CLOCK_RATE*10ll / clock_ticks;
+
+                rb->snprintf(str, sizeof(str), "%d.%d %d %d",
+                             fps / 10, fps % 10, num_skipped,
+                             info->display_picture->temporal_reference);
+                rb->lcd_putsxy(0, 0, str);
+                rb->lcd_update_rect(0, 0, LCD_WIDTH, 8);
+
+                last_showfps = *rb->current_tick;
             }
             break;
         default:
             break;
         }
+    rendering_finished:
 
         rb->yield();
     }
 
 done:
     videostatus = STREAM_DONE;
+
+    while (videostatus != PLEASE_STOP)
+        rb->sleep(HZ/5);
+
+    videostatus = THREAD_TERMINATED;
     /* Commit suicide */
     rb->remove_thread(NULL);
 }
 
 enum plugin_status plugin_start(struct plugin_api* api, void* parameter)
 {
+    int status = PLUGIN_ERROR; /* assume failure */
     void* audiobuf;
     int audiosize;
     int in_file;
@@ -968,15 +1563,22 @@ enum plugin_status plugin_start(struct plugin_api* api, void* parameter)
     /* We define this here so it is on the main stack (in IRAM) */
     mad_fixed_t mad_frame_overlap[2][32][18];       /* 4608 bytes */
 
-    /* This also stops audio playback - so we do it before using IRAM */
-    audiobuf = api->plugin_get_audio_buffer(&audiosize);
+    if (parameter == NULL)
+    {
+        api->splash(HZ*2, "No File");
+        return PLUGIN_ERROR;
+    }
 
+    /* Initialize IRAM - stops audio and voice as well */
     PLUGIN_IRAM_INIT(api)
+
     rb = api;
 
+    audiobuf = rb->plugin_get_audio_buffer(&audiosize);
+
     /* Set disk pointers to NULL */
     disk_buf_end = disk_buf = NULL;
-    
+
     /* Stream construction */
     /* We take the first stream of each (audio and video) */
     /* TODO : Search for these in the file first */
@@ -988,9 +1590,10 @@ enum plugin_status plugin_start(struct plugin_api* api, void* parameter)
     /* Initialise our malloc buffer */
     mpeg2_alloc_init(audiobuf,audiosize);
 
-    /* Grab most of the buffer for the compressed video - leave some for 
+    /* Grab most of the buffer for the compressed video - leave some for
        PCM audio data and some for libmpeg2 malloc use. */
-    buffer_size = audiosize - (PCMBUFFER_SIZE+AUDIOBUFFER_SIZE+LIBMPEG2BUFFER_SIZE);
+    buffer_size = audiosize - (PCMBUFFER_SIZE+PCMBUFFER_GUARD_SIZE+
+                               MPABUF_SIZE+LIBMPEG2BUFFER_SIZE);
 
     DEBUGF("audiosize=%d, buffer_size=%ld\n",audiosize,buffer_size);
     buffer = mpeg2_malloc(buffer_size,-1);
@@ -1014,43 +1617,29 @@ enum plugin_status plugin_start(struct plugin_api* api, void* parameter)
     buffer_size &= ~(0x7ff);  /* Round buffer down to nearest 2KB */
     DEBUGF("audiosize=%d, buffer_size=%ld\n",audiosize,buffer_size);
 
-    mpa_buffer_size = AUDIOBUFFER_SIZE;
-    mpa_buffer = mpeg2_malloc(mpa_buffer_size,-2);
-
-    if (mpa_buffer == NULL)
+    if (!init_mpabuf())
         return PLUGIN_ERROR;
 
-    pcm_buffer_size = PCMBUFFER_SIZE;
-    pcm_buffer = mpeg2_malloc(pcm_buffer_size,-2);
-
-    if (pcm_buffer == NULL)
+    if (!init_pcmbuf())
         return PLUGIN_ERROR;
 
     /* The remaining buffer is for use by libmpeg2 */
 
-#ifdef HAVE_LCD_COLOR
-    rb->lcd_set_backdrop(NULL);
-    rb->lcd_set_foreground(LCD_WHITE);
-    rb->lcd_set_background(LCD_BLACK);
-#endif
-    rb->lcd_clear_display();
-    rb->lcd_update();
-
-    if (parameter == NULL) {
-        return PLUGIN_ERROR;
-    }
-
     /* Open the video file */
     in_file = rb->open((char*)parameter,O_RDONLY);
 
-    if (in_file < 0) {
+    if (in_file < 0){
         //fprintf(stderr,"Could not open %s\n",argv[1]);
         return PLUGIN_ERROR;
     }
 
-    init_settings();
-
-    rb->queue_init( &msg_queue, false );    /* Msg queue init */
+#ifdef HAVE_LCD_COLOR
+    rb->lcd_set_backdrop(NULL);
+    rb->lcd_set_foreground(LCD_WHITE);
+    rb->lcd_set_background(LCD_BLACK);
+#endif
+    rb->lcd_clear_display();
+    rb->lcd_update();
 
     /* make sure the backlight is always on when viewing video
        (actually it should also set the timeout when plugged in,
@@ -1065,15 +1654,24 @@ enum plugin_status plugin_start(struct plugin_api* api, void* parameter)
     rb->cpu_boost(true);
 #endif
 
+    /* From this point on we've altered settings, colors, cpu_boost, etc. and
+       cannot just return PLUGIN_ERROR - instead drop though to cleanup code
+     */
+
+    init_settings();
+
+    /* Msg queue init - no need for queue_remove since it's not a registered
+       queue */
+    rb->queue_init( &msg_queue, false );
+
     /* Initialise libmad */
     rb->memset(mad_frame_overlap, 0, sizeof(mad_frame_overlap));
     init_mad(mad_frame_overlap);
 
-    eta = 0;
-
     file_remaining = rb->filesize(in_file);
     disk_buf_end = buffer + buffer_size-MPEG_GUARDBUF_SIZE;
 
+    /* Read some stream data */
     disk_buf_len = rb->read (in_file, buffer, MPEG_LOW_WATERMARK);
 
     DEBUGF("Initial Buffering - %d bytes\n",(int)disk_buf_len);
@@ -1082,15 +1680,11 @@ enum plugin_status plugin_start(struct plugin_api* api, void* parameter)
     file_remaining -= disk_buf_len;
 
     video_str.guard_bytes = audio_str.guard_bytes = 0;
-    video_str.first_pts = audio_str.first_pts = 0;
     video_str.prev_packet = disk_buf;
     audio_str.prev_packet = disk_buf;
     video_str.buffer_remaining = disk_buf_len;
     audio_str.buffer_remaining = disk_buf_len;
 
-    //DEBUGF("START: video = %d, audio = %d\n",audio_str.buffer_remaining,video_str.buffer_remaining);
-    rb->lcd_setfont(FONT_SYSFIXED);
-
     audiostatus = STREAM_BUFFERING;
     videostatus = STREAM_PLAYING;
 
@@ -1104,62 +1698,79 @@ enum plugin_status plugin_start(struct plugin_api* api, void* parameter)
         IF_COP(, COP, true))) == NULL)
     {
         rb->splash(HZ, "Cannot create video thread!");
-        return PLUGIN_ERROR;
+        videostatus = THREAD_TERMINATED;
     }
-    if ((audiothread_id = rb->create_thread(audio_thread,
+    else if ((audiothread_id = rb->create_thread(audio_thread,
         (uint8_t*)audio_stack,AUDIO_STACKSIZE,"mpgaudio" IF_PRIO(,PRIORITY_PLAYBACK)
         IF_COP(, CPU, false))) == NULL)
     {
         rb->splash(HZ, "Cannot create audio thread!");
-        /* To do: Handle this error correctly on dual-core targets */
-        rb->remove_thread(videothread_id);
-        return PLUGIN_ERROR;
+        audiostatus = THREAD_TERMINATED;
     }
+    else
+    {
+        //DEBUGF("START: video = %d, audio = %d\n",audio_str.buffer_remaining,video_str.buffer_remaining);
+        rb->lcd_setfont(FONT_SYSFIXED);
+
+        /* Wait until both threads have finished their work */
+        while ((audiostatus != STREAM_DONE) || (videostatus != STREAM_DONE)) {
+            audio_remaining = audio_str.buffer_remaining;
+            video_remaining = video_str.buffer_remaining;
+            if (MIN(audio_remaining,video_remaining) < MPEG_LOW_WATERMARK) {
+
+                // TODO: Add mutex when updating the A/V buffer_remaining variables.
+                bytes_to_read = buffer_size - MPEG_GUARDBUF_SIZE - MAX(audio_remaining,video_remaining);
+
+                bytes_to_read = MIN(bytes_to_read,(size_t)(disk_buf_end-disk_buf_tail));
+
+                while (( bytes_to_read > 0) && (file_remaining > 0) &&
+                       ((audiostatus != STREAM_DONE) || (videostatus != STREAM_DONE))) {
+                    n = rb->read(in_file, disk_buf_tail, MIN(32*1024,bytes_to_read));
+
+                    bytes_to_read -= n;
+                    file_remaining -= n;
+                    audio_str.buffer_remaining += n;
+                    video_str.buffer_remaining += n;
+                    disk_buf_tail += n;
+                    rb->yield();
+                }
 
-    /* Wait until both threads have finished their work */
-    while ((audiostatus != STREAM_DONE) || (videostatus != STREAM_DONE)) { 
-        audio_remaining = audio_str.buffer_remaining;
-        video_remaining = video_str.buffer_remaining;
-        if (MIN(audio_remaining,video_remaining) < MPEG_LOW_WATERMARK) {
-
-            // TODO: Add mutex when updating the A/V buffer_remaining variables.
-            bytes_to_read = buffer_size - MPEG_GUARDBUF_SIZE - MAX(audio_remaining,video_remaining);
-
-            bytes_to_read = MIN(bytes_to_read,(size_t)(disk_buf_end-disk_buf_tail));
-
-            while (( bytes_to_read > 0) && (file_remaining > 0) && 
-                   ((audiostatus != STREAM_DONE) || (videostatus != STREAM_DONE))) {
-                n = rb->read(in_file, disk_buf_tail, MIN(32*1024,bytes_to_read));
+                if (disk_buf_tail == disk_buf_end)
+                    disk_buf_tail = buffer;
 
-                bytes_to_read -= n;
-                file_remaining -= n;
-                audio_str.buffer_remaining += n;
-                video_str.buffer_remaining += n;
-                disk_buf_tail += n;
-                rb->yield();
             }
-
-            if (disk_buf_tail == disk_buf_end)
-                disk_buf_tail = buffer;
-
+            rb->sleep(HZ/10);
         }
-        rb->sleep(HZ/10);
+
+        rb->lcd_setfont(FONT_UI);
+        status = PLUGIN_OK;
     }
 
 #ifndef HAVE_LCD_COLOR
     gray_release();
 #endif
 
-    rb->remove_thread(audiothread_id);
-    rb->yield(); /* Is this needed? */
+    /* Stop the threads and wait for them to terminate */
+    if (audiothread_id != NULL)
+        audiostatus = PLEASE_STOP;
+
+    if (videothread_id != NULL)
+        videostatus = PLEASE_STOP;
+
+    while (audiostatus != THREAD_TERMINATED ||
+           videostatus != THREAD_TERMINATED)
+        rb->yield();
+
+    if (audiothread_id != NULL)
+        rb->remove_thread(audiothread_id);
+
+    pcm_playback_stop();
 
     rb->lcd_clear_display();
     rb->lcd_update();
 
     mpeg2_close (mpeg2dec);
 
-    rb->queue_delete( &msg_queue );
-
     rb->close (in_file);
 
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
@@ -1168,12 +1779,10 @@ enum plugin_status plugin_start(struct plugin_api* api, void* parameter)
 
     save_settings();  /* Save settings (if they have changed) */
 
-    rb->lcd_setfont(FONT_UI);
-
 #if CONFIG_BACKLIGHT
     /* reset backlight settings */
     rb->backlight_set_timeout(rb->global_settings->backlight_timeout);
 #endif
 
-    return PLUGIN_OK;
+    return status;
 }