5 files changed, 105 insertions, 1 deletions

diff --git a/apps/dsp.c b/apps/dsp.c
index de88c97ca6..f735456393 100644
--- a/apps/dsp.c
+++ b/apps/dsp.c
@@ -18,6 +18,7 @@
  ****************************************************************************/
 #include <inttypes.h>
 #include <string.h>
+#include <sound.h>
 #include "dsp.h"
 #include "eq.h"
 #include "kernel.h"
@@ -224,6 +225,8 @@ struct crossfeed_data crossfeed_data IBSS_ATTR;
 static struct eq_state eq_data;
 
 static int pitch_ratio = 1000;
+static int channels_mode = 0;
+static int32_t sw_gain, sw_cross;
 
 extern int current_codec;
 struct dsp_config *dsp;
@@ -773,6 +776,70 @@ static void apply_gain(int32_t* _src[], int _count)
     }
 }
 
+void channels_set(int value)
+{
+    channels_mode =  value;    
+}
+
+void stereo_width_set(int value)
+{
+    long width, straight, cross;
+    
+    width = value*0x7fffff/100;
+    if (value <= 100) {
+        straight = (0x7fffff + width)/2;
+        cross = straight - width;
+    } else {
+        straight = 0x7fffff; 
+        cross = 0x7fffff - ((int64_t)(2*width) << 23)/(0x7fffff + width);
+    }
+    sw_gain = straight << 8;
+    sw_cross = cross << 8;
+}
+
+/* Implements the different channel configurations and stereo width.
+ * We might want to combine this with the write_samples stage for efficiency,
+ * but for now we'll just let it stay as a stage of its own. 
+ */
+static void channels_process(int32_t **src, int num)
+{
+    int i;
+    int32_t *sl = src[0], *sr = src[1];
+
+    if (channels_mode == SOUND_CHAN_STEREO)
+        return;
+    switch (channels_mode) {
+    case SOUND_CHAN_MONO:
+        for (i = 0; i < num; i++)
+            sl[i] = sr[i] = sl[i]/2 + sr[i]/2;
+        break;
+    case SOUND_CHAN_CUSTOM:
+        for (i = 0; i < num; i++) {
+            int32_t left_sample = sl[i];
+
+            sl[i] = FRACMUL(sl[i], sw_gain) + FRACMUL(sr[i], sw_cross);
+            sr[i] = FRACMUL(sr[i], sw_gain) + FRACMUL(left_sample, sw_cross);
+        }
+        break;
+    case SOUND_CHAN_MONO_LEFT:
+        for (i = 0; i < num; i++)
+            sr[i] = sl[i];
+        break;
+    case SOUND_CHAN_MONO_RIGHT:
+        for (i = 0; i < num; i++)
+            sl[i] = sr[i];
+        break;
+    case SOUND_CHAN_KARAOKE:
+        for (i = 0; i < num; i++) {
+            int32_t left_sample = sl[i];
+            
+            sl[i] -= sr[i];
+            sr[i] -= left_sample;
+        }
+        break;
+    }
+}
+
 static void write_samples(short* dst, int32_t* src[], int count)
 {
     int32_t* s0 = src[0];
@@ -843,6 +910,8 @@ long dsp_process(char* dst, const char* src[], long size)
             apply_crossfeed(tmp, samples);
         if (dsp->eq_enabled)
             eq_process(tmp, samples);
+        if (dsp->stereo_mode != STEREO_MONO)
+            channels_process(tmp, samples);
         write_samples((short*) dst, tmp, samples);
         written += samples;
         dst += samples * sizeof(short) * 2;
diff --git a/apps/dsp.h b/apps/dsp.h
index c20def03b7..8b8b164d6e 100644
--- a/apps/dsp.h
+++ b/apps/dsp.h
@@ -57,4 +57,7 @@ void dsp_set_crossfeed(bool enable);
 void dsp_eq_update_data(bool enabled, int band);
 void sound_set_pitch(int r);
 int sound_get_pitch(void);
+void channels_set(int value);
+void stereo_width_set(int value);
+
 #endif
diff --git a/apps/settings.c b/apps/settings.c
index 7329b12a76..f5c0ba7c2d 100644
--- a/apps/settings.c
+++ b/apps/settings.c
@@ -950,8 +950,13 @@ void sound_settings_apply(void)
     sound_set(SOUND_TREBLE, global_settings.treble);
     sound_set(SOUND_BALANCE, global_settings.balance);
     sound_set(SOUND_VOLUME, global_settings.volume);
+#if CONFIG_CODEC == SWCODEC
+    channels_set(global_settings.channel_config);
+    stereo_width_set(global_settings.stereo_width);
+#else
     sound_set(SOUND_CHANNELS, global_settings.channel_config);
     sound_set(SOUND_STEREO_WIDTH, global_settings.stereo_width);
+#endif
 #if (CONFIG_CODEC == MAS3587F) || (CONFIG_CODEC == MAS3539F)
     sound_set(SOUND_LOUDNESS, global_settings.loudness);
     sound_set(SOUND_AVC, global_settings.avc);
diff --git a/apps/sound_menu.c b/apps/sound_menu.c
index 52a8d4e6e1..1c61c6630a 100644
--- a/apps/sound_menu.c
+++ b/apps/sound_menu.c
@@ -83,6 +83,14 @@ bool set_sound(const unsigned char * string,
     else if (*unit == 'H')
         talkunit = UNIT_HERTZ;
     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 */
+        if (setting == SOUND_STEREO_WIDTH)
+            return set_int(string, unit, talkunit,  variable, &stereo_width_set,
+                           steps, min, max, NULL );
+        else
+#endif   
         return set_int(string, unit, talkunit,  variable, sound_callback,
                        steps, min, max, NULL );
     else
@@ -375,8 +383,13 @@ static bool chanconf(void)
         { STR(LANG_CHANNEL_RIGHT) },
         { STR(LANG_CHANNEL_KARAOKE) }
     };
+#if CONFIG_CODEC == SWCODEC
+    return set_option(str(LANG_CHANNEL), &global_settings.channel_config, INT,
+                      names, 6, channels_set);
+#else
     return set_option(str(LANG_CHANNEL), &global_settings.channel_config, INT,
                       names, 6, sound_set_channels);
+#endif
 }
 
 static bool stereo_width(void)
