From 5f83f1e9d1411d5199bb0d6eb1b024186999357f Mon Sep 17 00:00:00 2001
From: raphaelcoeffic <1050031+raphaelcoeffic@users.noreply.github.com>
Date: Fri, 17 Jan 2025 13:11:03 +0100
Subject: [PATCH] fix(audio): use unsigned saturation for unsigned audio format

---
 radio/src/audio.cpp          | 19 ++++++++++++++++---
 radio/src/audio.h            |  4 ++--
 radio/src/hal/audio_driver.h |  2 +-
 3 files changed, 19 insertions(+), 6 deletions(-)

diff --git a/radio/src/audio.cpp b/radio/src/audio.cpp
index d6236a7cee9..faf75c5168d 100644
--- a/radio/src/audio.cpp
+++ b/radio/src/audio.cpp
@@ -388,14 +388,24 @@ void audioTask(void * pdata)
   #define _sat_s16(x) __SSAT((x), 16)
 #endif
 
+#if !defined(__USAT)
+  #define _sat_u16(x) ((uint16_t)limit<uint32_t>(UINT16_MIN, (x), UINT16_MAX))
+#else
+  #define _sat_u16(x) __USAT((x), 16)
+#endif
+
 inline void mixSample(audio_data_t * result, int16_t sample, unsigned int fade)
 {
   int32_t tmp = (int32_t)*result + ((int32_t)sample >> fade);
+#if AUDIO_SAMPLE_FMT == AUDIO_SAMPLE_FMT_S16
   *result = (audio_data_t)_sat_s16(tmp);
+#elif AUDIO_SAMPLE_FMT == AUDIO_SAMPLE_FMT_U16
+  *result = (audio_data_t)_sat_u16((uint32_t)tmp);
+#endif 
 }
 
 #define RIFF_CHUNK_SIZE 12
-uint8_t wavBuffer[AUDIO_BUFFER_SIZE*2] __DMA;
+uint8_t wavBuffer[AUDIO_BUFFER_SIZE * 2] __DMA;
 
 int WavContext::mixBuffer(AudioBuffer *buffer, int volume, unsigned int fade)
 {
@@ -648,8 +658,11 @@ void AudioQueue::wakeup()
 #if defined(SOFTWARE_VOLUME)
       if (currentSpeakerVolume > 0) {
         for (uint32_t i=0; i<buffer->size; ++i) {
-          int32_t tmpSample = (int32_t) ((uint32_t) (buffer->data[i]) - AUDIO_DATA_SILENCE);  // conversion from uint16_t
-          buffer->data[i] = (int16_t) (((tmpSample * currentSpeakerVolume) / VOLUME_LEVEL_MAX) + AUDIO_DATA_SILENCE);
+          int32_t tmpSample =
+              (int32_t)((uint32_t)(buffer->data[i]) - AUDIO_DATA_SILENCE);
+          buffer->data[i] = (int16_t)(((tmpSample * currentSpeakerVolume) /
+                                       VOLUME_LEVEL_MAX) +
+                                      AUDIO_DATA_SILENCE);
         }
         buffersFifo.audioPushBuffer();
       } else {
diff --git a/radio/src/audio.h b/radio/src/audio.h
index fecf1559848..c06be8f5fcd 100644
--- a/radio/src/audio.h
+++ b/radio/src/audio.h
@@ -84,7 +84,7 @@ constexpr uint8_t AUDIO_FILENAME_MAXLEN = (AUDIO_LUA_FILENAME_MAXLEN > AUDIO_MOD
   #if defined(SIMU) || defined(AUDIO_SPI)
     #define AUDIO_SAMPLE_FMT AUDIO_SAMPLE_FMT_S16
   #else
-    #define AUDIO_SAMPLE_FMT AUDIO_SAMPLE_FMT_U12
+    #define AUDIO_SAMPLE_FMT AUDIO_SAMPLE_FMT_U16
   #endif
 #endif
 
@@ -107,7 +107,7 @@ constexpr uint8_t AUDIO_FILENAME_MAXLEN = (AUDIO_LUA_FILENAME_MAXLEN > AUDIO_MOD
 #if AUDIO_SAMPLE_FMT == AUDIO_SAMPLE_FMT_S16
   typedef int16_t audio_data_t;
   #define AUDIO_DATA_SILENCE 0
-#elif AUDIO_SAMPLE_FMT == AUDIO_SAMPLE_FMT_U12
+#elif AUDIO_SAMPLE_FMT == AUDIO_SAMPLE_FMT_U16
   typedef uint16_t audio_data_t;
   #define AUDIO_DATA_SILENCE 0x8000 
 #else
diff --git a/radio/src/hal/audio_driver.h b/radio/src/hal/audio_driver.h
index 4644a8079dc..4b4150a2926 100644
--- a/radio/src/hal/audio_driver.h
+++ b/radio/src/hal/audio_driver.h
@@ -30,7 +30,7 @@
 #define AUDIO_SAMPLE_RATE 32000
 
 #define AUDIO_SAMPLE_FMT_S16 0
-#define AUDIO_SAMPLE_FMT_U12 1
+#define AUDIO_SAMPLE_FMT_U16 1
 
 
 void audioSetVolume(uint8_t volume);