Line data Source code
1 : /***********************************************************************
2 : Copyright (c) 2006-2011, Skype Limited. All rights reserved.
3 : Redistribution and use in source and binary forms, with or without
4 : modification, are permitted provided that the following conditions
5 : are met:
6 : - Redistributions of source code must retain the above copyright notice,
7 : this list of conditions and the following disclaimer.
8 : - Redistributions in binary form must reproduce the above copyright
9 : notice, this list of conditions and the following disclaimer in the
10 : documentation and/or other materials provided with the distribution.
11 : - Neither the name of Internet Society, IETF or IETF Trust, nor the
12 : names of specific contributors, may be used to endorse or promote
13 : products derived from this software without specific prior written
14 : permission.
15 : THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 : AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 : IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 : ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 : LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 : CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 : SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 : INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 : CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 : ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 : POSSIBILITY OF SUCH DAMAGE.
26 : ***********************************************************************/
27 :
28 : /* *
29 : * silk_biquad_alt.c *
30 : * *
31 : * Second order ARMA filter *
32 : * Can handle slowly varying filter coefficients *
33 : * */
34 :
35 : #ifdef HAVE_CONFIG_H
36 : #include "config.h"
37 : #endif
38 :
39 : #include "SigProc_FIX.h"
40 :
41 : /* Second order ARMA filter, alternative implementation */
42 0 : void silk_biquad_alt_stride1(
43 : const opus_int16 *in, /* I input signal */
44 : const opus_int32 *B_Q28, /* I MA coefficients [3] */
45 : const opus_int32 *A_Q28, /* I AR coefficients [2] */
46 : opus_int32 *S, /* I/O State vector [2] */
47 : opus_int16 *out, /* O output signal */
48 : const opus_int32 len /* I signal length (must be even) */
49 : )
50 : {
51 : /* DIRECT FORM II TRANSPOSED (uses 2 element state vector) */
52 : opus_int k;
53 : opus_int32 inval, A0_U_Q28, A0_L_Q28, A1_U_Q28, A1_L_Q28, out32_Q14;
54 :
55 : /* Negate A_Q28 values and split in two parts */
56 0 : A0_L_Q28 = ( -A_Q28[ 0 ] ) & 0x00003FFF; /* lower part */
57 0 : A0_U_Q28 = silk_RSHIFT( -A_Q28[ 0 ], 14 ); /* upper part */
58 0 : A1_L_Q28 = ( -A_Q28[ 1 ] ) & 0x00003FFF; /* lower part */
59 0 : A1_U_Q28 = silk_RSHIFT( -A_Q28[ 1 ], 14 ); /* upper part */
60 :
61 0 : for( k = 0; k < len; k++ ) {
62 : /* S[ 0 ], S[ 1 ]: Q12 */
63 0 : inval = in[ k ];
64 0 : out32_Q14 = silk_LSHIFT( silk_SMLAWB( S[ 0 ], B_Q28[ 0 ], inval ), 2 );
65 :
66 0 : S[ 0 ] = S[1] + silk_RSHIFT_ROUND( silk_SMULWB( out32_Q14, A0_L_Q28 ), 14 );
67 0 : S[ 0 ] = silk_SMLAWB( S[ 0 ], out32_Q14, A0_U_Q28 );
68 0 : S[ 0 ] = silk_SMLAWB( S[ 0 ], B_Q28[ 1 ], inval);
69 :
70 0 : S[ 1 ] = silk_RSHIFT_ROUND( silk_SMULWB( out32_Q14, A1_L_Q28 ), 14 );
71 0 : S[ 1 ] = silk_SMLAWB( S[ 1 ], out32_Q14, A1_U_Q28 );
72 0 : S[ 1 ] = silk_SMLAWB( S[ 1 ], B_Q28[ 2 ], inval );
73 :
74 : /* Scale back to Q0 and saturate */
75 0 : out[ k ] = (opus_int16)silk_SAT16( silk_RSHIFT( out32_Q14 + (1<<14) - 1, 14 ) );
76 : }
77 0 : }
78 :
79 0 : void silk_biquad_alt_stride2_c(
80 : const opus_int16 *in, /* I input signal */
81 : const opus_int32 *B_Q28, /* I MA coefficients [3] */
82 : const opus_int32 *A_Q28, /* I AR coefficients [2] */
83 : opus_int32 *S, /* I/O State vector [4] */
84 : opus_int16 *out, /* O output signal */
85 : const opus_int32 len /* I signal length (must be even) */
86 : )
87 : {
88 : /* DIRECT FORM II TRANSPOSED (uses 2 element state vector) */
89 : opus_int k;
90 : opus_int32 A0_U_Q28, A0_L_Q28, A1_U_Q28, A1_L_Q28, out32_Q14[ 2 ];
91 :
92 : /* Negate A_Q28 values and split in two parts */
93 0 : A0_L_Q28 = ( -A_Q28[ 0 ] ) & 0x00003FFF; /* lower part */
94 0 : A0_U_Q28 = silk_RSHIFT( -A_Q28[ 0 ], 14 ); /* upper part */
95 0 : A1_L_Q28 = ( -A_Q28[ 1 ] ) & 0x00003FFF; /* lower part */
96 0 : A1_U_Q28 = silk_RSHIFT( -A_Q28[ 1 ], 14 ); /* upper part */
97 :
98 0 : for( k = 0; k < len; k++ ) {
99 : /* S[ 0 ], S[ 1 ], S[ 2 ], S[ 3 ]: Q12 */
100 0 : out32_Q14[ 0 ] = silk_LSHIFT( silk_SMLAWB( S[ 0 ], B_Q28[ 0 ], in[ 2 * k + 0 ] ), 2 );
101 0 : out32_Q14[ 1 ] = silk_LSHIFT( silk_SMLAWB( S[ 2 ], B_Q28[ 0 ], in[ 2 * k + 1 ] ), 2 );
102 :
103 0 : S[ 0 ] = S[ 1 ] + silk_RSHIFT_ROUND( silk_SMULWB( out32_Q14[ 0 ], A0_L_Q28 ), 14 );
104 0 : S[ 2 ] = S[ 3 ] + silk_RSHIFT_ROUND( silk_SMULWB( out32_Q14[ 1 ], A0_L_Q28 ), 14 );
105 0 : S[ 0 ] = silk_SMLAWB( S[ 0 ], out32_Q14[ 0 ], A0_U_Q28 );
106 0 : S[ 2 ] = silk_SMLAWB( S[ 2 ], out32_Q14[ 1 ], A0_U_Q28 );
107 0 : S[ 0 ] = silk_SMLAWB( S[ 0 ], B_Q28[ 1 ], in[ 2 * k + 0 ] );
108 0 : S[ 2 ] = silk_SMLAWB( S[ 2 ], B_Q28[ 1 ], in[ 2 * k + 1 ] );
109 :
110 0 : S[ 1 ] = silk_RSHIFT_ROUND( silk_SMULWB( out32_Q14[ 0 ], A1_L_Q28 ), 14 );
111 0 : S[ 3 ] = silk_RSHIFT_ROUND( silk_SMULWB( out32_Q14[ 1 ], A1_L_Q28 ), 14 );
112 0 : S[ 1 ] = silk_SMLAWB( S[ 1 ], out32_Q14[ 0 ], A1_U_Q28 );
113 0 : S[ 3 ] = silk_SMLAWB( S[ 3 ], out32_Q14[ 1 ], A1_U_Q28 );
114 0 : S[ 1 ] = silk_SMLAWB( S[ 1 ], B_Q28[ 2 ], in[ 2 * k + 0 ] );
115 0 : S[ 3 ] = silk_SMLAWB( S[ 3 ], B_Q28[ 2 ], in[ 2 * k + 1 ] );
116 :
117 : /* Scale back to Q0 and saturate */
118 0 : out[ 2 * k + 0 ] = (opus_int16)silk_SAT16( silk_RSHIFT( out32_Q14[ 0 ] + (1<<14) - 1, 14 ) );
119 0 : out[ 2 * k + 1 ] = (opus_int16)silk_SAT16( silk_RSHIFT( out32_Q14[ 1 ] + (1<<14) - 1, 14 ) );
120 : }
121 0 : }
|
