6 files changed, 140 insertions, 3 deletions

diff --git a/apps/dsp.c b/apps/dsp.c
index d0f8b6d6d1..c490ed3824 100644
--- a/apps/dsp.c
+++ b/apps/dsp.c
@@ -41,6 +41,15 @@
 #define RESAMPLE_BUF_SIZE   (256 * 4)   /* Enough for 11,025 Hz -> 44,100 Hz*/
 #define DEFAULT_REPLAYGAIN  0x01000000
 
+/* These are the constants for the filters in the crossfeed */
+
+#define ATT 0x0CCCCCCDL /* 0.1 */
+#define ATT_COMP 0x73333333L /* 0.9 */
+#define LOW  0x4CCCCCCDL /* 0.6 */
+#define LOW_COMP 0x33333333L /* 0.4 */
+#define HIGH_NEG 0x9999999AL /* -0.2 */
+#define HIGH_COMP 0x66666666L /* 0.8 */
+
 #if defined(CPU_COLDFIRE) && !defined(SIMULATOR)
 
 /* Multiply two S.31 fractional integers and return the sign bit and the
@@ -86,6 +95,20 @@
     (t << 8) | (u & 0xff); \
 })
 
+
+#define ACC(acc, x, y) \
+    (void)acc; \
+    asm volatile ("mac.l %[a], %[b], %%acc0" \
+                  : : [a] "i,r" (x), [b] "i,r" (y));
+#define GET_ACC(acc) \
+({ \
+    long t; \
+    (void)acc; \
+    asm volatile ("movclr.l %%acc0, %[t]" \
+                  : [t] "=r" (t)); \
+    t; \
+})
+
 #else
 
 #define FRACMUL(x, y) (long) (((((long long) (x)) * ((long long) (y))) >> 31))
@@ -115,6 +138,7 @@ struct dsp_config
     int frac_bits;
     bool dither_enabled;
     bool new_gain;
+    bool crossfeed_enabled;
 };
 
 struct resample_data
@@ -130,9 +154,18 @@ struct dither_data
     long random;
 };
 
+struct crossfeed_data
+{
+    long lowpass[2];
+    long highpass[2];
+    long delay[2][13];
+    int index;
+};
+
 static struct dsp_config dsp_conf[2] IBSS_ATTR;
 static struct dither_data dither_data[2] IBSS_ATTR;
 static struct resample_data resample_data[2][2] IBSS_ATTR;
+static struct crossfeed_data crossfeed_data IBSS_ATTR;
 
 extern int current_codec;
 struct dsp_config *dsp;
@@ -145,7 +178,6 @@ struct dsp_config *dsp;
 static long sample_buf[SAMPLE_BUF_SIZE] IBSS_ATTR;
 static long resample_buf[RESAMPLE_BUF_SIZE] IBSS_ATTR;
 
-
 /* Convert at most count samples to the internal format, if needed. Returns
  * number of samples ready for further processing. Updates src to point
  * past the samples "consumed" and dst is set to point to the samples to
@@ -410,6 +442,76 @@ static long dither_sample(long sample, long bias, long mask,
  * the src array if gain was applied.
  * Note that this must be called before the resampler.
  */
+static void apply_crossfeed(long* src[], int count)
+{
+
+    if (dsp->crossfeed_enabled && src[0] != src[1])
+    {
+        long long a;
+
+        long low_left = crossfeed_data.lowpass[0];
+        long low_right = crossfeed_data.lowpass[1];
+        long high_left = crossfeed_data.highpass[0];
+        long high_right = crossfeed_data.highpass[1];
+        unsigned int index = crossfeed_data.index;
+
+        long left, right;
+
+        long * delay_l = crossfeed_data.delay[0];
+        long * delay_r = crossfeed_data.delay[1];
+
+        int i;
+
+        for (i = 0; i < count; i++)
+        {
+            /* use a low-pass filter on the signal */
+            left = src[0][i];
+            right = src[1][i];
+
+            ACC(a, LOW, low_left); ACC(a, LOW_COMP, left);
+            low_left = GET_ACC(a);
+
+            ACC(a, LOW, low_right); ACC(a, LOW_COMP, right);
+            low_right = GET_ACC(a);
+
+            /* use a high-pass filter on the signal */
+
+            ACC(a, HIGH_NEG, high_left); ACC(a, HIGH_COMP, left);
+            high_left = GET_ACC(a);
+
+            ACC(a, HIGH_NEG, high_right); ACC(a, HIGH_COMP, right);
+            high_right = GET_ACC(a);
+
+            /* New data is the high-passed signal + delayed and attenuated 
+             * low-passed signal from the other channel */
+
+            ACC(a, ATT, delay_r[index]); ACC(a, ATT_COMP, high_left);
+            src[0][i] = GET_ACC(a);
+
+            ACC(a, ATT, delay_l[index]); ACC(a, ATT_COMP, high_right);
+            src[1][i] = GET_ACC(a);
+
+            /* Store the low-passed signal in the ringbuffer */
+
+            delay_l[index] = low_left;
+            delay_r[index] = low_right;
+
+            index = (index + 1) % 13;
+        }
+
+        crossfeed_data.index = index;
+        crossfeed_data.lowpass[0] = low_left;
+        crossfeed_data.lowpass[1] = low_right;
+        crossfeed_data.highpass[0] = high_left;
+        crossfeed_data.highpass[1] = high_right;
+
+    }
+}
+
+/* Apply a constant gain to the samples (e.g., for ReplayGain). May update
+ * the src array if gain was applied.
+ * Note that this must be called before the resampler.
+ */
 static void apply_gain(long* src[], int count)
 {
     if (dsp->replaygain)
@@ -509,6 +611,7 @@ long dsp_process(char* dst, char* src[], long size)
         size -= samples;
         apply_gain(tmp, samples);
         samples = resample(tmp, samples);
+        apply_crossfeed(tmp, samples);
         write_samples((short*) dst, tmp, samples);
         written += samples;
         dst += samples * sizeof(short) * 2;
@@ -698,6 +801,13 @@ bool dsp_configure(int setting, void *value)
     return 1;
 }
 
