audio_arm.cpp

diff --git a/radio/src/audio_arm.cpp b/radio/src/audio_arm.cpp
index 1d1bd6f..0dccc94 100644
--- a/radio/src/audio_arm.cpp
+++ b/radio/src/audio_arm.cpp
@@ -511,9 +511,22 @@ void audioTask(void * pdata)
 }
 #endif
 
+#if defined(SOFTWARE_VOLUME)
+extern uint8_t currentSpeakerVolume;
+float vscale = 1.0;
+
+void calcVScale()
+{
+       vscale = currentSpeakerVolume / (float)VOLUME_LEVEL_MAX;
+}
+#endif
+
 void mixSample(audio_data_t * result, int sample, unsigned int fade)
 {
   *result = limit(AUDIO_DATA_MIN, *result + ((sample >> fade) >> (16-AUDIO_BITS_PER_SAMPLE)), AUDIO_DATA_MAX);
+#if defined(SOFTWARE_VOLUME)
+       *result = *result * vscale;
+#endif
 }
 
 #if defined(SDCARD)
@@ -581,6 +594,9 @@ int WavContext::mixBuffer(AudioBuffer *buffer, int volume, unsigned int fade)
         fragment.clear();
       }
 
+#if defined(SOFTWARE_VOLUME)
+      calcVScale();
+#endif
       audio_data_t * samples = buffer->data;
       if (state.codec == CODEC_ID_PCM_S16LE) {
         read /= 2;
@@ -683,6 +699,10 @@ int ToneContext::mixBuffer(AudioBuffer * buffer, int volume, unsigned int fade)
       points = (double(end) - toneIdx) / state.step;
     }
 
+#if defined(SOFTWARE_VOLUME)
+      calcVScale();
+#endif
+
     for (int i=0; i<points; i++) {
       int16_t sample = sineValues[int(toneIdx)] / state.volume;
       mixSample(&buffer->data[i], sample, fade);