6 files changed, 132 insertions, 30 deletions

diff --git a/apps/dsp.c b/apps/dsp.c
index 0ffaaea8d8..f306069a87 100644
--- a/apps/dsp.c
+++ b/apps/dsp.c
@@ -116,9 +116,10 @@ struct crossfeed_data
                             /* 8ch */
 };
 
-/* Current setup is one lowshelf filters, three peaking filters and one
-   highshelf filter. Varying the number of shelving filters make no sense,
-   but adding peaking filters is possible. */
+/* Current setup is one lowshelf filters three peaking filters and one
+ *  highshelf filter. Varying the number of shelving filters make no sense,
+ *  but adding peaking filters is possible.
+ */
 struct eq_state
 {
     char enabled[5];            /* 00h - Flags for active filters */
@@ -171,6 +172,13 @@ static long   dither_bias IBSS_ATTR;
 struct crossfeed_data crossfeed_data IBSS_ATTR;     /* A */
 /* Equalizer */
 static struct eq_state eq_data;                     /* A/V */
+#ifdef HAVE_SW_TONE_CONTROLS
+static int prescale;
+static int bass;
+static int treble;
+/* Filter struct for software bass/treble controls */
+static struct eqfilter tone_filter;
+#endif
 
 /* Settings applicable to audio codec only */
 static int  pitch_ratio = 1000;
@@ -704,11 +712,7 @@ void dsp_set_crossfeed(bool enable)
 
 void dsp_set_crossfeed_direct_gain(int gain)
 {
-    /* Work around bug in get_replaygain_int which returns 0 for 0 dB */
-    if (gain == 0)
-        crossfeed_data.gain = 0x7fffffff;
-    else
-        crossfeed_data.gain = get_replaygain_int(gain * -10) << 7;
+    crossfeed_data.gain = get_replaygain_int(gain * -10) << 7;
 }
 
 void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, long cutoff)
@@ -716,8 +720,8 @@ void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, long cutoff)
     long g1 = get_replaygain_int(lf_gain * -10) << 3;
     long g2 = get_replaygain_int(hf_gain * -10) << 3;
 
-    filter_bishelf_coefs(0xffffffff/NATIVE_FREQUENCY*cutoff, g1, g2,
-                         crossfeed_data.coefs);
+    filter_shelf_coefs(0xffffffff/NATIVE_FREQUENCY*cutoff, g1, g2,
+                       crossfeed_data.coefs);
 }
 
 /* Applies crossfeed to the stereo signal in src.
@@ -985,6 +989,36 @@ static void channels_process_sound_chan_mono(int count, int32_t *buf[])
 }
 #endif /* DSP_HAVE_ASM_SOUND_CHAN_MONO */
 
+#ifdef HAVE_SW_TONE_CONTROLS
+static void set_tone_controls(void)
+{
+    filter_bishelf_coefs(0xffffffff/NATIVE_FREQUENCY*200,
+                         0xffffffff/NATIVE_FREQUENCY*3500,
+                         bass, treble, -prescale, tone_filter.coefs);
+}
+
+int dsp_callback(int msg, intptr_t param)
+{
+    switch (msg) {
+    case DSP_CALLBACK_SET_PRESCALE:
+        prescale = param;
+        set_tone_controls();
+        break;
+    /* prescaler is always set after calling any of these, so we wait with
+     * calculating coefs until the above case is hit.
+     */
+    case DSP_CALLBACK_SET_BASS:
+        bass = param;
+        break;
+    case DSP_CALLBACK_SET_TREBLE:
+        treble = param;
+    default:
+        break;
+    }
+    return 0;
+}
+#endif
+
 #ifndef DSP_HAVE_ASM_SOUND_CHAN_CUSTOM
 static void channels_process_sound_chan_custom(int count, int32_t *buf[])
 {
@@ -1068,12 +1102,12 @@ int dsp_process(char *dst, const char *src[], int count)
     int written = 0;
     int samples;
 
-    #if defined(CPU_COLDFIRE) && !defined(SIMULATOR)
+#if defined(CPU_COLDFIRE) && !defined(SIMULATOR)
     /* set emac unit for dsp processing, and save old macsr, we're running in
        codec thread context at this point, so can't clobber it */
     unsigned long old_macsr = coldfire_get_macsr();
     coldfire_set_macsr(EMAC_FRACTIONAL | EMAC_SATURATE);
-    #endif
+#endif
 
     while (count > 0)
     {
@@ -1085,8 +1119,17 @@ int dsp_process(char *dst, const char *src[], int count)
             break; /* I'm pretty sure we're downsampling here */
         if (dsp->apply_crossfeed)
             dsp->apply_crossfeed(tmp, samples);
+        /* TODO: EQ and tone controls need separate structs for audio and voice
+         * DSP processing thanks to filter history. isn't really audible now, but
+         * might be the day we start handling voice more delicately.
+         */
         if (eq_enabled)
             eq_process(samples, tmp);
+#ifdef HAVE_SW_TONE_CONTROLS
+        if ((bass | treble) != 0)
+            eq_filter(tmp, &tone_filter, samples, dsp->data.num_channels,
+                      FILTER_BISHELF_SHIFT);
+#endif
         if (dsp->channels_process)
             dsp->channels_process(samples, tmp);
         dsp->output_samples(samples, &dsp->data, tmp, (int16_t *)dst);
@@ -1095,10 +1138,10 @@ int dsp_process(char *dst, const char *src[], int count)
         yield();
     }
 
-    #if defined(CPU_COLDFIRE) && !defined(SIMULATOR)
+#if defined(CPU_COLDFIRE) && !defined(SIMULATOR)
     /* set old macsr again */
     coldfire_set_macsr(old_macsr);
-    #endif
+#endif
     return written;
 }
 
