From 2a942f0d413b709fb2d247ca7773ae486a38a33b Mon Sep 17 00:00:00 2001 From: kub Date: Tue, 31 Dec 2019 10:55:40 +0100 Subject: [PATCH] add DC filter to sound mixer to remove potential PCM DC offset --- pico/sound/mix.c | 73 ++++++++++++++++++++++++++++++---------- pico/sound/mix.h | 1 + pico/sound/mix_arm.S | 79 +++++++++++++++++++++++++++++++++++--------- pico/sound/sound.c | 1 + 4 files changed, 120 insertions(+), 34 deletions(-) diff --git a/pico/sound/mix.c b/pico/sound/mix.c index 202ba355..242cb375 100644 --- a/pico/sound/mix.c +++ b/pico/sound/mix.c @@ -6,41 +6,72 @@ * See COPYING file in the top-level directory. */ +#include "string.h" + #define MAXOUT (+32767) #define MINOUT (-32768) /* limitter */ -#define Limit(val, max,min) { \ - if ( val > max ) val = max; \ - else if ( val < min ) val = min; \ +#define Limit16(val) { \ + val -= (val >> 2); \ + if ((short)val != val) val = (val < 0 ? MINOUT : MAXOUT); \ } int mix_32_to_16l_level; -void mix_32_to_16l_stereo_core(short *dest, int *src, int count, int level) +static struct iir2 { // 2-pole IIR + int x[2]; // sample buffer + int y[2]; // filter intermediates +} lfi2, rfi2; + +// NB ">>" rounds to -infinity, "/" to 0. To compensate the effect possibly use +// "-(-y>>n)" (round to +infinity) instead of "y>>n" in places. + +// NB uses Q12 fixpoint; samples mustn't have more than 20 bits for this. +#define QB 12 + + +// exponential moving average filter for DC filtering +// y[n] = (x[n]-y[n-1])*(1/8192) (corner approx. 20Hz, gain 1) +static inline int filter_exp(struct iir2 *fi2, int x) { - int l, r; + int xf = (x<y[0]; + fi2->y[0] += xf >> 13; + xf -= xf >> 2; // level reduction to avoid clipping from overshoot + return xf>>QB; +} - for (; count > 0; count--) - { - l = r = *dest; - l += *src++ >> level; - r += *src++ >> level; - Limit( l, MAXOUT, MINOUT ); - Limit( r, MAXOUT, MINOUT ); - *dest++ = l; - *dest++ = r; - } +// unfiltered (for testing) +static inline int filter_null(struct iir2 *fi2, int x) +{ + return x; +} + +#define mix_32_to_16l_stereo_core(dest, src, count, lv, fl) { \ + int l, r; \ + \ + for (; count > 0; count--) \ + { \ + l = r = *dest; \ + l += *src++ >> lv; \ + r += *src++ >> lv; \ + l = fl(&lfi2, l); \ + r = fl(&rfi2, r); \ + Limit16(l); \ + Limit16(r); \ + *dest++ = l; \ + *dest++ = r; \ + } \ } void mix_32_to_16l_stereo_lvl(short *dest, int *src, int count) { - mix_32_to_16l_stereo_core(dest, src, count, mix_32_to_16l_level); + mix_32_to_16l_stereo_core(dest, src, count, mix_32_to_16l_level, filter_exp); } void mix_32_to_16l_stereo(short *dest, int *src, int count) { - mix_32_to_16l_stereo_core(dest, src, count, 0); + mix_32_to_16l_stereo_core(dest, src, count, 0, filter_exp); } void mix_32_to_16_mono(short *dest, int *src, int count) @@ -51,7 +82,8 @@ void mix_32_to_16_mono(short *dest, int *src, int count) { l = *dest; l += *src++; - Limit( l, MAXOUT, MINOUT ); + l = filter_exp(&lfi2, l); + Limit16(l); *dest++ = l; } } @@ -87,3 +119,8 @@ void mix_16h_to_32_s2(int *dest_buf, short *mp3_buf, int count) } } +void mix_reset(void) +{ + memset(&lfi2, 0, sizeof(lfi2)); + memset(&rfi2, 0, sizeof(rfi2)); +} diff --git a/pico/sound/mix.h b/pico/sound/mix.h index b9315114..e128bad1 100644 --- a/pico/sound/mix.h +++ b/pico/sound/mix.h @@ -8,3 +8,4 @@ void mix_32_to_16_mono(short *dest, int *src, int count); extern int mix_32_to_16l_level; void mix_32_to_16l_stereo_lvl(short *dest, int *src, int count); +void mix_reset(void); diff --git a/pico/sound/mix_arm.S b/pico/sound/mix_arm.S index 5088e61b..bb7388d6 100644 --- a/pico/sound/mix_arm.S +++ b/pico/sound/mix_arm.S @@ -166,13 +166,6 @@ m16_32_s2_no_unal2: @ limit and shift up by 16 @ reg=int_sample, lr=1, r3=tmp, kills flags .macro Limitsh reg -@ movs r4, r3, asr #16 -@ cmnne r4, #1 -@ beq c32_16_no_overflow -@ tst r4, r4 -@ mov r3, #0x8000 -@ subpl r3, r3, #1 - add r3, lr, \reg, asr #15 bics r3, r3, #1 @ in non-overflow conditions r3 is 0 or 1 moveq \reg, \reg, lsl #16 @@ -180,20 +173,30 @@ m16_32_s2_no_unal2: subpl \reg, \reg, #0x00010000 .endm +@ filter out DC offset +@ in=int_sample (max 20 bit), y=filter memory, r3=tmp +.macro DCfilt in y + rsb r3, \y, \in, asl #12 @ fixpoint 20.12 + add \y, \y, r3, asr #13 + sub \in, \in, \y, asr #12 + sub \in, \in, \in, asr #2 @ reduce audio lvl some +.endm @ mix 32bit audio (with 16bits really used, upper bits indicate overflow) with normal 16 bit audio with left channel only @ warning: this function assumes dest is word aligned .global mix_32_to_16l_stereo @ short *dest, int *src, int count mix_32_to_16l_stereo: - stmfd sp!, {r4-r8,lr} - - mov lr, #1 + stmfd sp!, {r4-r8,r10-r11,lr} mov r2, r2, lsl #1 subs r2, r2, #4 bmi m32_16l_st_end + mov lr, #1 + ldr r12, =filter + ldmia r12, {r10-r11} + m32_16l_st_loop: ldmia r0, {r8,r12} ldmia r1!, {r4-r7} @@ -203,6 +206,10 @@ m32_16l_st_loop: add r5, r5, r8, asr #16 add r6, r6, r12,asr #16 add r7, r7, r12,asr #16 + DCfilt r4, r10 + DCfilt r5, r11 + DCfilt r6, r10 + DCfilt r7, r11 Limitsh r4 Limitsh r5 Limitsh r6 @@ -221,13 +228,17 @@ m32_16l_st_end: ldmia r1!,{r4,r5} add r4, r4, r6 add r5, r5, r6 + DCfilt r4, r10 + DCfilt r5, r11 Limitsh r4 Limitsh r5 orr r4, r5, r4, lsr #16 str r4, [r0], #4 m32_16l_st_no_unal2: - ldmfd sp!, {r4-r8,lr} + ldr r12, =filter + stmia r12, {r10-r11} + ldmfd sp!, {r4-r8,r10-r11,lr} bx lr @@ -235,9 +246,11 @@ m32_16l_st_no_unal2: .global mix_32_to_16_mono @ short *dest, int *src, int count mix_32_to_16_mono: - stmfd sp!, {r4-r8,lr} + stmfd sp!, {r4-r8,r10-r11,lr} mov lr, #1 + ldr r12, =filter + ldr r10, [r12] @ check if dest is word aligned tst r0, #2 @@ -262,6 +275,10 @@ m32_16_mo_loop: add r7, r7, r12,asr #16 mov r12,r12,lsl #16 add r6, r6, r12,asr #16 + DCfilt r4, r10 + DCfilt r5, r10 + DCfilt r6, r10 + DCfilt r7, r10 Limitsh r4 Limitsh r5 Limitsh r6 @@ -281,6 +298,8 @@ m32_16_mo_end: add r5, r5, r6, asr #16 mov r6, r6, lsl #16 add r4, r4, r6, asr #16 + DCfilt r4, r10 + DCfilt r5, r10 Limitsh r4 Limitsh r5 orr r4, r5, r4, lsr #16 @@ -288,14 +307,18 @@ m32_16_mo_end: m32_16_mo_no_unal2: tst r2, #1 - ldmeqfd sp!, {r4-r8,pc} + beq m32_16_mo_no_unal ldrsh r5, [r0] ldr r4, [r1], #4 add r4, r4, r5 + DCfilt r4, r10 Limit r4 strh r4, [r0], #2 - ldmfd sp!, {r4-r8,lr} +m32_16_mo_no_unal: + ldr r12, =filter + str r10, [r12] + ldmfd sp!, {r4-r8,r10-r11,lr} bx lr @@ -315,11 +338,13 @@ mix_32_to_16l_level: .global mix_32_to_16l_stereo_lvl @ short *dest, int *src, int count mix_32_to_16l_stereo_lvl: - stmfd sp!, {r4-r9,lr} + stmfd sp!, {r4-r11,lr} ldr r9, =mix_32_to_16l_level mov lr, #1 ldr r9, [r9] + ldr r12, =filter + ldm r12, {r10-r11} mov r2, r2, lsl #1 subs r2, r2, #4 @@ -338,6 +363,10 @@ m32_16l_st_l_loop: mov r5, r5, asr r9 mov r6, r6, asr r9 mov r7, r7, asr r9 + DCfilt r4, r10 + DCfilt r5, r11 + DCfilt r6, r10 + DCfilt r7, r11 Limitsh r4 Limitsh r5 Limitsh r6 @@ -358,15 +387,33 @@ m32_16l_st_l_end: add r5, r5, r6 mov r4, r4, asr r9 mov r5, r5, asr r9 + DCfilt r4, r10 + DCfilt r5, r11 Limitsh r4 Limitsh r5 orr r4, r5, r4, lsr #16 str r4, [r0], #4 m32_16l_st_l_no_unal2: - ldmfd sp!, {r4-r9,lr} + ldr r12, =filter + stmia r12, {r10-r11} + ldmfd sp!, {r4-r11,lr} + bx lr + +.global mix_reset @ void +mix_reset: + ldr r0, =filter + mov r1, #0 + str r1, [r0] + str r1, [r0, #4] bx lr +.data + DCfilt r4, r10 + DCfilt r5, r11 +filter: + .ds 8 + #endif /* __GP2X__ */ @ vim:filetype=armasm diff --git a/pico/sound/sound.c b/pico/sound/sound.c index 95aac128..30d4a072 100644 --- a/pico/sound/sound.c +++ b/pico/sound/sound.c @@ -86,6 +86,7 @@ PICO_INTERNAL void PsndReset(void) // PsndRerate calls YM2612Init, which also resets PsndRerate(0); timers_reset(); + mix_reset(); } -- 2.39.2