Mixer: Rework for performance, particularly of the S16 case

This removes downscaling (halving-add) when multiple voices are
being mixed.  To avoid clipping, and get similar behavior to before,
set the "level" of each voice to (1/voice_count).

Slow paths that were applicable to only M0 chips were removed.

As a side effect, the internal volume representation is now 0 ..
0x8000 (inclusive), which additionally makes a level of exactly 0.5
representable.

Testing performed, on PyGamer: For all 4 data cases, for stereo and
mono, for 1 and 2 voices, play pure sign waves represented as
RawSamples and view the result on a scope and through headphones.
Also, scope the amount of time spent in background tasks.

Code size: growth of +272 bytes

Performance (time in background task when mixing 2 stereo 16-bit voices):
76us per down from 135us (once per ~2.9ms long term average)
(Decrease from 4.7% to 2.4% of all CPU time)

3 files changed, 109 insertions, 231 deletions

diff --git a/shared-module/audiomixer/Mixer.c b/shared-module/audiomixer/Mixer.c
index f3a435a09..3c1c9a491 100644
--- a/shared-module/audiomixer/Mixer.c
+++ b/shared-module/audiomixer/Mixer.c
@@ -101,213 +101,60 @@ void audiomixer_mixer_reset_buffer(audiomixer_mixer_obj_t* self,
     }
 }
 
