Cleanup, improve volume calculations

Signed-off-by: Andrew Opalach <andrew@akon.city>

1 files changed, 36 insertions, 4 deletions

diff --git a/src/buffer/volume.h b/src/buffer/volume.h
index f785eb9..0cad8d3 100644
--- a/src/buffer/volume.h
+++ b/src/buffer/volume.h
@@ -2,13 +2,43 @@
 
 AL_UNUSED_FUNCTION_PUSH
 
-static inline f32 apply_audio_f32(f32 *data, size_t sample_count, s32 channel_count, f32 volume, f32 step)
+// https://godbolt.org/#z:OYLghAFBqd5QCxAYwPYBMCmBRdBLAF1QCcAaPECAMzwBtMA7AQwFtMQByARg9KtQYEAysib0QXACx8BBAKoBnTAAUAHpwAMvAFYTStJg1AB9U8lJL6yAngGVG6AMKpaAVxYMQ0hwBk8DTAA5dwAjTGIvAFZSAAdUBUJbBmc3Dy9Y%2BMSBPwDgljCIyWjLTGskoQImYgIU909pErKBCqqCHKDQ8KiLSuratIbetv8O/K6igEoLVFdiZHYOAFIAJgBmf2Q3LABqRdXHBQJ8QQA6BD3sRY0AQRX1hk3XHb2Do6wqM4ur27WNrcxdvsqLRUEwCJ9Vpcbt87u8Rttrj5jABZa4ADQgTFI2xCE22%2BIJu0ijm%2BEEWAHYAEKEmm0ukExbE76E0wEACeMUwqCopkxeOMTEBABFtkw9tSiSSbizjOzOdzebjtsYQsKceL8YypdcWYKLsrVXsAGLKwUgA0awlamHkoUTGFrOEBBFI5EASUCmOxStp1puZKp9KD9L9OoJrI5XJ5xj5prVYtWEtDMrlUcV/MNqxFIUtyfDev2BsBJoF23NKstDKZ0Nt9uhjswNGdiOMjh812RyggDGxIOxCDweJb7s9A%2Bxw/RED72wYEzrtxuqfe238BAAHLLtgo1%2BLvkvG9tXKuNwRDzvE3vI8vV1wAGybhR33eLq8Ho%2BCO%2Bb1xPi8vznXwRVmWB8gOfa59yoQ9VyAr9QN/cDX0g1db0kB8ULAiCoMEFCv3Q%2BDL3/A9gVBU8qDgykCMwZd0BmEJ6G2Kg8Io6EbkOME8GQBigOVAUCAIYg8BCVwCEwUxtigBhUH8WgRjnUUYhiWg2WMAA3Fx3FEsjlmobiACp0DBLEtzwAAvUTTwUVhFNEtBXEEbEFG45AEEMAJaGMWz7K45ZtjU1JMGxLStxEmJ5wpZiw3xfhiHE1xuLwNUNA1BKXi3Kz6A8mZBGSlYKOWSk8XC5k6TwSCIBS5ZIm2ZzXMwdzPNPPYhSa7YNEKqliuDPyNLVFs2w7LturYXZ8uCzAYmxDQTiobEuGm%2B14JDW1OppRjJDSlhrLVagUImAzKkZSk8EZZrFrpSzNvo3SWqGzAwPpfaxUiI6Tu2rSJj69tOwgC7rOxI0fAAFRRD1/qBlF0QWiKVopZrpQJYhMAIWYGF89S2DA2GYRuVdthYJh/AgNS8HQMKOvhqLuMew6uGWSRlhO%2B6CSC261TmpKzvxVmWqYBSlNU9HNKAiBHtmunlmxCW0f8yaTg0LguCoKGVsR5HiFRjnodtDgploThIl4TxuF4VBOEcLcZjmAE7h4UgCE0HWpgAaxASJVhOLgAE5PbXenPciOnPdWLhoj1jhJENh3SFNjheAUEANDth2pjgWAkDQTa6HCchKAzmIs4iZBNkMYAuFWeW%2BDoETiHjiAQijkJ/CqNlOFtxvmGINkAHkQm0Up7eN0gM7YQQu4YJSo6wITgEcMRaHjwesHxoxxA4LRSHwRGyhUzAF/XzBVFKYSFlt1c6qjmSQmIZvnCwKP%2BLwFhW94HfiBCeJMCFTBl%2BAGSjGTvgBhgAKAAGp4EwAAdy7pyI2tt%2BCCBEGIdgUgZCCEUCodQa8dAgAlgYf%2BpgPL6EEvHSAUxUAxBsAIBeABaQ46AmrIG9tsahXcfLUPxnMc4WYTIMFfmwlgyMAhNR4Xwk2r8BJYBIRAKYjRKGeAgA4fonguCkF8CMPIBQ9BxASHIpRWjMhyPaBoroKjZHlCGHo0xdV%2B7mNaEYzoERTEWPRsonodj1EOIkDIy28wvH6H1pHLB0dODbFUGuW81CULVTwcAbYZc5Ye3ErgQgJARrBwmLwAeWg5ykBdpIW8Jw1waHlpEckGhJDkkiBoW8a5yT%2BPDoE9eMc44JyTlgnJYdliNJNpwTJycpivwSHYSQQA
+
+static inline f32 apply_volume_f32(f32 *data, size_t sample_count, s32 channel_count, f32 volume, f32 step)
+{
+    for (u32 i = 0; i < sample_count; i++) {
+        if (i % channel_count == 0) {
+            volume = AL_CLAMP(volume + step, 0.f, 1.f);
+        }
+        f64 sample = (f64)data[i];
+        sample *= volume;
+        data[i] = (f32)AL_CLAMP(sample, FLT_MIN, FLT_MAX);
+    }
+    return volume;
+}
+
+static inline f32 apply_volume_s32(s32 *data, size_t sample_count, s32 channel_count, f32 volume, f32 step)
+{
+    for (u32 i = 0; i < sample_count; i++) {
+        if (i % channel_count == 0) {
+            volume = AL_CLAMP(volume + step, 0.f, 1.f);
+        }
+        f64 sample = (f64)data[i];
+        sample *= volume;
+        data[i] = (s32)AL_CLAMP(sample, INT32_MIN, INT32_MAX);
+    }
+    return volume;
+}
+
+static inline f32 apply_volume_s16(s16 *data, size_t sample_count, s32 channel_count, f32 volume, f32 step)
 {
     for (u32 i = 0; i < sample_count; i++) {
-        if ((i + 1) % channel_count == 0) {
+        if (i % channel_count == 0) {
             volume = AL_CLAMP(volume + step, 0.f, 1.f);
         }
-        data[i] *= volume;
+        f64 sample = (f64)data[i];
+        sample *= volume;
+        data[i] = (s16)AL_CLAMP(sample, INT16_MIN, INT16_MAX);
     }
     return volume;
 }
@@ -20,13 +50,15 @@ static inline f32 apply_volume(u8 *data, size_t size, struct camu_audio_format *
     case 4:
         sample_count = size / 4;
         if (fmt->format == CAMU_SAMPLE_FORMAT_S32) {
+            return apply_volume_s32((s32 *)data, sample_count, fmt->channel_count, volume, step);
         } else if (fmt->format == CAMU_SAMPLE_FORMAT_FLT) {
-            return apply_audio_f32((f32 *)data, sample_count, fmt->channel_count, volume, step);
+            return apply_volume_f32((f32 *)data, sample_count, fmt->channel_count, volume, step);
         }
         break;
     case 2:
         sample_count = size / 2;
         if (fmt->format == CAMU_SAMPLE_FORMAT_S16) {
+            return apply_volume_s16((s16 *)data, sample_count, fmt->channel_count, volume, step);
         }
         break;
     }