Gigabeat: Separate driver for audio codec. Tweak pcm driver to comply with intended interface.

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

8 files changed, 585 insertions, 153 deletions

diff --git a/firmware/SOURCES b/firmware/SOURCES
index d281e54fa5..aeb98a3562 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -195,7 +195,9 @@ drivers/mas.c
 #ifndef SIMULATOR
 #if defined(HAVE_UDA1380)
 drivers/audio/uda1380.c
-#elif defined(HAVE_WM8975) || defined(HAVE_WM8751)
+#elif defined(HAVE_WM8751)
+drivers/audio/wm8751.c
+#elif defined(HAVE_WM8975)
 drivers/audio/wm8975.c
 #elif defined(HAVE_WM8758)
 drivers/audio/wm8758.c
diff --git a/firmware/drivers/audio/wm8751.c b/firmware/drivers/audio/wm8751.c
new file mode 100644
index 0000000000..21ff4728b3
--- /dev/null
+++ b/firmware/drivers/audio/wm8751.c
@@ -0,0 +1,195 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Driver for WM8751 audio codec
+ *
+ * Based on code from the ipodlinux project - http://ipodlinux.org/
+ * Adapted for Rockbox in December 2005
+ *
+ * Original file: linux/arch/armnommu/mach-ipod/audio.c
+ *
+ * Copyright (c) 2003-2005 Bernard Leach (leachbj@bouncycastle.org)
+ *
+ * All files in this archive are subject to the GNU General Public License.
+ * See the file COPYING in the source tree root for full license agreement.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#include "kernel.h"
+#include "wmcodec.h"
+#include "i2s.h"
+#include "audio.h"
+#include "sound.h"
+
+/* Flags used in combination with settings */
+
+/* use zero crossing to reduce clicks during volume changes */
+#define LOUT1_BITS      (LOUT1_LO1ZC)
+/* latch left volume first then update left+right together */
+#define ROUT1_BITS      (ROUT1_RO1ZC | ROUT1_RO1VU)
+#define LOUT2_BITS      (LOUT2_LO2ZC)
+#define ROUT2_BITS      (ROUT2_RO2ZC | ROUT2_RO2VU)
+/* We use linear bass control with 200 Hz cutoff */
+#define BASSCTRL_BITS   (BASSCTRL_BC)
+/* We use linear treble control with 4 kHz cutoff */
+#define TREBCTRL_BITS   (TREBCTRL_TC)
+
+/* convert tenth of dB volume (-730..60) to master volume register value */
+int tenthdb2master(int db)
+{
+    /* +6 to -73dB 1dB steps (plus mute == 80levels) 7bits */
+    /* 1111111 ==  +6dB  (0x7f)                            */
+    /* 1111001 ==   0dB  (0x79)                            */
+    /* 0110000 == -73dB  (0x30)                            */
+    /* 0101111..0000000 == mute  (<= 0x2f)                 */
+    if (db < VOLUME_MIN)
+        return 0x0;
+    else
+        return (db / 10) + 73 + 0x30;
+}
+
+/* convert tenth of dB volume (-780..0) to mixer volume register value */
+int tenthdb2mixer(int db)
+{
+    (void)db;
+    return 0;
+}
+
+static int tone_tenthdb2hw(int value)
+{
+    /* -6.0db..+0db..+9.0db step 1.5db - translate -60..+0..+90 step 15
+        to 10..6..0 step -1.
+    */
+    value = 10 - (value + 60) / 15;
+
+    if (value == 6)
+        value = 0xf; /* 0db -> off */
+
+    return value;
+}
+
+void audiohw_reset(void);
+
+/* Silently enable / disable audio output */
+void audiohw_enable_output(bool enable)
+{
+    if (enable) 
+    {
+        /* reset the I2S controller into known state */
+        i2s_reset();
+
+        /*
+         * 1. Switch on power supplies.
+         *    By default the WM87551L is in Standby Mode, the DAC is
+         *    digitally muted and the Audio Interface, Line outputs
+         *    and Headphone outputs are all OFF (DACMU = 1 Power
+         *    Management registers 1 and 2 are all zeros).
+         */
+        wmcodec_write(RESET, RESET_RESET);    /*Reset*/
+
+         /* 2. Enable Vmid and VREF. */
+        wmcodec_write(PWRMGMT1, PWRMGMT1_VREF | PWRMGMT1_VMIDSEL_500K);
+
+        /* From app notes: allow Vref to stabilize to reduce clicks */
+        sleep(HZ/2);
+
+         /* 3. Enable DACs as required. */
+        wmcodec_write(PWRMGMT2, PWRMGMT2_DACL | PWRMGMT2_DACR);
+    
+         /* 4. Enable line and / or headphone output buffers as required. */
+        wmcodec_write(PWRMGMT2, PWRMGMT2_DACL | PWRMGMT2_DACR |
+                      PWRMGMT2_LOUT1 | PWRMGMT2_ROUT1 | PWRMGMT2_LOUT2 |
+                      PWRMGMT2_ROUT2);
+    
+        /* BCLKINV=0(Dont invert BCLK) MS=1(Enable Master) LRSWAP=0 LRP=0 */
+        /* IWL=00(16 bit) FORMAT=10(I2S format) */
+        wmcodec_write(AINTFCE, AINTFCE_MS | AINTFCE_WL_16 |
+                      AINTFCE_FORMAT_I2S);
+
+        /* Keep it quiet */
+        wmcodec_write(LOUT1, LOUT1_BITS | OUTPUT_MUTED);
+        wmcodec_write(ROUT1, ROUT1_BITS | OUTPUT_MUTED);
+        wmcodec_write(LOUT2, LOUT2_BITS | OUTPUT_MUTED);
+        wmcodec_write(ROUT2, ROUT2_BITS | OUTPUT_MUTED);
+
+        wmcodec_write(LEFTMIX1, LEFTMIX1_LD2LO | LEFTMIX1_LI2LO_DEFAULT);
+        wmcodec_write(RIGHTMIX2, RIGHTMIX2_RD2RO | RIGHTMIX2_RI2RO_DEFAULT);
+    }
+
+    audiohw_mute(!enable);
+}
+
+int audiohw_set_master_vol(int vol_l, int vol_r)
+{
+    /* +6 to -73dB 1dB steps (plus mute == 80levels) 7bits */
+    /* 1111111 ==  +6dB                                    */
+    /* 1111001 ==   0dB                                    */
+    /* 0110000 == -73dB                                    */
+    /* 0101111 == mute (0x2f)                              */
+
+    wmcodec_write(LOUT1, LOUT1_BITS | LOUT1_LOUT1VOL(vol_l));
+    wmcodec_write(ROUT1, ROUT1_BITS | ROUT1_ROUT1VOL(vol_r));
+    return 0;
+}
+
+int audiohw_set_lineout_vol(int vol_l, int vol_r)
+{
+    wmcodec_write(LOUT2, LOUT2_BITS | LOUT2_LOUT2VOL(vol_l));
+    wmcodec_write(ROUT2, ROUT2_BITS | ROUT2_ROUT2VOL(vol_r));
+    return 0;
+}
+
+int audiohw_set_mixer_vol(int channel1, int channel2)
+{
+    (void)channel1;
+    (void)channel2;
+
+    return 0;
+}
+
+void audiohw_set_bass(int value)
+{
+    wmcodec_write(BASSCTRL, BASSCTRL_BITS |
+        BASSCTRL_BASS(tone_tenthdb2hw(value)));
+}
+
+void audiohw_set_treble(int value)
+{
+    wmcodec_write(TREBCTRL, TREBCTRL_BITS |
+        TREBCTRL_TREB(tone_tenthdb2hw(value)));
+}
+
+int audiohw_mute(int mute)
+{
+    /* Mute:   Set DACMU = 1 to soft-mute the audio DACs. */
+    /* Unmute: Set DACMU = 0 to soft-un-mute the audio DACs. */
+    wmcodec_write(DACCTRL, mute ? DACCTRL_DACMU : 0);
+    return 0;
+}
+
+/* Nice shutdown of WM8751 codec */
+void audiohw_close(void)
+{
+    /* 1. Set DACMU = 1 to soft-mute the audio DACs. */
+    audiohw_mute(true);
+
+    /* 2. Disable all output buffers. */
+    wmcodec_write(PWRMGMT2, 0x0);
+
+    /* 3. Switch off the power supplies. */
+    wmcodec_write(PWRMGMT1, 0x0);
+}
+
+/* Note: Disable output before calling this function */
+void audiohw_set_frequency(int fsel)
+{
+    wmcodec_write(CLOCKING, fsel);
+}
diff --git a/firmware/export/config-gigabeat.h b/firmware/export/config-gigabeat.h
index 6a6e1d2977..cd2fac360c 100644
--- a/firmware/export/config-gigabeat.h
+++ b/firmware/export/config-gigabeat.h
@@ -133,9 +133,7 @@
 #define BOOTFILE "rockbox." BOOTFILE_EXT
 #define BOOTDIR "/.rockbox"
 
