3 files changed, 40 insertions, 9 deletions

diff --git a/firmware/target/arm/imx233/audioout-imx233.c b/firmware/target/arm/imx233/audioout-imx233.c
index 0a48a909e8..e03de5c861 100644
--- a/firmware/target/arm/imx233/audioout-imx233.c
+++ b/firmware/target/arm/imx233/audioout-imx233.c
@@ -23,6 +23,8 @@
 #include "rtc-imx233.h"
 #include "pcm_sampr.h"
 
+static int hp_vol_l, hp_vol_r;
+
 void imx233_audioout_preinit(void)
 {
     /* Enable AUDIOOUT block */
@@ -78,8 +80,10 @@ void imx233_audioout_close(void)
     /* will also gate off the module */
     __REG_CLR(HW_AUDIOOUT_CTRL) = HW_AUDIOOUT_CTRL__RUN;
 }
-/* volume in half dB */
-void imx233_audioout_set_dac_vol(int vol_l, int vol_r)
+
+/* volume in half dB
+ * don't check input values */
+static void set_dac_vol(int vol_l, int vol_r)
 {
     /* minimum is -100dB and max is 0dB */
     vol_l = MAX(-200, MIN(vol_l, 0));
@@ -92,14 +96,16 @@ void imx233_audioout_set_dac_vol(int vol_l, int vol_r)
         HW_AUDIOOUT_DACVOLUME__EN_ZCD;
 }
 
-void imx233_audioout_set_hp_vol(int vol_l, int vol_r)
+/* volume in half dB
+ * don't check input values */
+static void set_hp_vol(int vol_l, int vol_r)
 {
     uint32_t select = (HW_AUDIOOUT_HPVOL & HW_AUDIOOUT_HPVOL__SELECT);
     /* minimum is -57.5dB and max is 6dB in DAC mode
      * and -51.5dB / 12dB in Line1 mode */
     int min = select ? -103 : -115;
     int max = select ? 24 : 12;
-    
+
     vol_l = MAX(min, MIN(vol_l, max));
     vol_r = MAX(min, MIN(vol_r, max));
     /* unmute, enable zero cross and set volume. Keep select value. */
@@ -110,6 +116,30 @@ void imx233_audioout_set_hp_vol(int vol_l, int vol_r)
         HW_AUDIOOUT_HPVOL__EN_MSTR_ZCD;
 }
 
+static void apply_volume(void)
+{
+    /* Two cases: line1 and dac */
+    if(HW_AUDIOOUT_HPVOL & HW_AUDIOOUT_HPVOL__SELECT)
+    {
+        /* In line1 mode, the HP is the only way to adjust the volume */
+        set_hp_vol(hp_vol_l, hp_vol_r);
+    }
+    else
+    {
+        /* In DAC mode we can use both the HP and the DAC volume.
+         * Use the DAC for volume <0 and HP for volume >0 */
+        set_dac_vol(MIN(0, hp_vol_l), MIN(0, hp_vol_r));
+        set_hp_vol(MAX(0, hp_vol_l), MAX(0, hp_vol_r));
+    }
+}
+
+void imx233_audioout_set_hp_vol(int vol_l, int vol_r)
+{
+    hp_vol_l = vol_l;
+    hp_vol_r = vol_r;
+    apply_volume();
+}
+
 void imx233_audioout_set_freq(int fsel)
 {
     static struct
@@ -170,10 +200,12 @@ void imx233_audioout_set_freq(int fsel)
 }
 
 /* select between DAC and Line1 */
-void imx233_audiout_select_hp_input(bool line1)
+void imx233_audioout_select_hp_input(bool line1)
 {
     if(line1)
         __REG_SET(HW_AUDIOOUT_HPVOL) = HW_AUDIOOUT_HPVOL__SELECT;
     else
         __REG_CLR(HW_AUDIOOUT_HPVOL) = HW_AUDIOOUT_HPVOL__SELECT;
+    /* reapply volume setting */
+    apply_volume();
 }
diff --git a/firmware/target/arm/imx233/audioout-imx233.h b/firmware/target/arm/imx233/audioout-imx233.h
index cd997e7d31..acfb56138f 100644
--- a/firmware/target/arm/imx233/audioout-imx233.h
+++ b/firmware/target/arm/imx233/audioout-imx233.h
@@ -112,11 +112,10 @@ void imx233_audioout_preinit(void);
 void imx233_audioout_postinit(void);
 void imx233_audioout_close(void);
 /* volume in half dB */
-void imx233_audioout_set_dac_vol(int vol_l, int vol_r);
 void imx233_audioout_set_hp_vol(int vol_l, int vol_r);
 /* frequency index, NOT the frequency itself */
 void imx233_audioout_set_freq(int fsel);
 /* select between DAC and Line1 */
-void imx233_audiout_select_hp_input(bool line1);
+void imx233_audioout_select_hp_input(bool line1);
 
 #endif /* __audioout_imx233__ */
diff --git a/firmware/target/arm/imx233/sansa-fuzeplus/audio-fuzeplus.c b/firmware/target/arm/imx233/sansa-fuzeplus/audio-fuzeplus.c
index abdcda90cb..57a3efd642 100644
--- a/firmware/target/arm/imx233/sansa-fuzeplus/audio-fuzeplus.c
+++ b/firmware/target/arm/imx233/sansa-fuzeplus/audio-fuzeplus.c
@@ -32,9 +32,9 @@ static int output_source = AUDIO_SRC_PLAYBACK;
 static void select_audio_path(void)
 {
     if(input_source == AUDIO_SRC_PLAYBACK)
-        imx233_audiout_select_hp_input(false);
+        imx233_audioout_select_hp_input(false);
     else
-        imx233_audiout_select_hp_input(true);
+        imx233_audioout_select_hp_input(true);
 }
 
 void audio_input_mux(int source, unsigned flags)