diff --git a/shared-module/audiomixer/Mixer.c b/shared-module/audiomixer/Mixer.c index 212686a092ee9..b6cb3318e54c1 100644 --- a/shared-module/audiomixer/Mixer.c +++ b/shared-module/audiomixer/Mixer.c @@ -92,15 +92,15 @@ static inline uint32_t add16signed(uint32_t a, uint32_t b) { } __attribute__((always_inline)) -static inline uint32_t mult16signed(uint32_t val, int32_t mul[2]) { +static inline uint32_t mult16signed(uint32_t val, int32_t lomul, int32_t himul) { #if (defined(__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) - mul[0] <<= 16; - mul[1] <<= 16; + lomul <<= 16; + himul <<= 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[0]), "r" (val)); - __asm__ volatile ("smulwt %0, %1, %2" : "=r" (hi) : "r" (mul[1]), "r" (val)); + __asm__ volatile ("smulwb %0, %1, %2" : "=r" (lo) : "r" (lomul), "r" (val)); + __asm__ volatile ("smulwt %0, %1, %2" : "=r" (hi) : "r" (himul), "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 @@ -108,7 +108,7 @@ static inline uint32_t mult16signed(uint32_t val, int32_t mul[2]) { #else uint32_t result = 0; for (int8_t i = 0; i < 2; i++) { - float mod_mul = (float)mul[i] / (float)((1 << 15) - 1); + float mod_mul = (float)(i ? himul : lomul) / (float)((1 << 15) - 1); int16_t ai = (val >> (sizeof(uint16_t) * 8 * i)); int32_t intermediate = (int32_t)(ai * mod_mul); if (intermediate > SHRT_MAX) { @@ -156,23 +156,21 @@ static inline uint32_t pack8(uint32_t val) { } static inline uint32_t copy16lsb(uint32_t val) { + #if (defined(__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) + return __PKHBT(val, val, 16); + #else val &= 0x0000ffff; return val | (val << 16); + #endif } static inline uint32_t copy16msb(uint32_t val) { + #if (defined(__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1)) + return __PKHTB(val, val, 16); + #else val &= 0xffff0000; return val | (val >> 16); -} - -static inline uint32_t copy8lsb(uint32_t val) { - val &= 0x00ff; - return val | (val << 8); -} - -static inline uint32_t copy8msb(uint32_t val) { - val &= 0xff00; - return val | (val >> 8); + #endif } #define ALMOST_ONE (MICROPY_FLOAT_CONST(32767.) / 32768) @@ -228,16 +226,17 @@ static void mix_down_one_voice(audiomixer_mixer_obj_t *self, uint16_t left_panning_scaled = 32768, right_panning_scaled = 32768; if (MP_LIKELY(self->base.channel_count == 2)) { if (panning >= 0) { - right_panning_scaled = 32767 - panning; + left_panning_scaled = 32767 - panning; } else { - left_panning_scaled = 32767 + panning; + right_panning_scaled = 32767 + panning; } } - int32_t loudness[2] = { level, level }; + int32_t lo_level = level; + int32_t hi_level = level; if (MP_LIKELY(self->base.channel_count == 2)) { - loudness[0] = (left_panning_scaled * loudness[0]) >> 15; - loudness[1] = (right_panning_scaled * loudness[1]) >> 15; + lo_level = (left_panning_scaled * lo_level) >> 15; + hi_level = (right_panning_scaled * hi_level) >> 15; } // First active voice gets copied over verbatim. @@ -247,13 +246,13 @@ static void mix_down_one_voice(audiomixer_mixer_obj_t *self, if (MP_LIKELY(self->base.channel_count == sample->channel_count)) { for (uint32_t i = 0; i < n; i++) { uint32_t v = src[i]; - word_buffer[i] = mult16signed(v, loudness); + word_buffer[i] = mult16signed(v, lo_level, hi_level); } } else { for (uint32_t i = 0; i < n; i += 2) { uint32_t v = src[i >> 1]; - word_buffer[i] = mult16signed(copy16lsb(v), loudness); - word_buffer[i + 1] = mult16signed(copy16msb(v), loudness); + word_buffer[i] = mult16signed(copy16lsb(v), lo_level, hi_level); + word_buffer[i + 1] = mult16signed(copy16msb(v), lo_level, hi_level); } } } else { @@ -261,14 +260,14 @@ static void mix_down_one_voice(audiomixer_mixer_obj_t *self, for (uint32_t i = 0; i < n; i++) { uint32_t v = src[i]; v = tosigned16(v); - word_buffer[i] = mult16signed(v, loudness); + word_buffer[i] = mult16signed(v, lo_level, hi_level); } } else { for (uint32_t i = 0; i + 1 < n; i += 2) { uint32_t v = src[i >> 1]; v = tosigned16(v); - word_buffer[i] = mult16signed(copy16lsb(v), loudness); - word_buffer[i + 1] = mult16signed(copy16msb(v), loudness); + word_buffer[i] = mult16signed(copy16lsb(v), lo_level, hi_level); + word_buffer[i + 1] = mult16signed(copy16msb(v), lo_level, hi_level); } } } @@ -281,7 +280,7 @@ static void mix_down_one_voice(audiomixer_mixer_obj_t *self, if (MP_LIKELY(!self->base.samples_signed)) { word = tosigned16(word); } - word = mult16signed(word, loudness); + word = mult16signed(word, lo_level, hi_level); hword_buffer[i] = pack8(word); } } else { @@ -290,8 +289,8 @@ static void mix_down_one_voice(audiomixer_mixer_obj_t *self, if (MP_LIKELY(!self->base.samples_signed)) { word = tosigned16(word); } - hword_buffer[i] = pack8(mult16signed(copy16lsb(word), loudness)); - hword_buffer[i + 1] = pack8(mult16signed(copy16msb(word), loudness)); + hword_buffer[i] = pack8(mult16signed(copy16lsb(word), lo_level, hi_level)); + hword_buffer[i + 1] = pack8(mult16signed(copy16msb(word), lo_level, hi_level)); } } } @@ -301,13 +300,13 @@ static void mix_down_one_voice(audiomixer_mixer_obj_t *self, if (MP_LIKELY(self->base.channel_count == sample->channel_count)) { for (uint32_t i = 0; i < n; i++) { uint32_t word = src[i]; - word_buffer[i] = add16signed(mult16signed(word, loudness), word_buffer[i]); + word_buffer[i] = add16signed(mult16signed(word, lo_level, hi_level), word_buffer[i]); } } else { for (uint32_t i = 0; i + 1 < n; i += 2) { uint32_t word = src[i >> 1]; - word_buffer[i] = add16signed(mult16signed(copy16lsb(word), loudness), word_buffer[i]); - word_buffer[i + 1] = add16signed(mult16signed(copy16msb(word), loudness), word_buffer[i + 1]); + word_buffer[i] = add16signed(mult16signed(copy16lsb(word), lo_level, hi_level), word_buffer[i]); + word_buffer[i + 1] = add16signed(mult16signed(copy16msb(word), lo_level, hi_level), word_buffer[i + 1]); } } } else { @@ -315,14 +314,14 @@ static void mix_down_one_voice(audiomixer_mixer_obj_t *self, for (uint32_t i = 0; i < n; i++) { uint32_t word = src[i]; word = tosigned16(word); - word_buffer[i] = add16signed(mult16signed(word, loudness), word_buffer[i]); + word_buffer[i] = add16signed(mult16signed(word, lo_level, hi_level), word_buffer[i]); } } else { for (uint32_t i = 0; i + 1 < n; i += 2) { uint32_t word = src[i >> 1]; word = tosigned16(word); - word_buffer[i] = add16signed(mult16signed(copy16lsb(word), loudness), word_buffer[i]); - word_buffer[i + 1] = add16signed(mult16signed(copy16msb(word), loudness), word_buffer[i + 1]); + word_buffer[i] = add16signed(mult16signed(copy16lsb(word), lo_level, hi_level), word_buffer[i]); + word_buffer[i + 1] = add16signed(mult16signed(copy16msb(word), lo_level, hi_level), word_buffer[i + 1]); } } } @@ -335,7 +334,7 @@ static void mix_down_one_voice(audiomixer_mixer_obj_t *self, if (MP_LIKELY(!self->base.samples_signed)) { word = tosigned16(word); } - word = mult16signed(word, loudness); + word = mult16signed(word, lo_level, hi_level); word = add16signed(word, unpack8(hword_buffer[i])); hword_buffer[i] = pack8(word); } @@ -345,8 +344,8 @@ static void mix_down_one_voice(audiomixer_mixer_obj_t *self, if (MP_LIKELY(!self->base.samples_signed)) { word = tosigned16(word); } - hword_buffer[i] = pack8(add16signed(mult16signed(copy16lsb(word), loudness), unpack8(hword_buffer[i]))); - hword_buffer[i + 1] = pack8(add16signed(mult16signed(copy16msb(word), loudness), unpack8(hword_buffer[i + 1]))); + hword_buffer[i] = pack8(add16signed(mult16signed(copy16lsb(word), lo_level, hi_level), unpack8(hword_buffer[i]))); + hword_buffer[i + 1] = pack8(add16signed(mult16signed(copy16msb(word), lo_level, hi_level), unpack8(hword_buffer[i + 1]))); } } } diff --git a/shared-module/audiomixer/MixerVoice.c b/shared-module/audiomixer/MixerVoice.c index 6c23a39215cbc..3a598516e7ca8 100644 --- a/shared-module/audiomixer/MixerVoice.c +++ b/shared-module/audiomixer/MixerVoice.c @@ -51,7 +51,7 @@ void common_hal_audiomixer_mixervoice_set_panning(audiomixer_mixervoice_obj_t *s #if CIRCUITPY_SYNTHIO synthio_block_assign_slot(arg, &self->panning, MP_QSTR_panning); #else - self->panning = (uint16_t)(mp_arg_validate_obj_float_range(arg, -1, 1, MP_QSTR_panning) * ((1 << 15) - 1)); + self->panning = (int16_t)(mp_arg_validate_obj_float_range(arg, -1, 1, MP_QSTR_panning) * ((1 << 15) - 1)); #endif }