diff --git a/apps/dsp.h b/apps/dsp.h
index 63dc68cbb4..03118e8c31 100644
--- a/apps/dsp.h
+++ b/apps/dsp.h
@@ -51,6 +51,12 @@ enum
     DSP_CROSSFEED
 };
 
+enum {
+    DSP_CALLBACK_SET_PRESCALE = 0,
+    DSP_CALLBACK_SET_BASS,
+    DSP_CALLBACK_SET_TREBLE
+};
+
 /* A bunch of fixed point assembler helper macros */
 #if defined(CPU_COLDFIRE) && !defined(SIMULATOR)
 /* These macros use the Coldfire EMAC extension and need the MACSR flags set
@@ -209,6 +215,7 @@ void dsp_set_eq_precut(int precut);
 void dsp_set_eq_coefs(int band);
 void sound_set_pitch(int r);
 int sound_get_pitch(void);
+int dsp_callback(int msg, intptr_t param);
 void channels_set(int value);
 void stereo_width_set(int value);
 void dsp_dither_enable(bool enable);
diff --git a/apps/eq.c b/apps/eq.c
index 588c23f89f..1d74db790e 100644
--- a/apps/eq.c
+++ b/apps/eq.c
@@ -7,7 +7,7 @@
  *                     \/            \/     \/    \/            \/
  * $Id$
  *
- * Copyright (C) 2006 Thom Johansen 
+ * Copyright (C) 2006-2007 Thom Johansen 
  *
  * 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.
@@ -127,7 +127,7 @@ static long fsincos(unsigned long phase, long *cos) {
  * @param an gain at Nyquist frequency. s3.27 fixed point.
  * @param c pointer to coefficient storage. The coefs are s0.31 format.
  */
-void filter_bishelf_coefs(unsigned long cutoff, long ad, long an, int32_t *c)
+void filter_shelf_coefs(unsigned long cutoff, long ad, long an, int32_t *c)
 {
     const long one = 1 << 27;
     long a0, a1;
@@ -137,7 +137,7 @@ void filter_bishelf_coefs(unsigned long cutoff, long ad, long an, int32_t *c)
     cs = one + (cs >> 4);
 
     /* For max A = 4 (24 dB) */
-    b0 = FRACMUL_SHL(an, cs, 4) + FRACMUL_SHL(ad, s, 4);
+    b0 = FRACMUL_SHL(ad, s, 4) + FRACMUL_SHL(an, cs, 4);
     b1 = FRACMUL_SHL(ad, s, 4) - FRACMUL_SHL(an, cs, 4);
     a0 = s + cs;
     a1 = s - cs;
@@ -147,6 +147,58 @@ void filter_bishelf_coefs(unsigned long cutoff, long ad, long an, int32_t *c)
     c[2] = -DIV64(a1, a0, 31);
 }
 
