4 files changed, 191 insertions, 40 deletions

diff --git a/firmware/id3.c b/firmware/id3.c
index 743ff74902..62150e1ac8 100644
--- a/firmware/id3.c
+++ b/firmware/id3.c
@@ -383,7 +383,7 @@ static int getsonglength(int fd, struct mp3entry *entry)
     unsigned int filetime = 0;
     unsigned long header=0;
     unsigned char tmp;
-    unsigned char frame[64];
+    unsigned char frame[156];
     unsigned char* xing;
     
     int version;
@@ -412,7 +412,7 @@ static int getsonglength(int fd, struct mp3entry *entry)
         
     /* Loop trough file until we find a frame header */
     bytecount = 0;
- restart:
+  restart:
     do {
         header <<= 8;
         if(!read(fd, &tmp, 1))
@@ -441,29 +441,29 @@ static int getsonglength(int fd, struct mp3entry *entry)
 #endif
     /* MPEG Audio Version */
     switch((header & 0x180000) >> 19) {
-    case 2:
-        version = 2;
-        break;
-    case 3:
-        version = 1;
-        break;
-    default:
-        goto restart;
+        case 2:
+            version = 2;
+            break;
+        case 3:
+            version = 1;
+            break;
+        default:
+            goto restart;
     }
 
     /* Layer */
     switch((header & 0x060000) >> 17) {
-    case 1:
-        layer = 3;
-        break;
-    case 2:
-        layer = 2;
-        break;
-    case 3:
-        layer = 1;
-        break;
-    default:
-        goto restart;
+        case 1:
+            layer = 3;
+            break;
+        case 2:
+            layer = 2;
+            break;
+        case 3:
+            layer = 1;
+            break;
+        default:
+            goto restart;
     }
 
     /* Bitrate */
@@ -488,24 +488,27 @@ static int getsonglength(int fd, struct mp3entry *entry)
         
     /* Calculate bytes per frame, calculation depends on layer */
     switch(layer) {
-    case 1:
-        bpf = bitrate_table[version - 1][layer - 1][bitindex];
-        bpf *= 48000;
-        bpf /= freqtab[version-1][freqindex] << (version - 1);
-        break;
-    case 2:
-    case 3:
-        bpf = bitrate_table[version - 1][layer - 1][bitindex];
-        bpf *= 144000;
-        bpf /= freqtab[version-1][freqindex] << (version - 1);
-        break;
-    default:
-        bpf = 1;
+        case 1:
+            bpf = bitrate_table[version - 1][layer - 1][bitindex];
+            bpf *= 48000;
+            bpf /= freqtab[version-1][freqindex] << (version - 1);
+            break;
+        case 2:
+        case 3:
+            bpf = bitrate_table[version - 1][layer - 1][bitindex];
+            bpf *= 144000;
+            bpf /= freqtab[version-1][freqindex] << (version - 1);
+            break;
+        default:
+            bpf = 1;
     }
         
     /* Calculate time per frame */
     tpf = bs[layer] / (freqtab[version-1][freqindex] << (version - 1));
         
+    entry->bpf = bpf;
+    entry->tpf = tpf;
+ 
     /* OK, we have found a frame. Let's see if it has a Xing header */
     if(read(fd, frame, sizeof frame) < 0)
         return -1;
@@ -535,21 +538,26 @@ static int getsonglength(int fd, struct mp3entry *entry)
         /* Yes, it is a VBR file */
         entry->vbr = true;
         
-        if (xing[7] & 0x01) /* Is the frame count there? */
+        if (entry->vbrflags & VBR_FRAMES_FLAG) /* Is the frame count there? */
         {
             int framecount = (xing[8] << 24) | (xing[9] << 16) |
                 (xing[10] << 8) | xing[11];
-
+ 
             filetime = framecount * tpf;
         }
-        if (xing[7] & 0x02) /* is byte count there? */
+
+        if (entry->vbrflags & VBR_BYTES_FLAG) /* is byte count there? */
         {
             int bytecount = (xing[12] << 24) | (xing[13] << 16) |
                 (xing[14] << 8) | xing[15];
-
+ 
             bitrate = bytecount * 8 / filetime;
         }
-        /* We don't care about the TOC just yet. Maybe another time. */
+
+        if (entry->vbrflags & VBR_TOC_FLAG) /* is table-of-contents there? */
+        {
+            memcpy( entry->toc, xing+16, 100 );
+        }
     }
 
     entry->bitrate = bitrate;
diff --git a/firmware/id3.h b/firmware/id3.h
index 203e997073..1a5bc7441e 100644
--- a/firmware/id3.h
+++ b/firmware/id3.h
@@ -29,7 +29,6 @@ struct mp3entry {
     int tracknum;
     int version;
     int layer;
-    bool vbr;
     unsigned int bitrate;
     unsigned int frequency;
     unsigned int id3v2len;
@@ -37,12 +36,23 @@ struct mp3entry {
     unsigned int filesize; /* in bytes */
     unsigned int length;   /* song length */
     unsigned int elapsed;  /* ms played */
+    long bpf;              /* bytes per frame */
+    long tpf;              /* time per frame */
+
+    /* Xing VBR fields */
+    bool vbr;
+    unsigned char vbrflags;
+    unsigned char toc[100];/* table of contents */
 
     /* these following two fields are used for local buffering */
     char id3v2buf[300];
     char id3v1buf[3][32];
 };
 
+#define VBR_FRAMES_FLAG 0x01
+#define VBR_BYTES_FLAG  0x02
+#define VBR_TOC_FLAG    0x04
+
 bool mp3info(struct mp3entry *entry, char *filename);
 
 #endif
diff --git a/firmware/mpeg.c b/firmware/mpeg.c
index ab2fefc872..4c75345f66 100644
--- a/firmware/mpeg.c
+++ b/firmware/mpeg.c
@@ -47,6 +47,7 @@
 #define MPEG_RESUME       4
 #define MPEG_NEXT         5
 #define MPEG_PREV         6
+#define MPEG_FF_REWIND    7
 #define MPEG_NEED_DATA    100
 #define MPEG_SWAP_DATA    101
 #define MPEG_TRACK_CHANGE 102
@@ -216,6 +217,22 @@ static void remove_current_tag(void)
     }
 }
 