-#define HW_SAMPR_CAPS (SAMPR_CAP_96 | SAMPR_CAP_88 | SAMPR_CAP_48 | \
-                       SAMPR_CAP_44 | SAMPR_CAP_32 | SAMPR_CAP_24 | \
-                       SAMPR_CAP_22 | SAMPR_CAP_16 | SAMPR_CAP_12 | \
+#define HW_SAMPR_CAPS (SAMPR_CAP_88 | SAMPR_CAP_44 | SAMPR_CAP_22 | \
                        SAMPR_CAP_11 | SAMPR_CAP_8)
 
 #endif
diff --git a/firmware/export/sound.h b/firmware/export/sound.h
index 7589338da9..b060b97312 100644
--- a/firmware/export/sound.h
+++ b/firmware/export/sound.h
@@ -22,7 +22,9 @@
 #include <inttypes.h>
 #ifdef HAVE_UDA1380
 #include "uda1380.h"
-#elif defined(HAVE_WM8975) || defined(HAVE_WM8751)
+#elif defined(HAVE_WM8751)
+#include "wm8751.h"
+#elif defined(HAVE_WM8975)
 #include "wm8975.h"
 #elif defined(HAVE_WM8758)
 #include "wm8758.h"
diff --git a/firmware/export/wm8751.h b/firmware/export/wm8751.h
new file mode 100644
index 0000000000..244d378b26
--- /dev/null
+++ b/firmware/export/wm8751.h
@@ -0,0 +1,209 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2005 by Dave Chapman
+ *
+ * All files in this archive are subject to the GNU General Public License.
+ * See the file COPYING in the source tree root for full license agreement.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#ifndef _WM8751_H
+#define _WM8751_H
+
+/* volume/balance/treble/bass interdependency */
+#define VOLUME_MIN -730
+#define VOLUME_MAX  60
+
+extern int tenthdb2master(int db);
+extern int tenthdb2mixer(int db);
+
+extern void audiohw_reset(void);
+extern int  audiohw_init(void);
+extern void audiohw_enable_output(bool enable);
+extern int  audiohw_set_master_vol(int vol_l, int vol_r);
+extern int  audiohw_set_lineout_vol(int vol_l, int vol_r);
+extern int  audiohw_set_mixer_vol(int channel1, int channel2);
+extern void audiohw_set_bass(int value);
+extern void audiohw_set_treble(int value);
+extern int  audiohw_mute(int mute);
+extern void audiohw_close(void);
+extern void audiohw_set_frequency(int fsel);
+
+/* Register addresses and bits */
+#define OUTPUT_MUTED                0x2f
+#define OUTPUT_0DB                  0x79
+
+#define LOUT1                       0x02
+#define LOUT1_LOUT1VOL_MASK         (0x07f << 0)
+#define LOUT1_LOUT1VOL(x)           ((x) & 0x7f)
+#define LOUT1_LO1ZC                 (1 << 7)
+#define LOUT1_LO1VU                 (1 << 8)
+
+#define ROUT1                       0x03
+#define ROUT1_ROUT1VOL(x)           ((x) & 0x7f)
+#define ROUT1_RO1ZC                 (1 << 7)
+#define ROUT1_RO1VU                 (1 << 8)
+
+#define DACCTRL                     0x05
+#define DACCTRL_DEEMPH_NONE         (0 << 1)
+#define DACCTRL_DEEMPH_32           (1 << 1)
+#define DACCTRL_DEEMPH_44           (2 << 1)
+#define DACCTRL_DEEMPH_48           (3 << 1)
+#define DACCTRL_DEEMPH(x)           ((x) & (0x3 << 1))
+#define DACCTRL_DACMU               (1 << 3)
+#define DACCTRL_DAT                 (1 << 7)
+
+#define AINTFCE                     0x07
+#define AINTFCE_FORMAT_RJUST        (0 << 0)
+#define AINTFCE_FORMAT_LJUST        (1 << 0)
+#define AINTFCE_FORMAT_I2S          (2 << 0)
+#define AINTFCE_FORMAT_DSP          (3 << 0)
+#define AINTFCE_FORMAT(x)           ((x) & 0x3)
+#define AINTFCE_WL_16               (0 << 2)
+#define AINTFCE_WL_20               (1 << 2)
+#define AINTFCE_WL_24               (2 << 2)
+#define AINTFCE_WL_32               (3 << 2)
+#define AINTFCE_WL(x)               ((x) & (0x3 << 2))
+#define AINTFCE_LRP                 (1 << 4)
+#define AINTFCE_LRSWAP              (1 << 5)
+#define AINTFCE_MS                  (1 << 6)
+#define AINTFCE_BCLKINV             (1 << 7)
+
+#define CLOCKING                    0x08
+#define CLOCKING_SR_USB             (1 << 0)
+/* Register settings for the supported samplerates: */
+#define WM8975_8000HZ               0x4d
+#define WM8975_12000HZ              0x61
+#define WM8975_16000HZ              0x55
+#define WM8975_22050HZ              0x77
+#define WM8975_24000HZ              0x79
+#define WM8975_32000HZ              0x59
+#define WM8975_44100HZ              0x63
+#define WM8975_48000HZ              0x41
+#define WM8975_88200HZ              0x7f
+#define WM8975_96000HZ              0x5d
+#define CLOCKING_SR(x)              ((x) & (0x1f << 1))
+#define CLOCKING_MCLK_DIV2          (1 << 6)
+#define CLOCKING_BCLK_DIV2          (1 << 7)
+
+#define LEFTGAIN                    0x0a
+#define LEFTGAIN_LDACVOL(x)         ((x) & 0xff)
+#define LEFTGAIN_LDVU               (1 << 8)
+
+#define RIGHTGAIN                   0x0b
+#define RIGHTGAIN_LDACVOL(x)        ((x) & 0xff)
+#define RIGHTGAIN_LDVU              (1 << 8)
+
+#define BASSCTRL                    0x0c
+#define BASSCTRL_BASS(x)            ((x) & 0xf)
+#define BASSCTRL_BC                 (1 << 7)
+#define BASSCTRL_BB                 (1 << 8)
+
+#define TREBCTRL                    0x0d
+#define TREBCTRL_TREB(x)            ((x) & 0xf)
+#define TREBCTRL_TC                 (1 << 7)
+
+#define RESET                       0x0f
+#define RESET_RESET                 0x000
+
+#define ADDITIONAL1                 0x17
+#define ADDITIONAL1_TOEN            (1 << 0)
+#define ADDITIONAL1_DACINV          (1 << 1)
+#define ADDITIONAL1_DMONOMIX_LLRR   (0 << 4)
+#define ADDITIONAL1_DMONOMIX_ML0R   (1 << 4)
+#define ADDITIONAL1_DMONOMIX_0LMR   (2 << 4)
+#define ADDITIONAL1_DMONOMIX_MLMR   (3 << 4)
+#define ADDITIONAL1_DMONOMIX(x)     ((x) & (0x03 << 4))
+#define ADDITIONAL1_VSEL_LOWEST     (0 << 6)
+#define ADDITIONAL1_VSEL_LOW        (1 << 6)
+#define ADDITIONAL1_VSEL_DEFAULT2   (2 << 6)
+#define ADDITIONAL1_VSEL_DEFAULT    (3 << 6)
+#define ADDITIONAL1_VSEL(x)         ((x) & (0x3 << 6))
+#define ADDITIONAL1_TSDEN           (1 << 7)
+
+#define ADDITIONAL2                 0x18
+#define ADDITIONAL2_ROUT2INV        (1 << 4)
+#define ADDITIONAL2_DACOSR          (1 << 0)
+
+#define PWRMGMT1                    0x19
+#define PWRMGMT1_DIGENB             (1 << 0)
+#define PWRMGMT1_VREF               (1 << 6)
+#define PWRMGMT1_VMIDSEL_DISABLED   (0 << 7)
+#define PWRMGMT1_VMIDSEL_50K        (1 << 7)
+#define PWRMGMT1_VMIDSEL_500K       (2 << 7)
+#define PWRMGMT1_VMIDSEL_5K         (3 << 7)
+#define PWRMGMT1_VMIDSEL(x)         ((x) & (0x3 << 7))
+
+#define PWRMGMT2                    0x1a
+#define PWRMGMT2_DACL               (1 << 8)
+#define PWRMGMT2_DACR               (1 << 7)
+#define PWRMGMT2_LOUT1              (1 << 6)
+#define PWRMGMT2_ROUT1              (1 << 5)
+#define PWRMGMT2_LOUT2              (1 << 4)
+#define PWRMGMT2_ROUT2              (1 << 3)
+#define PWRMGMT2_MOUT               (1 << 2)
+#define PWRMGMT2_OUT3               (1 << 1)
+
+#define ADDITIONAL3                 0x1b
+#define ADDITIONAL3_ADCLRM          ((x) & (0x3 << 7))
+#define ADDITIONAL3_VROI            (1 << 6)
+#define ADDITIONAL3_HPFLREN         (1 << 5)
+
+#define LEFTMIX1                    0x22
+#define LEFTMIX1_LD2LO              (1 << 8)
+#define LEFTMIX1_LI2LO              (1 << 7)
+#define LEFTMIX1_LI2LO_DEFAULT      (5 << 4)
+#define LEFTMIX1_LI2LOVOL(x)        ((x) & (0x7 << 4))
+
+#define LEFTMIX2                    0x23
+#define LEFTMIX2_RD2LO              (1 << 8)
+#define LEFTMIX2_MI2LO              (1 << 7)
+#define LEFTMIX2_MI2LO_DEFAULT      (5 << 4)
+#define LEFTMIX2_MI2LOVOL(x)        ((x) & (0x7 << 4))
+
+#define RIGHTMIX1                   0x24
+#define RIGHTMIX1_LD2RO             (1 << 8)
+#define RIGHTMIX1_MI2RO             (1 << 7)
+#define RIGHTMIX1_MI2RO_DEFAULT     (5 << 4)
+#define RIGHTMIX1_MI2ROVOL(x)       ((x) & (0x7 << 4))
+
+#define RIGHTMIX2                   0x25
+#define RIGHTMIX2_RD2RO             (1 << 8)
+#define RIGHTMIX2_RI2RO             (1 << 7)
+#define RIGHTMIX2_RI2RO_DEFAULT     (5 << 4)
+#define RIGHTMIX2_RI2ROVOL(x)       ((x) & (0x7 << 4))
+
+#define MONOMIX1                    0x26
+#define MONOMIX1_LI2MOVOL(x)        ((x) & (0x7 << 4))
+#define MONOMIX1_LI2MO              (1 << 7)
+#define MONOMIX1_LD2MO              (1 << 8)
+#define MONOMIX1_DMEN               (1 << 0)
+
+#define MONOMIX2                    0x27
+#define MONOMIX2_RD2MO              (1 << 8)
+#define MONOMIX2_RI2MO              (1 << 7)
+#define MONOMIX2_RI2MOVOL(x)        ((x) & (0x7 << 4))
+
+#define LOUT2                       0x28
+#define LOUT2_LOUT2VOL(x)           ((x) & 0x7f)
+#define LOUT2_LO2ZC                 (1 << 7)
+#define LOUT2_LO2VU                 (1 << 8)
+
+#define ROUT2                       0x29
+#define ROUT2_ROUT2VOL(x)           ((x) & 0x7f)
+#define ROUT2_RO2ZC                 (1 << 7)
+#define ROUT2_RO2VU                 (1 << 8)
+
+#define MONOOUT                     0x2a
+#define MONOOUT_MOZC                (1 << 7)
+
+#endif /* _WM8751_H */
diff --git a/firmware/sound.c b/firmware/sound.c
index 9835711b54..2589306290 100644
--- a/firmware/sound.c
+++ b/firmware/sound.c
@@ -64,7 +64,11 @@ static const struct sound_settings_info sound_settings_table[] = {
     [SOUND_TREBLE]        = {"dB", 0,  2,   0,   6,   0, sound_set_treble},
 #elif defined(HAVE_TLV320)
     [SOUND_VOLUME]        = {"dB", 0,  1, -73,   6, -20, sound_set_volume},
-#elif defined(HAVE_WM8975) || defined(HAVE_WM8751)
+#elif defined(HAVE_WM8751)
+    [SOUND_VOLUME]        = {"dB", 0,  1, -74,   6, -25, sound_set_volume},
+    [SOUND_BASS]          = {"dB", 1, 15, -60,  90,   0, sound_set_bass},
+    [SOUND_TREBLE]        = {"dB", 1, 15, -60,  90,   0, sound_set_treble},
+#elif defined(HAVE_WM8975)
     [SOUND_VOLUME]        = {"dB", 0,  1, -74,   6, -25, sound_set_volume},
     [SOUND_BASS]          = {"dB", 0,  1,  -6,   9,   0, sound_set_bass},
     [SOUND_TREBLE]        = {"dB", 0,  1,  -6,   9,   0, sound_set_treble},
@@ -514,8 +518,12 @@ void sound_set_bass(int value)
     mas_writereg(MAS_REG_KBASS, bass_table[value+15]);
     current_bass = value * 10;
     set_prescaled_volume();
+#elif defined(HAVE_WM8751)
+    current_bass = value;
+    audiohw_set_bass(value);
+    set_prescaled_volume();
 #elif defined HAVE_WM8975 || defined HAVE_WM8758 || defined(HAVE_UDA1380) \
-    || defined HAVE_WM8731 || defined(HAVE_WM8721) || defined(HAVE_WM8751)
+    || defined HAVE_WM8731 || defined(HAVE_WM8721)
     current_bass = value * 10;
     audiohw_set_bass(value);
     set_prescaled_volume();
@@ -540,8 +548,12 @@ void sound_set_treble(int value)
     mas_writereg(MAS_REG_KTREBLE, treble_table[value+15]);
     current_treble = value * 10;
     set_prescaled_volume();
+#elif defined(HAVE_WM8751)
+    audiohw_set_treble(value);
+    current_treble = value;
+    set_prescaled_volume();
 #elif defined(HAVE_WM8975) || defined(HAVE_WM8758) || defined(HAVE_UDA1380) \
-   || defined(HAVE_WM8731) || defined(HAVE_WM8721) || defined(HAVE_WM8751)
+   || defined(HAVE_WM8731) || defined(HAVE_WM8721)
     audiohw_set_treble(value);
     current_treble = value * 10;
     set_prescaled_volume();
diff --git a/firmware/target/arm/s3c2440/gigabeat-fx/pcm-meg-fx.c b/firmware/target/arm/s3c2440/gigabeat-fx/pcm-meg-fx.c
index 0f22aa5c5c..2b4842f880 100644
--- a/firmware/target/arm/s3c2440/gigabeat-fx/pcm-meg-fx.c
+++ b/firmware/target/arm/s3c2440/gigabeat-fx/pcm-meg-fx.c
@@ -16,6 +16,7 @@
  * KIND, either express or implied.
  *
  ****************************************************************************/
+#include <stdlib.h>
 #include "system.h"
 #include "kernel.h"
 #include "logf.h"
@@ -24,20 +25,14 @@
 #include "file.h"
 #include "mmu-meg-fx.h"
 
-static int pcm_freq = HW_SAMPR_DEFAULT; /* 44.1 is default */
-
-#define GIGABEAT_8000HZ      0x4d
-#define GIGABEAT_11025HZ     0x32
-#define GIGABEAT_12000HZ     0x61
-#define GIGABEAT_16000HZ     0x55
-#define GIGABEAT_22050HZ     0x36
-#define GIGABEAT_24000HZ     0x79
-#define GIGABEAT_32000HZ     0x59
-#define GIGABEAT_44100HZ     0x22
-#define GIGABEAT_48000HZ     0x41
-#define GIGABEAT_88200HZ     0x3e
-#define GIGABEAT_96000HZ     0x5d
+#define GIGABEAT_8000HZ      (0x02 << 1)
+#define GIGABEAT_11025HZ     (0x19 << 1)
+#define GIGABEAT_22050HZ     (0x1b << 1)
+#define GIGABEAT_44100HZ     (0x11 << 1)
+#define GIGABEAT_88200HZ     (0x1f << 1)
 
+static int pcm_freq = HW_SAMPR_DEFAULT; /* 44.1 is default */
+static int sr_ctrl = GIGABEAT_44100HZ;
 #define FIFO_COUNT ((IISFCON >> 6) & 0x01F)
 
 /* number of bytes in FIFO */
@@ -49,50 +44,28 @@ static int pcm_freq = HW_SAMPR_DEFAULT; /* 44.1 is default */
 unsigned short * p;
 size_t p_size;
 
-
-
 /* DMA count has hit zero - no more data */
 /* Get more data from the callback and top off the FIFO */
 //void fiq(void) __attribute__ ((interrupt ("naked")));
 void fiq(void) ICODE_ATTR __attribute__ ((interrupt ("FIQ")));
-void fiq(void)
-{
-    /* clear any pending interrupt */
-    SRCPND = (1<<19);
 
-    /* Buffer empty.  Try to get more. */
-    if (pcm_callback_for_more)
-    {
-        pcm_callback_for_more((unsigned char**)&p, &p_size);
-    }
-    else
-    {
-        /* callback func is missing? */
-        pcm_play_dma_stop();
-        return;
-    }
+static void _pcm_apply_settings(void)
+{
+    static int last_freqency = 0;
 
-    if (p_size)
+    if (pcm_freq != last_freqency)
     {
-        /* Flush any pending cache writes */
-        clean_dcache_range(p, p_size);
-
-        /* set the new DMA values */
-        DCON2 = DMA_CONTROL_SETUP | (p_size >> 1);
-        DISRC2 = (int)p + 0x30000000;
-
-        /* Re-Activate the channel */
-        DMASKTRIG2 = 0x2;
+        last_freqency = pcm_freq;
+        audiohw_set_frequency(sr_ctrl);
     }
-    else
-    {
-        /* No more DMA to do */
-        pcm_play_dma_stop();
-    }
-
 }
 
-
+void pcm_apply_settings(void)
+{
+    int oldstatus = set_fiq_status(FIQ_DISABLED);
+    _pcm_apply_settings();
+    set_fiq_status(oldstatus);
+}
 
 void pcm_init(void)
 {
@@ -103,8 +76,6 @@ void pcm_init(void)
     audiohw_init();
     audiohw_enable_output(true);
 
-    /* cannot use the WM8975 defaults since our clock is not the same */
-    /* the input master clock is 16.9344MHz - we can divide exact for that */
     pcm_set_frequency(SAMPR_44);
 
     /* init GPIO */
@@ -130,6 +101,8 @@ void pcm_play_dma_start(const void *addr, size_t size)
     /* sanity check: bad pointer or too small file */
     if (NULL == addr || size <= IIS_FIFO_SIZE) return;
 
+    disable_fiq();
+
     p = (unsigned short *)addr;
     p_size = size;
 
@@ -163,8 +136,11 @@ void pcm_play_dma_start(const void *addr, size_t size)
     /* clear pending DMA interrupt */
     SRCPND = 1<<19;
 
+    pcm_playing = true;
+
+    _pcm_apply_settings();
+
     set_fiq_handler(fiq);
-    enable_fiq();
 
     /* unmask the DMA interrupt */
     INTMSK &= ~(1<<19);
@@ -178,17 +154,13 @@ void pcm_play_dma_start(const void *addr, size_t size)
     /* turn off the idle */
     IISCON &= ~(1<<3);
 
-    pcm_playing = true;
-
     /* start the IIS */
     IISCON |= (1<<0);
 
+    enable_fiq();
 }
 
-
-
-/* Disconnect the DMA and wait for the FIFO to clear */
-void pcm_play_dma_stop(void)
+static void pcm_play_dma_stop_fiq(void)
 {
     /* mask the DMA interrupt */
     INTMSK |= (1<<19);
@@ -207,17 +179,59 @@ void pcm_play_dma_stop(void)
 
     /* Disconnect the IIS clock */
     CLKCON &= ~(1<<17);
+}
 
-    disable_fiq();
+void fiq(void)
+{
+    /* clear any pending interrupt */
+    SRCPND = (1<<19);
+
+    /* Buffer empty.  Try to get more. */
+    if (pcm_callback_for_more)
+    {
+        pcm_callback_for_more((unsigned char**)&p, &p_size);
+    }
+    else
+    {
+        /* callback func is missing? */
+        pcm_play_dma_stop_fiq();
+        return;
+    }
+
+    if (p_size)
+    {
+        /* Flush any pending cache writes */
+        clean_dcache_range(p, p_size);
+
+        /* set the new DMA values */
+        DCON2 = DMA_CONTROL_SETUP | (p_size >> 1);
+        DISRC2 = (int)p + 0x30000000;
+
+        /* Re-Activate the channel */
+        DMASKTRIG2 = 0x2;
+    }
+    else
+    {
+        /* No more DMA to do */
+        pcm_play_dma_stop_fiq();
+    }
 
 }
 
+/* Disconnect the DMA and wait for the FIFO to clear */
+void pcm_play_dma_stop(void)
+{
+    disable_fiq();
+    pcm_play_dma_stop_fiq();
+}
 
 
 void pcm_play_pause_pause(void)
 {
     /* stop servicing refills */
+    int oldstatus = set_fiq_status(FIQ_DISABLED);
     INTMSK |= (1<<19);
+    set_fiq_status(oldstatus);
 }
 
 
@@ -225,13 +239,14 @@ void pcm_play_pause_pause(void)
 void pcm_play_pause_unpause(void)
 {
     /* refill buffer and keep going */
+    int oldstatus = set_fiq_status(FIQ_DISABLED);
+    _pcm_apply_settings();
     INTMSK &= ~(1<<19);
+    set_fiq_status(oldstatus);
 }
 
 void pcm_set_frequency(unsigned int frequency)
 {
-    int sr_ctrl;
-
     switch(frequency)
     {
         case SAMPR_8:
@@ -240,38 +255,19 @@ void pcm_set_frequency(unsigned int frequency)
         case SAMPR_11:
             sr_ctrl = GIGABEAT_11025HZ;
             break;
-        case SAMPR_12:
-            sr_ctrl = GIGABEAT_12000HZ;
-            break;
-        case SAMPR_16:
-            sr_ctrl = GIGABEAT_16000HZ;
-            break;
         case SAMPR_22:
             sr_ctrl = GIGABEAT_22050HZ;
             break;
-        case SAMPR_24:
-            sr_ctrl = GIGABEAT_24000HZ;
-            break;
-        case SAMPR_32:
-            sr_ctrl = GIGABEAT_32000HZ;
-            break;
         default:
             frequency = SAMPR_44;
         case SAMPR_44:
             sr_ctrl = GIGABEAT_44100HZ;
             break;
-        case SAMPR_48:
-            sr_ctrl = GIGABEAT_48000HZ;
-            break;
         case SAMPR_88:
             sr_ctrl = GIGABEAT_88200HZ;
             break;
-        case SAMPR_96:
-            sr_ctrl = GIGABEAT_96000HZ;
-            break;
     }
 
-    audiohw_set_sample_rate(sr_ctrl);
     pcm_freq = frequency;
 }
 
@@ -282,17 +278,12 @@ size_t pcm_get_bytes_waiting(void)
     return (DSTAT2 & 0xFFFFF) * 2;
 }
 
-
-
-/* dummy functions for those not actually supporting all this yet */
-void pcm_apply_settings(void)
-{
-}
-
+#if 0
 void pcm_set_monitor(int monitor)
 {
     (void)monitor;
 }
+#endif
 /** **/
 
 void pcm_mute(bool mute)
@@ -302,75 +293,79 @@ void pcm_mute(bool mute)
         sleep(HZ/16);
 }
 