+/** 
+ * Calculate second order section filter consisting of one low-shelf and one
+ * high-shelf section.
+ * @param cutoff_low low-shelf midpoint frequency. See eq_pk_coefs for format.
+ * @param cutoff_high high-shelf midpoint frequency.
+ * @param A_low decibel value multiplied by ten, describing gain/attenuation of
+ * low-shelf part. Max value is 24 dB.
+ * @param A_high decibel value multiplied by ten, describing gain/attenuation of
+ * high-shelf part. Max value is 24 dB.
+ * @param A decibel value multiplied by ten, describing additional overall gain.
+ * @param c pointer to coefficient storage. Coefficients are s4.27 format.
+ */
+void filter_bishelf_coefs(unsigned long cutoff_low, unsigned long cutoff_high,
+                          long A_low, long A_high, long A, int32_t *c)
+{
+    long sin1, cos2;        /* s0.31 */
+    long cos1, sin2;        /* s3.28 */
+    int32_t b0, b1, b2, b3; /* s3.28 */
+    int32_t a0, a1, a2, a3;
+    const long gd = get_replaygain_int(A_low*5) << 4; /* 10^(db/40), s3.28 */
+    const long gn = get_replaygain_int(A_high*5) << 4; /* 10^(db/40), s3.28 */
+    const long g = get_replaygain_int(A*10) << 7; /* 10^(db/20), s0.31 */
+
+    sin1 = fsincos(cutoff_low/2, &cos1);
+    sin2 = fsincos(cutoff_high/2, &cos2) >> 3;
+    cos1 >>= 3;
+
+    /* lowshelf filter, ranges listed are for all possible cutoffs */
+    b0 = FRACMUL(sin1, gd) + cos1;   /* 0.25 .. 4.10 */
+    b1 = FRACMUL(sin1, gd) - cos1;   /* -1 .. 3.98 */
+    a0 = DIV64(sin1, gd, 25) + cos1; /* 0.25 .. 4.10 */
+    a1 = DIV64(sin1, gd, 25) - cos1; /* -1 .. 3.98 */
+
+    /* highshelf filter */
+    b2 = sin2 + FRACMUL(cos2, gn);   /* 0.25 .. 4.10 */
+    b3 = sin2 - FRACMUL(cos2, gn);   /* -3.98 .. 1 */
+    a2 = sin2 + DIV64(cos2, gn, 25); /* 0.25 .. 4.10 */
+    a3 = sin2 - DIV64(cos2, gn, 25); /* -3.98 .. 1 */
+
+    /* now we cascade the two first order filters to one second order filter
+     * which can be used by eq_filter(). these resulting coefficients have a
+     * really wide numerical range, so we use a fixed point format which will
+     * work for the selected cutoff frequencies (in dsp.c) only.
+     */
+    const int32_t rcp_a0 = DIV64(1, FRACMUL(a0, a2), 53); /* s3.28 */
+    *c++ = FRACMUL(g, FRACMUL_SHL(FRACMUL(b0, b2), rcp_a0, 5));
+    *c++ = FRACMUL(g, FRACMUL_SHL(FRACMUL(b0, b3) + FRACMUL(b1, b2), rcp_a0, 5));
+    *c++ = FRACMUL(g, FRACMUL_SHL(FRACMUL(b1, b3), rcp_a0, 5));
+    *c++ = -FRACMUL_SHL(FRACMUL(a0, a3) + FRACMUL(a1, a2), rcp_a0, 5);
+    *c++ = -FRACMUL_SHL(FRACMUL(a1, a3), rcp_a0, 5);
+}
+
 /* Coef calculation taken from Audio-EQ-Cookbook.txt by Robert Bristow-Johnson.
  * Slightly faster calculation can be done by deriving forms which use tan()
  * instead of cos() and sin(), but the latter are far easier to use when doing
@@ -162,7 +214,7 @@ void filter_bishelf_coefs(unsigned long cutoff, long ad, long an, int32_t *c)
  * @param Q Q factor value multiplied by ten. Lower bound is artificially set
  * at 0.5.
  * @param db decibel value multiplied by ten, describing gain/attenuation at
- * peak freq.
+ * peak freq. Max value is 24 dB.
  * @param c pointer to coefficient storage. Coefficients are s3.28 format.
  */
 void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c)
