207 lines
5.5 KiB
ArmAsm
207 lines
5.5 KiB
ArmAsm
# mach: bfin
|
|
|
|
// GENERIC BIQUAD:
|
|
// ---------------
|
|
// x ---------+---------|---------+-------y
|
|
// | |t1 |
|
|
// | D |
|
|
// | a1 | b1 |
|
|
// +---<-----|---->----+
|
|
// | | |
|
|
// | D | D's are delays
|
|
// | a2 | b2 | ">" represent multiplications
|
|
// +---<-----|---->----+
|
|
// To test this routine, use a biquad with a pole pair at z = (0.7 +- 0.1j),
|
|
// and a double zero at z = -1.0, which is a low-pass. The transfer function is:
|
|
// 1 + 2z^-1 + z^-2
|
|
// H(z) = ----------------------
|
|
// 1 - 1.4z^-1 + 0.5z^-2
|
|
// a1 = 1.4
|
|
// a2 = -0.5
|
|
// b1 = 2
|
|
// b2 = 1
|
|
// This filter conforms to the biquad test in BDT, since it has coefficients
|
|
// larger than 1.0 in magnitude, and b0=1. (Note that the a's have a negative
|
|
// sign.)
|
|
// This filter can be simulated in matlab. To simulate one biquad, use
|
|
// A = [1.0, -1.4, 0.5]
|
|
// B = [1, 2, 1]
|
|
// Y=filter(B,A,X)
|
|
// To simulate two cascaded biquads, use
|
|
// Y=filter(B,A,filter(B,A,X))
|
|
// SCALED COEFFICIENTS:
|
|
// --------------------
|
|
// In order to conform to 1.15 representation, must scale coeffs by 0.5.
|
|
// This requires an additional internal re-scale. The equations for the Type II
|
|
// filter are:
|
|
// t1 = x + a1*t1*z^-1 + a2*t1*z^-2
|
|
// y = b0*t1 + b1*t1*z^-1 + b2*t1*z^-2
|
|
// (Note inclusion of term b0, which in the example is b0 = 1.)
|
|
// If all coeffs are replaced by
|
|
// ai --> ai' = 0.5*a1
|
|
// then the two equations become
|
|
// t1 = x + 2*a1'*t1*z^-1 + 2*a2'*t1*z^-2
|
|
// 0.5*y = b0'*t1 + b1'*t1*z^-1 + b2'*t1*z^-2
|
|
// which can be implemented as:
|
|
// 2.0 b0'=0.5
|
|
// x ---------+--->-----|---->----+-------y
|
|
// | |t1 |
|
|
// | D |
|
|
// | a1' | b1' |
|
|
// +---<-----|---->----+
|
|
// | | |
|
|
// | D |
|
|
// | a2' | b2' |
|
|
// +---<-----|---->----+
|
|
// But, b0' can be eliminated by:
|
|
// x ---------+---------|---------+-------y
|
|
// | | |
|
|
// | V 2.0 |
|
|
// | | |
|
|
// | |t1 |
|
|
// | D |
|
|
// | a1' | b1' |
|
|
// +---<-----|---->----+
|
|
// | | |
|
|
// | D |
|
|
// | a2' | b2' |
|
|
// +---<-----|---->----+
|
|
// Function biquadf() computes this implementation on float data.
|
|
// CASCADED BIQUADS
|
|
// ----------------
|
|
// Cascaded biquads are simulated by simply cascading copies of the
|
|
// filter defined above. However, one must be careful with the resulting
|
|
// filter, as it is not very stable numerically (double poles in the
|
|
// vecinity of +1). It would of course be better to cascade different
|
|
// filters, as that would result in more stable structures.
|
|
// The functions biquadf() and biquadR() have been tested with up to 3
|
|
// stages using this technique, with inputs having small signal amplitude
|
|
// (less than 0.001) and under 300 samples.
|
|
//
|
|
// In order to pipeline, need to maintain two pointers into the state
|
|
// array: one to load (I0) and one to store (I2). This is required since
|
|
// the load of iteration i+1 is hoisted above the store of iteration i.
|
|
|
|
.include "testutils.inc"
|
|
start
|
|
|
|
|
|
// I3 points to input buffer
|
|
loadsym I3, input;
|
|
|
|
// P1 points to output buffer
|
|
loadsym P1, output;
|
|
|
|
R0 = 0; R7 = 0;
|
|
|
|
P2 = 10;
|
|
LSETUP ( L$0 , L$0end ) LC0 = P2;
|
|
L$0:
|
|
|
|
// I0 and I2 are pointers to state
|
|
loadsym I0, state;
|
|
I2 = I0;
|
|
|
|
// pointer to coeffs
|
|
loadsym I1, Coeff;
|
|
|
|
R0.H = W [ I3 ++ ]; // load input value into RH0
|
|
A0.w = R0; // A0 holds x
|
|
|
|
P2 = 2;
|
|
LSETUP ( L$1 , L$1end ) LC1 = P2;
|
|
|
|
// load 2 coeffs into R1 and R2
|
|
// load state into R3
|
|
R1 = [ I1 ++ ];
|
|
MNOP || R2 = [ I1 ++ ] || R3 = [ I0 ++ ];
|
|
|
|
L$1:
|
|
|
|
// A1=b1*s0 A0=a1*s0+x
|
|
A1 = R1.L * R3.L, A0 += R1.H * R3.L || R1 = [ I1 ++ ] || NOP;
|
|
|
|
// A1+=b2*s1 A0+=a2*s1
|
|
// and move scaled value in A0 (t1) into RL4
|
|
A1 += R2.L * R3.H, R4.L = ( A0 += R2.H * R3.H ) (S2RND) || R2 = [ I1 ++ ] || NOP;
|
|
|
|
// Advance state. before:
|
|
// R4 = uuuu t1
|
|
// R3 = stat[1] stat[0]
|
|
// after PACKLL:
|
|
// R3 = stat[0] t1
|
|
R5 = PACK( R3.L , R4.L ) || R3 = [ I0 ++ ] || NOP;
|
|
|
|
// collect output into A0, and move to RL0.
|
|
// Keep output value in A0, since it is also
|
|
// the accumulator used to store the input to
|
|
// the next stage. Also, store updated state
|
|
L$1end:
|
|
R0.L = ( A0 += A1 ) || [ I2 ++ ] = R5 || NOP;
|
|
|
|
// store output
|
|
L$0end:
|
|
W [ P1 ++ ] = R0;
|
|
|
|
// Check results
|
|
loadsym I2, output;
|
|
R0.L = W [ I2 ++ ]; DBGA ( R0.L , 0x0028 );
|
|
R0.L = W [ I2 ++ ]; DBGA ( R0.L , 0x0110 );
|
|
R0.L = W [ I2 ++ ]; DBGA ( R0.L , 0x0373 );
|
|
R0.L = W [ I2 ++ ]; DBGA ( R0.L , 0x075b );
|
|
R0.L = W [ I2 ++ ]; DBGA ( R0.L , 0x0c00 );
|
|
R0.L = W [ I2 ++ ]; DBGA ( R0.L , 0x1064 );
|
|
R0.L = W [ I2 ++ ]; DBGA ( R0.L , 0x13d3 );
|
|
R0.L = W [ I2 ++ ]; DBGA ( R0.L , 0x15f2 );
|
|
R0.L = W [ I2 ++ ]; DBGA ( R0.L , 0x16b9 );
|
|
R0.L = W [ I2 ++ ]; DBGA ( R0.L , 0x1650 );
|
|
|
|
pass
|
|
|
|
.data
|
|
state:
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
|
|
.data
|
|
Coeff:
|
|
.dw 0x7fff
|
|
.dw 0x5999
|
|
.dw 0x4000
|
|
.dw 0xe000
|
|
.dw 0x7fff
|
|
.dw 0x5999
|
|
.dw 0x4000
|
|
.dw 0xe000
|
|
input:
|
|
.dw 0x0028
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
output:
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|
|
.dw 0x0000
|