-/*
- * This function goes directly into the DMA buffer to calculate the left and
- * right peak values. To avoid missing peaks it tries to look forward two full
- * peek periods (2/HZ sec, 100% overlap), although it's always possible that
- * the entire period will not be visible. To reduce CPU load it only looks at
- * every third sample, and this can be reduced even further if needed (even
- * every tenth sample would still be pretty accurate).
+/**
+ * Return playback peaks - Peaks ahead in the DMA buffer based upon the
+ *                         calling period to attempt to compensate for
+ *                         delay.
  */
-
-/* Check for a peak every PEAK_STRIDE samples */
-#define PEAK_STRIDE   3
-/* Up to 1/50th of a second of audio for peak calculation */
-/* This should use NATIVE_FREQUENCY, or eventually an adjustable freq. value */
-#define PEAK_SAMPLES  (44100/50)
 void pcm_calculate_peaks(int *left, int *right)
 {
-    short *addr;
-    short *end;
-    {
-        size_t samples = p_size / 4;
-        addr = p;
+    static unsigned long last_peak_tick = 0;
+    static unsigned long frame_period   = 0;
+    static int peaks_l = 0, peaks_r = 0;
 
-        if (samples > PEAK_SAMPLES)
-            samples = PEAK_SAMPLES - (PEAK_STRIDE - 1);
-        else
-            samples -= MIN(PEAK_STRIDE - 1, samples);
-
-        end = &addr[samples * 2];
-    }
+    /* Throttled peak ahead based on calling period */
+    unsigned long period = current_tick - last_peak_tick;
 
-    if (left && right) {
-        int left_peak = 0, right_peak = 0;
+    /* Keep reasonable limits on period */
+    if (period < 1)
+        period = 1;
+    else if (period > HZ/5)
+        period = HZ/5;
 
-        while (addr < end) {
-            int value;
-            if ((value = addr [0]) > left_peak)
-                left_peak = value;
-            else if (-value > left_peak)
-                left_peak = -value;
+    frame_period = (3*frame_period + period) >> 2;
 
-            if ((value = addr [PEAK_STRIDE | 1]) > right_peak)
-                right_peak = value;
-            else if (-value > right_peak)
-                right_peak = -value;
+    last_peak_tick = current_tick;
 
-            addr = &addr[PEAK_STRIDE * 2];
+    if (pcm_playing && !pcm_paused)
+    {
+        unsigned long *addr = (unsigned long *)DCSRC2;
+        long samples = DSTAT2;
+        long samp_frames;
+
+        samples    &= 0xFFFFE;
+        samp_frames = frame_period*pcm_freq/(HZ/2);
+        samples     = MIN(samp_frames, samples) >> 1;
+
+        if (samples > 0)
+        {
+            long peak_l   = 0, peak_r   = 0;
+            long peaksq_l = 0, peaksq_r = 0;
+
+            addr -= 0x30000000 >> 2;
+            addr = (long *)((long)addr & ~3);
+
+            do
+            {
+                long value = *addr;
+                long ch, chsq;
+
+                ch   = (int16_t)value;
+                chsq = ch*ch;
+                if (chsq > peaksq_l)
+                    peak_l = ch, peaksq_l = chsq;
+
+                ch   = value >> 16;
+                chsq = ch*ch;
+                if (chsq > peaksq_r)
+                    peak_r = ch, peaksq_r = chsq;
+
+                addr += 4;
+            }
+            while ((samples -= 4) > 0);
+
+            peaks_l = abs(peak_l);
+            peaks_r = abs(peak_r);
         }
-
-        *left = left_peak;
-        *right = right_peak;
     }
-    else if (left || right) {
-        int peak_value = 0, value;
-
-        if (right)
-            addr += (PEAK_STRIDE | 1);
+    else
+    {
+        peaks_l = peaks_r = 0;
+    }
 
-        while (addr < end) {
-            if ((value = addr [0]) > peak_value)
-                peak_value = value;
-            else if (-value > peak_value)
-                peak_value = -value;
+    if (left)
+        *left = peaks_l;
 
-            addr += PEAK_STRIDE * 2;
-        }
-
-        if (left)
-            *left = peak_value;
-        else
-            *right = peak_value;
-    }
-}
+    if (right)
+        *right = peaks_r;
+} /* pcm_calculate_peaks */
diff --git a/firmware/target/arm/system-arm.h b/firmware/target/arm/system-arm.h
index 775b1ba1c9..26b8ac3513 100644
--- a/firmware/target/arm/system-arm.h
+++ b/firmware/target/arm/system-arm.h
@@ -132,4 +132,23 @@ static inline void disable_fiq(void)
     );
 }
 
+/* This one returns the old status */
+#define FIQ_ENABLED  0x00
+#define FIQ_DISABLED 0x40
+static inline int set_fiq_status(int status)
+{
+    unsigned long cpsr;
+    int oldstatus;
+    /* Read the old level and set the new one */
+    asm volatile (
+        "mrs    %1, cpsr        \n"
+        "bic    %0, %1, #0x40   \n"
+        "orr    %0, %0, %2      \n"
+        "msr    cpsr_c, %0      \n"
+        : "=&r,r"(cpsr), "=&r,r"(oldstatus) : "r,i"(status & 0x40)
+    );
+    return oldstatus;
+}
+
+
 #endif /* SYSTEM_ARM_H */