+void dsp_set_crossfeed(bool enable)
+{
+    if (enable)
+        memset(&crossfeed_data, 0, sizeof(crossfeed_data));
+    dsp->crossfeed_enabled = enable;
+}
+
 void dsp_set_replaygain(bool always)
 {
     dsp = &dsp_conf[current_codec];
diff --git a/apps/dsp.h b/apps/dsp.h
index fdd9a191cb..19ba6db980 100644
--- a/apps/dsp.h
+++ b/apps/dsp.h
@@ -44,7 +44,8 @@ enum {
     DSP_SET_TRACK_GAIN,
     DSP_SET_ALBUM_GAIN,
     DSP_SET_TRACK_PEAK,
-    DSP_SET_ALBUM_PEAK
+    DSP_SET_ALBUM_PEAK,
+    DSP_CROSSFEED
 };
 
 long dsp_process(char *dest, char *src[], long size);
@@ -53,5 +54,6 @@ long dsp_output_size(long size);
 int dsp_stereo_mode(void);
 bool dsp_configure(int setting, void *value);
 void dsp_set_replaygain(bool always);
+void dsp_set_crossfeed(bool enable);
 
 #endif
diff --git a/apps/lang/english.lang b/apps/lang/english.lang
index 001fe4584a..1a4b88c96a 100644
--- a/apps/lang/english.lang
+++ b/apps/lang/english.lang
@@ -3388,3 +3388,15 @@ desc: error message when preset list is empty
 eng: "Screen frozen!"
 voice: ""
 new:
+
+id: LANG_CROSSFEED_ENABLE
+desc: In the crossfeed menu
+eng: "Enable crossfeed"
+voice:
+new:
+
+id: LANG_CROSSFEED
+desc: in the sound settings menu
+eng: "Crossfeed"
+voice:
+new:
diff --git a/apps/settings.c b/apps/settings.c
index 742b11276d..f483b8dd73 100644
--- a/apps/settings.c
+++ b/apps/settings.c
@@ -85,7 +85,7 @@ const char rec_base_directory[] = REC_BASE_DIR;
 #include "dsp.h"
 #endif
 
-#define CONFIG_BLOCK_VERSION 30
+#define CONFIG_BLOCK_VERSION 31
 #define CONFIG_BLOCK_SIZE 512
 #define RTC_BLOCK_SIZE 44
 
@@ -444,6 +444,7 @@ static const struct bit_entry hd_bits[] =
     {4, S_O(crossfade_fade_in_duration), 0, "crossfade fade in duration", NULL},
     {4, S_O(crossfade_fade_out_duration), 0, "crossfade fade out duration", NULL},
     {1, S_O(crossfade_fade_out_mixmode), 0, "crossfade fade out mode", "crossfade,mix"},
+    {1, S_O(crossfeed), false, "crossfeed", off_on },
 #endif
 #ifdef HAVE_DIRCACHE
     {1, S_O(dircache), false, "dircache", off_on },
@@ -912,6 +913,7 @@ void settings_apply(void)
 #if CONFIG_CODEC == SWCODEC
     audio_set_crossfade(global_settings.crossfade);
     dsp_set_replaygain(true);
+    dsp_set_crossfeed(global_settings.crossfeed);
 #endif
 
 #ifdef HAVE_SPDIF_POWER
diff --git a/apps/settings.h b/apps/settings.h
index e605a8d1fa..d1a367cc64 100644
--- a/apps/settings.h
+++ b/apps/settings.h
@@ -353,6 +353,7 @@ struct user_settings
                                shuffle is on, album gain otherwise */
     int  replaygain_preamp; /* scale replaygained tracks by this */
     int  beep;              /* system beep volume when changing tracks etc. */
+    bool crossfeed;         /* enable crossfeed */
 #endif
 #ifdef HAVE_DIRCACHE
     bool dircache;          /* enable directory cache */
diff --git a/apps/settings_menu.c b/apps/settings_menu.c
index 102453033e..ce2399b1e5 100644
--- a/apps/settings_menu.c
+++ b/apps/settings_menu.c
@@ -1203,6 +1203,15 @@ static bool replaygain_settings_menu(void)
     return result;
 }
 
+static bool crossfeed(void)
+{
+    bool result = set_bool(str(LANG_CROSSFEED_ENABLE), 
+        &global_settings.crossfeed);
+
+    dsp_set_crossfeed(global_settings.crossfeed);
+    return result;
+}
+
 static bool crossfade(void)
 {
     static const struct opt_items names[] = {
@@ -1354,6 +1363,7 @@ static bool playback_settings_menu(void)
 #if CONFIG_CODEC == SWCODEC
         { ID2P(LANG_CROSSFADE), crossfade_settings_menu },
         { ID2P(LANG_REPLAYGAIN), replaygain_settings_menu },
+        { ID2P(LANG_CROSSFEED), crossfeed },
         { ID2P(LANG_BEEP), beep },
 #endif
 #ifdef HAVE_SPDIF_POWER