From 1b8fe922c267844f08b1795682ff8ab7e2b2a40c Mon Sep 17 00:00:00 2001
From: XANTRONIX Development
Date: Mon, 28 Feb 2022 13:10:56 -0500
Subject: Implement xas_audio_apply_gain()

---
 include/xas/audio.h |  6 ++++++
 src/audio.c         | 52 ++++++++++++++++++++++++++++++++++++++++++++--------
 2 files changed, 50 insertions(+), 8 deletions(-)

diff --git a/include/xas/audio.h b/include/xas/audio.h
index d79615d..5e36f19 100644
--- a/include/xas/audio.h
+++ b/include/xas/audio.h
@@ -72,6 +72,12 @@ void xas_audio_copy(xas_audio_format format,
                       size_t index_src,
                       size_t count);
 
+void xas_audio_apply_gain(xas_audio_format format,
+                            void *dest,
+                            float gain,
+                            size_t index,
+                            size_t count);
+
 xas_audio_stream *xas_audio_stream_new_sink(xas_audio_drain drain,
                                                 xas_audio_cleanup cleanup,
                                                 xas_audio_format format,
diff --git a/src/audio.c b/src/audio.c
index bc2e04d..9caf199 100644
--- a/src/audio.c
+++ b/src/audio.c
@@ -1,6 +1,7 @@
 #include <stdlib.h>
 #include <string.h>
 #include <errno.h>
+#include <math.h>
 
 #include <xas/audio.h>
 
@@ -10,6 +11,17 @@ int xas_audio_format_eq(xas_audio_format a, xas_audio_format b) {
         && a.sample_rate == b.sample_rate;
 }
 
+void xas_audio_zero(xas_audio_format format,
+                      void *dest,
+                      size_t index,
+                      size_t count) {
+    size_t stride = format.channels * format.sample_size;
+
+    memset((uint8_t *)dest + stride * index,
+           '\0',
+           stride * count);
+}
+
 void xas_audio_copy(xas_audio_format format,
                       void *dest,
                       void *src,
@@ -23,15 +35,39 @@ void xas_audio_copy(xas_audio_format format,
            stride * count);
 }
 
-void xas_audio_zero(xas_audio_format format,
-                      void *dest,
-                      size_t index,
-                      size_t count) {
-    size_t stride = format.channels * format.sample_size;
+static inline void apply_gain_mono(int16_t *samples,
+                                   float gain,
+                                   size_t index,
+                                   size_t count) {
+    size_t i;
 
-    memset((uint8_t *)dest + stride * index,
-           '\0',
-           stride * count);
+    for (i=index; i<count; i++) {
+        samples[i] *= (int16_t)roundf(gain * samples[i]);
+    }
+}
+
+static inline void apply_gain_stereo(int16_t *samples,
+                                     float gain,
+                                     size_t index,
+                                     size_t count) {
+    size_t i;
+
+    for (i=index; i<count; i++) {
+        samples[i*2]   *= (int16_t)roundf(gain * samples[i*2]);
+        samples[i*2+1] *= (int16_t)roundf(gain * samples[i*2+1]);
+    }
+}
+
+void xas_audio_apply_gain(xas_audio_format format,
+                            void *dest,
+                            float gain,
+                            size_t index,
+                            size_t count) {
+    if (format.channels == XAS_AUDIO_MONO) {
+        apply_gain_mono(dest, gain, index, count);
+    } else if (format.channels == XAS_AUDIO_STEREO) {
+        apply_gain_stereo(dest, gain, index, count);
+    }
 }
 
 static xas_audio_stream *stream_new(enum xas_audio_stream_type type,
-- 
cgit v1.2.3