+static void remove_all_non_current_tags(void)
+{
+    int i = (tag_read_idx+1) & MAX_ID3_TAGS_MASK;
+
+    while (i != tag_write_idx)
+    {
+        id3tags[i]->used = false;
+        id3tags[i] = NULL;
+
+        i = (i+1) & MAX_ID3_TAGS_MASK;
+    }
+
+    tag_write_idx = (tag_read_idx+1) & MAX_ID3_TAGS_MASK;
+    debug_tags();
+}
+
 static void remove_all_tags(void)
 {
     int i;
@@ -823,6 +840,112 @@ static void mpeg_thread(void)
                 break;
             }
 
+            case MPEG_FF_REWIND: {
+                struct mp3entry *id3 = mpeg_current_track();
+                int newtime  = id3->elapsed + (int)ev.data;
+                int curpos, newpos, diffpos;
+                DEBUGF("MPEG_FF_REWIND\n");
+
+                if (id3->vbr && (id3->vbrflags & VBR_TOC_FLAG))
+                {
+                    /* Use the TOC to find the new position */
+                    int percent = (newtime*100)/id3->length;
+                    int curtoc, nexttoc, nextpos, remainder;
+
+                    if (percent > 99)
+                        percent = 99;
+
+                    curtoc = id3->toc[percent];
+
+                    if (percent < 99)
+                        nexttoc = id3->toc[percent+1];
+                    else
+                        nexttoc = 256;
+
+                    newpos = (curtoc*id3->filesize)/256;
+
+                    /* Use the remainder to get a more accurate position */
+                    nextpos = (nexttoc*id3->filesize)/256;
+                    remainder = (newtime*10000)/id3->length - (percent*100);
+                    newpos += ((nextpos-newpos)*remainder)/100;
+                }
+                else if (id3->bpf && id3->tpf)
+                    newpos = (newtime*id3->bpf)/id3->tpf;
+                else
+                    /* Not enough information to FF/Rewind */
+                    break;
+
+                newpos = newpos & ~1;
+                curpos = lseek(mpeg_file, 0, SEEK_CUR);
+
+                if (num_tracks_in_memory() > 1)
+                {
+                    /* We have started loading other tracks that need to be
+                       accounted for */
+                    int i = tag_read_idx;
+                    int j = tag_write_idx - 1;
+
+                    if (j < 0)
+                        j = MAX_ID3_TAGS - 1;
+
+                    while (i != j)
+                    {
+                        curpos += id3tags[i]->id3.filesize;
+                        i = (i+1) & MAX_ID3_TAGS_MASK;
+                    }
+                }
+
+                diffpos = curpos - newpos;
+
+#warning "Borde inte mp3buflen vara mp3buf_write?"
+                if(diffpos >= 0 && diffpos < mp3buflen)
+                {
+                    /* We are changing to a position that's already in
+                       memory */
+                    mp3buf_read = mp3buf_write - diffpos;
+                    if (mp3buf_read < 0)
+                    {
+                        mp3buf_read += mp3buflen;
+                    }
+
+                    playing = true;
+                    last_dma_tick = current_tick;
+                    init_dma();
+                    start_dma();
+                }
+                else
+                {
+                    /* Move to the new position in the file and start
+                       loading data */
+                    reset_mp3_buffer();
+
+                    if (num_tracks_in_memory() > 1)
+                    {
+                        /* We have to reload the current track */
+                        close(mpeg_file);
+                        remove_all_non_current_tags();
+
+                        mpeg_file = open(id3->path, O_RDONLY);
+                        if (mpeg_file < 0)
+                            break;
+                    }
+
+                    if(-1 == lseek(mpeg_file, newpos, SEEK_SET))
+                        break;
+
+                    filling = true;
+                    queue_post(&mpeg_queue, MPEG_NEED_DATA, 0);
+
+                    /* Tell the file loading code that we want to start playing
+                       as soon as we have some data */
+                    play_pending = true;
+                }
+
+                id3->elapsed = newtime;
+
+                break;
+            }
+
             case MPEG_SWAP_DATA:
                 free_space_left = mp3buf_write - mp3buf_swapwrite;
 
@@ -1128,6 +1251,15 @@ void mpeg_prev(void)
 #endif
 }
 
+void mpeg_ff_rewind(int change)
+{
+#ifndef SIMULATOR
+    queue_post(&mpeg_queue, MPEG_FF_REWIND, (void *)change);
+#else
+    (void)change;
+#endif
+}
+
 bool mpeg_is_playing(void)
 {
     return playing || play_pending;
diff --git a/firmware/mpeg.h b/firmware/mpeg.h
index 4ea0001e25..2838c47d45 100644
--- a/firmware/mpeg.h
+++ b/firmware/mpeg.h
@@ -28,6 +28,7 @@ void mpeg_pause(void);
 void mpeg_resume(void);
 void mpeg_next(void);
 void mpeg_prev(void);
+void mpeg_ff_rewind(int change);
 bool mpeg_is_playing(void);
 void mpeg_sound_set(int setting, int value);
 int mpeg_sound_min(int setting);