-uint32_t add8signed(uint32_t a, uint32_t b) {
-    #if (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) //Cortex-M4 w/FPU
-    return __SHADD8(a, b);
-    #else
-    uint32_t result = 0;
-    for (int8_t i = 0; i < 4; i++) {
-        int8_t ai = a >> (sizeof(int8_t) * 8 * i);
-        int8_t bi = b >> (sizeof(int8_t) * 8 * i);
-        int32_t intermediate = (int32_t) ai + bi / 2;
-        if (intermediate > CHAR_MAX) {
-            intermediate = CHAR_MAX;
-        } else if (intermediate < CHAR_MIN) {
-            intermediate = CHAR_MIN;
-        }
-        result |= ((uint32_t) intermediate & 0xff) << (sizeof(int8_t) * 8 * i);
-    }
-    return result;
-    #endif
+__attribute__((always_inline))
+static inline uint32_t add16signed(uint32_t a, uint32_t b) {
+    return __QADD16(a, b);
 }
 
-uint32_t add8unsigned(uint32_t a, uint32_t b) {
-    #if (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) //Cortex-M4 w/FPU
-    return __UHADD8(a, b);
-    #else
-    uint32_t result = 0;
-    for (int8_t i = 0; i < 4; i++) {
-        uint8_t ai = (a >> (sizeof(uint8_t) * 8 * i));
-        uint8_t bi = (b >> (sizeof(uint8_t) * 8 * i));
-        int32_t intermediate = (int32_t) (ai + bi) / 2;
-        if (intermediate > UCHAR_MAX) {
-            intermediate = UCHAR_MAX;
-        }
-        result |= ((uint32_t) intermediate & 0xff) << (sizeof(uint8_t) * 8 * i);
-    }
-    return result;
-    #endif
+__attribute__((always_inline))
+static inline uint32_t mult16signed(uint32_t val, int32_t mul) {
+    mul <<= 16;
+    int32_t hi, lo;
+    enum { bits = 16 }; // saturate to 16 bits
+    enum { shift = 15 }; // 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;
 }
 
-uint32_t add16signed(uint32_t a, uint32_t b) {
-    #if (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) //Cortex-M4 w/FPU
-    return __SHADD16(a, b);
-    #else
-    uint32_t result = 0;
-    for (int8_t i = 0; i < 2; i++) {
-        int16_t ai = a >> (sizeof(int16_t) * 8 * i);
-        int16_t bi = b >> (sizeof(int16_t) * 8 * i);
-        int32_t intermediate = (int32_t) ai + bi / 2;
-        if (intermediate > SHRT_MAX) {
-            intermediate = SHRT_MAX;
-        } else if (intermediate < SHRT_MIN) {
-            intermediate = SHRT_MIN;
-        }
-        result |= (((uint32_t) intermediate) & 0xffff) << (sizeof(int16_t) * 8 * i);
-    }
-    return result;
-    #endif
+static inline uint32_t tounsigned8(uint32_t val) {
+    return __UADD8(val, 0x80808080);
 }
 
-uint32_t add16unsigned(uint32_t a, uint32_t b) {
-    #if (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) //Cortex-M4 w/FPU
-    return __UHADD16(a, b);
-    #else
-    uint32_t result = 0;
-    for (int8_t i = 0; i < 2; i++) {
-        int16_t ai = (a >> (sizeof(uint16_t) * 8 * i)) - 0x8000;
-        int16_t bi = (b >> (sizeof(uint16_t) * 8 * i)) - 0x8000;
-        int32_t intermediate = (int32_t) ai + bi / 2;
-        if (intermediate > USHRT_MAX) {
-            intermediate = USHRT_MAX;
-        }
-        result |= ((uint32_t) intermediate & 0xffff) << (sizeof(int16_t) * 8 * i);
-    }
-    return result;
-    #endif
+static inline uint32_t tounsigned16(uint32_t val) {
+    return __UADD16(val, 0x80008000);
 }
 
-static inline uint32_t mult8unsigned(uint32_t val, int32_t mul) {
-    // if mul == 0, no need in wasting cycles
-    if (mul == 0) {
-        return 0;
-    }
-    /* TODO: workout ARMv7 instructions
-    #if (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) //Cortex-M4 w/FPU
-    return val;
-    #else*/
-    uint32_t result = 0;
-    float mod_mul = (float) mul / (float) ((1<<15)-1);
-    for (int8_t i = 0; i < 4; i++) {
-        uint8_t ai = val >> (sizeof(uint8_t) * 8 * i);
-        int32_t intermediate = ai * mod_mul;
-        if (intermediate > SHRT_MAX) {
-            intermediate = SHRT_MAX;
-        }
-        result |= ((uint32_t) intermediate & 0xff) << (sizeof(uint8_t) * 8 * i);
-    }
-
-    return result;
-    //#endif
-}
-
-static inline uint32_t mult8signed(uint32_t val, int32_t mul) {
-    // if mul == 0, no need in wasting cycles
-    if (mul == 0) {
-        return 0;
-    }
-    /* TODO: workout ARMv7 instructions
-    #if (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) //Cortex-M4 w/FPU
-    return val;
-    #else
-    */
-    uint32_t result = 0;
-    float mod_mul = (float)mul / (float)((1<<15)-1);
-    for (int8_t i = 0; i < 4; i++) {
-        int16_t ai = val >> (sizeof(int8_t) * 8 * i);
-        int32_t intermediate = ai * mod_mul;
-        if (intermediate > CHAR_MAX) {
-            intermediate = CHAR_MAX;
-        } else if (intermediate < CHAR_MIN) {
-            intermediate = CHAR_MIN;
-        }
-        result |= (((uint32_t) intermediate) & 0xff) << (sizeof(int16_t) * 8 * i);
-    }
-    return result;
-    //#endif
+static inline uint32_t tosigned16(uint32_t val) {
+    return __UADD16(val, 0x80008000);
 }
 
-//TODO:
-static inline uint32_t mult16unsigned(uint32_t val, int32_t mul) {
-    // if mul == 0, no need in wasting cycles
-    if (mul == 0) {
-        return 0;
-    }
-    /* TODO: the below ARMv7m instructions "work", but the amplitude is much higher/louder
-    #if (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) //Cortex-M4 w/FPU
-    // there is no unsigned equivalent to the 'SMULWx' ARMv7 Thumb function,
-    // so we have to do it by hand.
-    uint32_t lo = val & 0xffff;
-    uint32_t hi = val >> 16;
-    //mp_printf(&mp_plat_print, "pre-asm: (mul: %d)\n\tval: %x\tlo: %x\thi: %x\n", mul, val, lo, hi);
-    uint32_t val_lo;
-    asm volatile("mul %0, %1, %2" : "=r" (val_lo) : "r" (mul), "r" (lo));
-    asm volatile("mla %0, %1, %2, %3" : "=r" (val) : "r" (mul), "r" (hi), "r" (val_lo));
-    //mp_printf(&mp_plat_print, "post-asm:\n\tval: %x\tlo: %x\n\n", val, val_lo);
-    return val;
-    #else
-    */
-    uint32_t result = 0;
-    float mod_mul = (float)mul / (float)((1<<15)-1);
-    for (int8_t i = 0; i < 2; i++) {
-        int16_t ai = (val >> (sizeof(uint16_t) * 8 * i)) - 0x8000;
-        int32_t intermediate = ai * mod_mul;
-        if (intermediate > SHRT_MAX) {
-            intermediate = SHRT_MAX;
-        } else if (intermediate < SHRT_MIN) {
-            intermediate = SHRT_MIN;
-        }
-        result |= (((uint32_t) intermediate) + 0x8000) << (sizeof(int16_t) * 8 * i);
-    }
-    return result;
-    //#endif
+static inline uint32_t unpack8(uint16_t val) {
+    return ((val & 0xff00) << 16) | ((val & 0x00ff) << 8);
 }
 
-static inline uint32_t mult16signed(uint32_t val, int32_t mul) {
-    // if mul == 0, no need in wasting cycles
-    if (mul == 0) {
-        return 0;
-    }
-    #if (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) //Cortex-M4 w/FPU
-    int32_t hi, lo;
-    enum { bits = 16 }; // saturate to 16 bits
-    enum { 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;
-    float mod_mul = (float)mul / (float)((1<<15)-1);
-    for (int8_t i = 0; i < 2; i++) {
-        int16_t ai = val >> (sizeof(int16_t) * 8 * i);
-        int32_t intermediate = ai * mod_mul;
-        if (intermediate > SHRT_MAX) {
-            intermediate = SHRT_MAX;
-        } else if (intermediate < SHRT_MIN) {
-            intermediate = SHRT_MIN;
-        }
-        result |= (((uint32_t) intermediate) & 0xffff) << (sizeof(int16_t) * 8 * i);
-    }
-    return result;
-    #endif
+static inline uint32_t pack8(uint32_t val) {
+    return ((val & 0xff000000) >> 16) | ((val & 0xff00) >> 8);
 }
 
+#define LIKELY(x) (__builtin_expect(!!(x), 1))
+#define UNLIKELY(x) (__builtin_expect(!!(x), 0))
 static void mix_one_voice(audiomixer_mixer_obj_t* self,
         audiomixer_mixervoice_obj_t* voice, bool voices_active,
         uint32_t* word_buffer, uint32_t length) {
-    uint32_t j = 0;
     bool voice_done = voice->sample == NULL;
-    for (uint32_t i = 0; i < length; i++) {
-        if (!voice_done && j >= voice->buffer_length) {
+    while (!voice_done && length != 0) {
+        if (voice->buffer_length == 0) {
             if (!voice->more_data) {
                 if (voice->loop) {
                     audiosample_reset_buffer(voice->sample, false, 0);
                 } else {
                     voice->sample = NULL;
                     voice_done = true;
+                    break;
                 }
             }
             if (!voice_done) {
@@ -316,64 +163,81 @@ static void mix_one_voice(audiomixer_mixer_obj_t* self,
                 // Track length in terms of words.
                 voice->buffer_length /= sizeof(uint32_t);
                 voice->more_data = result == GET_BUFFER_MORE_DATA;
-                j = 0;
             }
         }
+
+        uint32_t n = MIN(voice->buffer_length, length);
+        uint32_t *src = voice->remaining_buffer;
+        uint16_t level = voice->level;
+
         // First active voice gets copied over verbatim.
-        uint32_t sample_value;
-        if (voice_done) {
-            // Exit early if another voice already set all samples once.
-            if (voices_active) {
-                continue;
-            }
-            sample_value = 0;
-            if (!self->samples_signed) {
-                if (self->bits_per_sample == 8) {
-                    sample_value = 0x7f7f7f7f;
+        if (!voices_active) {
+            if (LIKELY(self->bits_per_sample == 16)) {
+                if (LIKELY(self->samples_signed)) {
+                    for (uint32_t i = 0; i<n; i++) {
+                        uint32_t v = src[i];
+                        word_buffer[i] = mult16signed(v, level);
+                    }
                 } else {
-                    sample_value = 0x7fff7fff;
+                    for (uint32_t i = 0; i<n; i++) {
+                        uint32_t v = src[i];
+                        v = tosigned16(v);
+                        word_buffer[i] = mult16signed(v, level);
+                    }
                 }
-            }
-        } else {
-            sample_value = voice->remaining_buffer[j];
-        }
-
-        // apply the mixer level
-        if (!self->samples_signed) {
-            if (self->bits_per_sample == 8) {
-                sample_value = mult8unsigned(sample_value, voice->level);
-            } else {
-                sample_value = mult16unsigned(sample_value, voice->level);
-            }
-        } else {
-            if (self->bits_per_sample == 8) {
-                sample_value = mult8signed(sample_value, voice->level);
             } else {
-                sample_value = mult16signed(sample_value, voice->level);
+                uint16_t *hword_buffer = (uint16_t*)word_buffer;
+                uint16_t *hsrc = (uint16_t*)src;
+                for (uint32_t i = 0; i<n*2; i++) {
+                    uint32_t word = unpack8(hsrc[i]);
+                    if (LIKELY(!self->samples_signed)) {
+                        word = tosigned16(word);
+                    }
+                    word = mult16signed(word, level);
+                    hword_buffer[i] = pack8(word);
+                }
             }
-        }
-
-        if (!voices_active) {
-            word_buffer[i] = sample_value;
         } else {
-            if (self->bits_per_sample == 8) {
-                if (self->samples_signed) {
-                    word_buffer[i] = add8signed(word_buffer[i], sample_value);
+            if (LIKELY(self->bits_per_sample == 16)) {
+                if (LIKELY(self->samples_signed)) {
+                    for (uint32_t i = 0; i<n; i++) {
+                        uint32_t word = src[i];
+                        word_buffer[i] = add16signed(mult16signed(word, level), word_buffer[i]);
+                    }
                 } else {
-                    word_buffer[i] = add8unsigned(word_buffer[i], sample_value);
+                    for (uint32_t i = 0; i<n; i++) {
+                        uint32_t word = src[i];
+                        word = tosigned16(word);
+                        word_buffer[i] = add16signed(mult16signed(word, level), word_buffer[i]);
+                        }
                 }
             } else {
-                if (self->samples_signed) {
-                    word_buffer[i] = add16signed(word_buffer[i], sample_value);
-                } else {
-                    word_buffer[i] = add16unsigned(word_buffer[i], sample_value);
+                uint16_t *hword_buffer = (uint16_t*)word_buffer;
+                uint16_t *hsrc = (uint16_t*)src;
+                for (uint32_t i = 0; i<n*2; i++) {
+                    uint32_t word = unpack8(hsrc[i]);
+                    if (LIKELY(!self->samples_signed)) {
+                        word = tosigned16(word);
+                    }
+                    word = mult16signed(word, level);
+                    word = add16signed(word, unpack8(hword_buffer[i]));
+                    hword_buffer[i] = pack8(word);
                 }
             }
         }
-        j++;
+        length -= n;
+        word_buffer += n;
+        voice->remaining_buffer += n;
+        voice->buffer_length -= n;
+    }
+
+    if (length && !voices_active) {
+        uint32_t sample_value = self->bits_per_sample == 8
+            ?  0x80808080 : 0x80008000;
+        for (uint32_t i = 0; i<length; i++) {
+            word_buffer[i] = sample_value;
+        }
     }
-    voice->buffer_length -= j;
-    voice->remaining_buffer += j;
 }
 
 audioio_get_buffer_result_t audiomixer_mixer_get_buffer(audiomixer_mixer_obj_t* self,
@@ -403,13 +267,27 @@ audioio_get_buffer_result_t audiomixer_mixer_get_buffer(audiomixer_mixer_obj_t*
         }
         self->use_first_buffer = !self->use_first_buffer;
         bool voices_active = false;
+        uint32_t length = self->len / sizeof(uint32_t);
+
         for (int32_t v = 0; v < self->voice_count; v++) {
             audiomixer_mixervoice_obj_t* voice = MP_OBJ_TO_PTR(self->voice[v]);
 
-            mix_one_voice(self, voice, voices_active, word_buffer, self->len / sizeof(uint32_t));
+            mix_one_voice(self, voice, voices_active, word_buffer, length);
             voices_active = true;
         }
 
+        if (!self->samples_signed) {
+            if (self->bits_per_sample == 16) {
+                for (uint32_t i = 0; i < length; i++) {
+                    word_buffer[i] = tounsigned16(word_buffer[i]);
+                }
+            } else {
+                for (uint32_t i = 0; i < length; i++) {
+                    word_buffer[i] = tounsigned8(word_buffer[i]);
+                }
+            }
+        }
+
         self->read_count += 1;
     } else if (!self->use_first_buffer) {
         *buffer = (uint8_t*) self->first_buffer;
diff --git a/shared-module/audiomixer/MixerVoice.c b/shared-module/audiomixer/MixerVoice.c
index ff05dc93e..9be104afc 100644
--- a/shared-module/audiomixer/MixerVoice.c
+++ b/shared-module/audiomixer/MixerVoice.c
@@ -34,7 +34,7 @@
 
 void common_hal_audiomixer_mixervoice_construct(audiomixer_mixervoice_obj_t *self) {
     self->sample = NULL;
-    self->level = ((1 << 15) - 1);
+    self->level = 1 << 15;
 }
 
 void common_hal_audiomixer_mixervoice_set_parent(audiomixer_mixervoice_obj_t* self, audiomixer_mixer_obj_t *parent) {
@@ -42,11 +42,11 @@ void common_hal_audiomixer_mixervoice_set_parent(audiomixer_mixervoice_obj_t* se
 }
 
 float common_hal_audiomixer_mixervoice_get_level(audiomixer_mixervoice_obj_t* self) {
-	return ((float) self->level / ((1 << 15) - 1));
+	return ((float) self->level / (1 << 15));
 }
 
 void common_hal_audiomixer_mixervoice_set_level(audiomixer_mixervoice_obj_t* self, float level) {
-	self->level = level * ((1 << 15)-1);
+	self->level = level * (1 << 15);
 }
 
 void common_hal_audiomixer_mixervoice_play(audiomixer_mixervoice_obj_t* self, mp_obj_t sample, bool loop) {
diff --git a/shared-module/audiomixer/MixerVoice.h b/shared-module/audiomixer/MixerVoice.h
index efac19156..a85316e3d 100644
--- a/shared-module/audiomixer/MixerVoice.h
+++ b/shared-module/audiomixer/MixerVoice.h
@@ -39,7 +39,7 @@ typedef struct {
     bool more_data;
     uint32_t* remaining_buffer;
     uint32_t buffer_length;
-    int16_t level;
+    uint16_t level;
 } audiomixer_mixervoice_obj_t;