From 008799dc520218ecb96a782feb145b37b842862a Mon Sep 17 00:00:00 2001
From: dean <deanm1278@gmail.com>
Date: Wed, 31 Oct 2018 18:27:08 -0400
Subject: [PATCH] DM: adding mixer gain

---
 shared-bindings/audioio/Mixer.c | 34 +++++++++++++
 shared-bindings/audioio/Mixer.h |  1 +
 shared-module/audioio/Mixer.c   | 84 +++++++++++++++++++++++++++++++++
 shared-module/audioio/Mixer.h   |  1 +
 4 files changed, 120 insertions(+)

diff --git a/shared-bindings/audioio/Mixer.c b/shared-bindings/audioio/Mixer.c
index dc7a12ebc0..a20e95b3b4 100644
--- a/shared-bindings/audioio/Mixer.c
+++ b/shared-bindings/audioio/Mixer.c
@@ -190,6 +190,39 @@ STATIC mp_obj_t audioio_mixer_obj_stop_voice(size_t n_args, const mp_obj_t *pos_
 }
 MP_DEFINE_CONST_FUN_OBJ_KW(audioio_mixer_stop_voice_obj, 1, audioio_mixer_obj_stop_voice);
 
+//|   .. method:: set_gain(voice, gain)
+//|
+//|     Set the gain of a voice.
+//|
+//|     gain must be a floating point number between 0 and 1
+//|
+STATIC mp_obj_t audioio_mixer_obj_set_gain(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
+    enum { ARG_voice, ARG_gain };
+    static const mp_arg_t allowed_args[] = {
+        { MP_QSTR_voice,    MP_ARG_INT | MP_ARG_REQUIRED },
+        { MP_QSTR_gain,     MP_ARG_OBJ | MP_ARG_REQUIRED },
+    };
+    audioio_mixer_obj_t *self = MP_OBJ_TO_PTR(pos_args[0]);
+    raise_error_if_deinited(common_hal_audioio_mixer_deinited(self));
+    mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
+    mp_arg_parse_all(n_args - 1, pos_args + 1, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
+
+	#if MICROPY_PY_BUILTINS_FLOAT
+	float gain = mp_obj_get_float(args[ARG_gain].u_obj);
+	#else
+	#error "floating point not supported"
+	#endif
+
+    if (gain > 1 || gain < 0) {
+        mp_raise_ValueError(translate("gain must be between 0 and 1"));
+    }
+
+    common_hal_audioio_mixer_set_gain(self,args[ARG_voice].u_int, gain);
+
+    return mp_const_none;
+}
+MP_DEFINE_CONST_FUN_OBJ_KW(audioio_mixer_set_gain_obj, 1, audioio_mixer_obj_set_gain);
+
 //|   .. attribute:: playing
 //|
 //|     True when any voice is being output. (read-only)
@@ -234,6 +267,7 @@ STATIC const mp_rom_map_elem_t audioio_mixer_locals_dict_table[] = {
     { MP_ROM_QSTR(MP_QSTR___exit__), MP_ROM_PTR(&audioio_mixer___exit___obj) },
     { MP_ROM_QSTR(MP_QSTR_play), MP_ROM_PTR(&audioio_mixer_play_obj) },
     { MP_ROM_QSTR(MP_QSTR_stop_voice), MP_ROM_PTR(&audioio_mixer_stop_voice_obj) },
+	{ MP_ROM_QSTR(MP_QSTR_set_gain), MP_ROM_PTR(&audioio_mixer_set_gain_obj) },
 
     // Properties
     { MP_ROM_QSTR(MP_QSTR_playing), MP_ROM_PTR(&audioio_mixer_playing_obj) },
diff --git a/shared-bindings/audioio/Mixer.h b/shared-bindings/audioio/Mixer.h
index 832072b8f0..11ca2aac54 100644
--- a/shared-bindings/audioio/Mixer.h
+++ b/shared-bindings/audioio/Mixer.h
@@ -45,6 +45,7 @@ void common_hal_audioio_mixer_deinit(audioio_mixer_obj_t* self);
 bool common_hal_audioio_mixer_deinited(audioio_mixer_obj_t* self);
 void common_hal_audioio_mixer_play(audioio_mixer_obj_t* self, mp_obj_t sample, uint8_t voice, bool loop);
 void common_hal_audioio_mixer_stop_voice(audioio_mixer_obj_t* self, uint8_t voice);
+void common_hal_audioio_mixer_set_gain(audioio_mixer_obj_t* self, uint8_t voice, float gain);
 
 bool common_hal_audioio_mixer_get_playing(audioio_mixer_obj_t* self);
 uint32_t common_hal_audioio_mixer_get_sample_rate(audioio_mixer_obj_t* self);
diff --git a/shared-module/audioio/Mixer.c b/shared-module/audioio/Mixer.c
index 8020a621ed..a6ce1937b7 100644
--- a/shared-module/audioio/Mixer.c
+++ b/shared-module/audioio/Mixer.c
@@ -61,6 +61,7 @@ void common_hal_audioio_mixer_construct(audioio_mixer_obj_t* self,
 
     for (uint8_t i = 0; i < self->voice_count; i++) {
         self->voice[i].sample = NULL;
+        self->voice[i].gain = ((1<<15)-1);
     }
 }
 
@@ -131,6 +132,11 @@ void audioio_mixer_reset_buffer(audioio_mixer_obj_t* self,
     }
 }
 
+void common_hal_audioio_mixer_set_gain(audioio_mixer_obj_t* self, uint8_t voice, float gain) {
+	audioio_mixer_voice_t* v = &self->voice[voice];
+	v->gain = gain * ((1 << 15)-1);
+}
+
 uint32_t add8signed(uint32_t a, uint32_t b) {
     #if (defined (__ARM_FEATURE_DSP) && (__ARM_FEATURE_DSP == 1))
     return __QADD8(a, b);
@@ -215,6 +221,68 @@ uint32_t add16unsigned(uint32_t a, uint32_t b) {
     #endif
 }
 
+//TODO:
+static inline uint32_t mult8unsigned(uint32_t val, int32_t mul) {
+	#if (defined (__ARM_FEATURE_DSP) && (__ARM_FEATURE_DSP == 1))
+    return val;
+    val = __USUB8(val, 0x80808080);
+	#else
+	uint32_t result = 0;
+	for (int8_t i = 0; i < 4; i++) {
+		int8_t ai = (val >> (sizeof(uint8_t) * 8 * i)) - 128;
+	}
+	return result;
+	#endif
+}
+
+//TODO:
+static inline uint32_t mult8signed(uint32_t val, int32_t mul) {
+	#if (defined (__ARM_FEATURE_DSP) && (__ARM_FEATURE_DSP == 1))
+	return val;
+	#else
+	uint32_t result = 0;
+	for (int8_t i = 0; i < 4; i++) {
+		int8_t ai = val >> (sizeof(int8_t) * 8 * i);
+	}
+	return result;
+	#endif
+}
+
+//TODO:
+static inline uint32_t mult16unsigned(uint32_t val, int32_t mul) {
+	#if (defined (__ARM_FEATURE_DSP) && (__ARM_FEATURE_DSP == 1))
+    return val;
+	val = __USUB16(val, 0x80008000);
+	#else
+	uint32_t result = 0;
+	for (int8_t i = 0; i < 2; i++) {
+		int16_t ai = (val >> (sizeof(uint16_t) * 8 * i)) - 0x8000;
+	}
+	return result;
+	#endif
+}
+
+static inline uint32_t mult16signed(uint32_t val, int32_t mul) {
+	#if (defined (__ARM_FEATURE_DSP) && (__ARM_FEATURE_DSP == 1))
+	int32_t hi, lo;
+	int32_t bits = 16; // saturate to 16 bits
+	int32_t shift = 0; // shift is done automatically
+	asm volatile("smulwb %0, %1, %2" : "=r" (lo) : "r" (mul), "r" (val));
+	asm volatile("smulwt %0, %1, %2" : "=r" (hi) : "r" (mul), "r" (val));
+	asm volatile("ssat %0, %1, %2, asr %3" : "=r" (lo) : "I" (bits), "r" (lo), "I" (shift));
+	asm volatile("ssat %0, %1, %2, asr %3" : "=r" (hi) : "I" (bits), "r" (hi), "I" (shift));
+	asm volatile("pkhbt %0, %1, %2, lsl #16" : "=r" (val) : "r" (lo), "r" (hi)); // pack
+    return val;
+	#else
+	uint32_t result = 0;
+	//TODO:
+	for (int8_t i = 0; i < 2; i++) {
+		int16_t ai = val >> (sizeof(int16_t) * 8 * i);
+	}
+	return result;
+	#endif
+}
+
 audioio_get_buffer_result_t audioio_mixer_get_buffer(audioio_mixer_obj_t* self,
                                                      bool single_channel,
                                                      uint8_t channel,
@@ -285,6 +353,22 @@ audioio_get_buffer_result_t audioio_mixer_get_buffer(audioio_mixer_obj_t* self,
                     sample_value = voice->remaining_buffer[j];
                 }
 
+                // apply the mixer gain
+            	if (!self->samples_signed) {
+					if (self->bits_per_sample == 8) {
+						sample_value = mult8unsigned(sample_value, voice->gain);
+					} else {
+						sample_value = mult16unsigned(sample_value, voice->gain);
+					}
+				}
+				else{
+					if (self->bits_per_sample == 8) {
+						sample_value = mult8signed(sample_value, voice->gain);
+					} else {
+						sample_value = mult16signed(sample_value, voice->gain);
+					}
+				}
+
                 if (!voices_active) {
                     word_buffer[i] = sample_value;
                 } else {
diff --git a/shared-module/audioio/Mixer.h b/shared-module/audioio/Mixer.h
index 6a88fe0bd5..a48793a8ff 100644
--- a/shared-module/audioio/Mixer.h
+++ b/shared-module/audioio/Mixer.h
@@ -37,6 +37,7 @@ typedef struct {
     bool more_data;
     uint32_t* remaining_buffer;
     uint32_t buffer_length;
+    int16_t gain;
 } audioio_mixer_voice_t;
 
 typedef struct {