From 0c5b15121f124f09db476059af092b69f806ac69 Mon Sep 17 00:00:00 2001
From: raphaelcoeffic <1050031+raphaelcoeffic@users.noreply.github.com>
Date: Fri, 17 Jan 2025 11:42:14 +0100
Subject: [PATCH] fix: use saturated sample addition

---
 radio/src/audio.cpp                    | 11 +++++++++--
 radio/src/targets/simu/simpgmspace.cpp |  8 +++++---
 2 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/radio/src/audio.cpp b/radio/src/audio.cpp
index c58c5db4c..d6236a7ce 100644
--- a/radio/src/audio.cpp
+++ b/radio/src/audio.cpp
@@ -382,9 +382,16 @@ void audioTask(void * pdata)
 }
 #endif
 
-inline void mixSample(audio_data_t * result, int sample, unsigned int fade)
+#if !defined(__SSAT)
+  #define _sat_s16(x) ((int16_t)limit<int32_t>(INT16_MIN, (x), INT16_MAX))
+#else
+  #define _sat_s16(x) __SSAT((x), 16)
+#endif
+
+inline void mixSample(audio_data_t * result, int16_t sample, unsigned int fade)
 {
-  *result += (sample >> fade);
+  int32_t tmp = (int32_t)*result + ((int32_t)sample >> fade);
+  *result = (audio_data_t)_sat_s16(tmp);
 }
 
 #define RIFF_CHUNK_SIZE 12
diff --git a/radio/src/targets/simu/simpgmspace.cpp b/radio/src/targets/simu/simpgmspace.cpp
index 9a111fa46..3c546fd58 100644
--- a/radio/src/targets/simu/simpgmspace.cpp
+++ b/radio/src/targets/simu/simpgmspace.cpp
@@ -285,10 +285,12 @@ void audioSetVolume(uint8_t volume)
 #if defined(SIMU_AUDIO)
 void copyBuffer(void* dest, const int16_t* buff, unsigned samples)
 {
+  int16_t* i16_dst = (int16_t*)dest;
   for (unsigned i = 0; i < samples; i++) {
-    int16_t sample = buff[i];
-    *((int16_t*)dest) = (int16_t)((sample * simuAudio.currentVolume) / 127);
-    dest = (int16_t*)dest + 1;
+    int32_t sample = (((int32_t)buff[i] * (int32_t)simuAudio.currentVolume) / 127);
+    if (sample > INT16_MAX) sample = INT16_MAX;
+    else if (sample < INT16_MIN) sample = INT16_MIN;
+    *(i16_dst++) = (int16_t)sample;
   }
 }