diff --git a/firmware/sound.c b/firmware/sound.c
index 501e01e2d4..a3eb6da8a8 100644
--- a/firmware/sound.c
+++ b/firmware/sound.c
@@ -458,10 +458,14 @@ static void set_prescaled_volume(void)
 #endif /* (CONFIG_CODEC == MAS3507D) || defined HAVE_UDA1380 */
 #endif /* !SIMULATOR */
 
+#if CONFIG_CODEC != SWCODEC
 int channel_configuration = SOUND_CHAN_STEREO;
 int stereo_width = 100;
+#endif
 
 #ifndef SIMULATOR
+
+#if CONFIG_CODEC != SWCODEC
 static void set_channel_config(void)
 {
     /* default values: stereo */
@@ -540,6 +544,8 @@ static void set_channel_config(void)
 #endif
 }
 
+#endif /* CONFIG_CODEC != SWCODEC */
+
 #if (CONFIG_CODEC == MAS3587F) || (CONFIG_CODEC == MAS3539F)
 unsigned long mdb_shape_shadow = 0;
 unsigned long loudness_shadow = 0;
@@ -631,19 +637,27 @@ void sound_set_treble(int value)
 
 void sound_set_channels(int value)
 {
+#if CONFIG_CODEC == SWCODEC
+    (void)value;
+#else
     if(!audio_is_initialized)
         return;
     channel_configuration = value;
-    set_channel_config();  
+    set_channel_config();
+#endif
 }
 
 void sound_set_stereo_width(int value)
 {
+#if CONFIG_CODEC == SWCODEC
+    (void)value;
+#else
     if(!audio_is_initialized)
         return;
     stereo_width = value;
     if (channel_configuration == SOUND_CHAN_CUSTOM)
         set_channel_config();
+#endif
 }
 
 #if (CONFIG_CODEC == MAS3587F) || (CONFIG_CODEC == MAS3539F)