diff --git a/apps/eq.h b/apps/eq.h
index 095c8e82f0..83d235959d 100644
--- a/apps/eq.h
+++ b/apps/eq.h
@@ -7,7 +7,7 @@
  *                     \/            \/     \/    \/            \/
  * $Id$
  *
- * Copyright (C) 2006 Thom Johansen
+ * Copyright (C) 2006-2007 Thom Johansen
  *
  * 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.
@@ -25,6 +25,7 @@
 /* These depend on the fixed point formats used by the different filter types
    and need to be changed when they change.
  */
+#define FILTER_BISHELF_SHIFT 5
 #define EQ_PEAK_SHIFT 4
 #define EQ_SHELF_SHIFT 6
 
@@ -33,7 +34,9 @@ struct eqfilter {
     int32_t history[2][4];
 };
 
-void filter_bishelf_coefs(unsigned long cutoff, long ad, long an, int32_t *c);
+void filter_shelf_coefs(unsigned long cutoff, long ad, long an, int32_t *c);
+void filter_bishelf_coefs(unsigned long cutoff_low, unsigned long cutoff_high,
+                          long A_low, long A_high, long A, int32_t *c);
 void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c);
 void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c);
 void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c);
diff --git a/apps/menus/sound_menu.c b/apps/menus/sound_menu.c
index 9dc9579b0e..287b3ec904 100644
--- a/apps/menus/sound_menu.c
+++ b/apps/menus/sound_menu.c
@@ -55,12 +55,8 @@ int soundmenu_callback(int action,const struct menu_item_ex *this_item)
 #endif
 
 MENUITEM_SETTING(volume, &global_settings.volume, soundmenu_callback);
-
-#ifndef HAVE_TLV320
-    MENUITEM_SETTING(bass, &global_settings.bass, soundmenu_callback);
-    MENUITEM_SETTING(treble, &global_settings.treble, soundmenu_callback);
-#endif
-
+MENUITEM_SETTING(bass, &global_settings.bass, soundmenu_callback);
+MENUITEM_SETTING(treble, &global_settings.treble, soundmenu_callback);
 MENUITEM_SETTING(balance, &global_settings.balance, soundmenu_callback);
 MENUITEM_SETTING(channel_config, &global_settings.channel_config, soundmenu_callback);
 MENUITEM_SETTING(stereo_width, &global_settings.stereo_width, soundmenu_callback);
@@ -99,9 +95,7 @@ MENUITEM_SETTING(stereo_width, &global_settings.stereo_width, soundmenu_callback
 
 MAKE_MENU(sound_settings, ID2P(LANG_SOUND_SETTINGS), NULL, bitmap_icons_6x8[Icon_Audio],
           &volume,
-#ifndef HAVE_TLV320
           &bass,&treble,
-#endif
           &balance,&channel_config,&stereo_width
 #if CONFIG_CODEC == SWCODEC
          ,&crossfeed_menu, &equalizer_menu, &dithering_enabled
diff --git a/apps/settings.c b/apps/settings.c
index dd5e7c5ae3..cc5ab12d6f 100644
--- a/apps/settings.c
+++ b/apps/settings.c
@@ -642,6 +642,9 @@ void settings_apply_pm_range(void)
 
 void sound_settings_apply(void)
 {
+#ifdef HAVE_SW_TONE_CONTROLS
+    sound_set_dsp_callback(dsp_callback);
+#endif
     sound_set(SOUND_BASS, global_settings.bass);
     sound_set(SOUND_TREBLE, global_settings.treble);
     sound_set(SOUND_BALANCE, global_settings.balance);
@@ -967,7 +970,7 @@ bool set_sound(const unsigned char * string,
         talkunit = UNIT_PERCENT;
     else if (*unit == 'H')
         talkunit = UNIT_HERTZ;
-    if(!numdec)
+    if (!numdec)
 #if CONFIG_CODEC == SWCODEC
         /* We need to hijack this one and send it off to apps/dsp.c instead of
            firmware/sound.c */
@@ -975,7 +978,7 @@ bool set_sound(const unsigned char * string,
             return set_int(string, unit, talkunit, variable, &stereo_width_set,
                            steps, min, max, NULL );
         else
-#endif   
+#endif
         return set_int(string, unit, talkunit,  variable, sound_callback,
                        steps, min, max, NULL );
     else