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 #ifdef HAVE_CONFIG_H
29 #include "config.h"
30 #endif
31
32 #include "main_FIX.h"
33 #include "tuning_parameters.h"
34
35 /*****************************/
36 /* Internal function headers */
37 /*****************************/
38
39 typedef struct {
40 opus_int32 Q36_part;
41 opus_int32 Q48_part;
42 } inv_D_t;
43
44 /* Factorize square matrix A into LDL form */
45 static inline void silk_LDL_factorize_FIX(
46 opus_int32 *A, /* I/O Pointer to Symetric Square Matrix */
47 opus_int M, /* I Size of Matrix */
48 opus_int32 *L_Q16, /* I/O Pointer to Square Upper triangular Matrix */
49 inv_D_t *inv_D /* I/O Pointer to vector holding inverted diagonal elements of D */
50 );
51
52 /* Solve Lx = b, when L is lower triangular and has ones on the diagonal */
53 static inline void silk_LS_SolveFirst_FIX(
54 const opus_int32 *L_Q16, /* I Pointer to Lower Triangular Matrix */
55 opus_int M, /* I Dim of Matrix equation */
56 const opus_int32 *b, /* I b Vector */
57 opus_int32 *x_Q16 /* O x Vector */
58 );
59
60 /* Solve L^t*x = b, where L is lower triangular with ones on the diagonal */
61 static inline void silk_LS_SolveLast_FIX(
62 const opus_int32 *L_Q16, /* I Pointer to Lower Triangular Matrix */
63 const opus_int M, /* I Dim of Matrix equation */
64 const opus_int32 *b, /* I b Vector */
65 opus_int32 *x_Q16 /* O x Vector */
66 );
67
68 static inline void silk_LS_divide_Q16_FIX(
69 opus_int32 T[], /* I/O Numenator vector */
70 inv_D_t *inv_D, /* I 1 / D vector */
71 opus_int M /* I dimension */
72 );
73
74 /* Solves Ax = b, assuming A is symmetric */
silk_solve_LDL_FIX(opus_int32 * A,opus_int M,const opus_int32 * b,opus_int32 * x_Q16)75 void silk_solve_LDL_FIX(
76 opus_int32 *A, /* I Pointer to symetric square matrix A */
77 opus_int M, /* I Size of matrix */
78 const opus_int32 *b, /* I Pointer to b vector */
79 opus_int32 *x_Q16 /* O Pointer to x solution vector */
80 )
81 {
82 opus_int32 L_Q16[ MAX_MATRIX_SIZE * MAX_MATRIX_SIZE ];
83 opus_int32 Y[ MAX_MATRIX_SIZE ];
84 inv_D_t inv_D[ MAX_MATRIX_SIZE ];
85
86 silk_assert( M <= MAX_MATRIX_SIZE );
87
88 /***************************************************
89 Factorize A by LDL such that A = L*D*L',
90 where L is lower triangular with ones on diagonal
91 ****************************************************/
92 silk_LDL_factorize_FIX( A, M, L_Q16, inv_D );
93
94 /****************************************************
95 * substitute D*L'*x = Y. ie:
96 L*D*L'*x = b => L*Y = b <=> Y = inv(L)*b
97 ******************************************************/
98 silk_LS_SolveFirst_FIX( L_Q16, M, b, Y );
99
100 /****************************************************
101 D*L'*x = Y <=> L'*x = inv(D)*Y, because D is
102 diagonal just multiply with 1/d_i
103 ****************************************************/
104 silk_LS_divide_Q16_FIX( Y, inv_D, M );
105
106 /****************************************************
107 x = inv(L') * inv(D) * Y
108 *****************************************************/
109 silk_LS_SolveLast_FIX( L_Q16, M, Y, x_Q16 );
110 }
111
silk_LDL_factorize_FIX(opus_int32 * A,opus_int M,opus_int32 * L_Q16,inv_D_t * inv_D)112 static inline void silk_LDL_factorize_FIX(
113 opus_int32 *A, /* I/O Pointer to Symetric Square Matrix */
114 opus_int M, /* I Size of Matrix */
115 opus_int32 *L_Q16, /* I/O Pointer to Square Upper triangular Matrix */
116 inv_D_t *inv_D /* I/O Pointer to vector holding inverted diagonal elements of D */
117 )
118 {
119 opus_int i, j, k, status, loop_count;
120 const opus_int32 *ptr1, *ptr2;
121 opus_int32 diag_min_value, tmp_32, err;
122 opus_int32 v_Q0[ MAX_MATRIX_SIZE ], D_Q0[ MAX_MATRIX_SIZE ];
123 opus_int32 one_div_diag_Q36, one_div_diag_Q40, one_div_diag_Q48;
124
125 silk_assert( M <= MAX_MATRIX_SIZE );
126
127 status = 1;
128 diag_min_value = silk_max_32( silk_SMMUL( silk_ADD_SAT32( A[ 0 ], A[ silk_SMULBB( M, M ) - 1 ] ), SILK_FIX_CONST( FIND_LTP_COND_FAC, 31 ) ), 1 << 9 );
129 for( loop_count = 0; loop_count < M && status == 1; loop_count++ ) {
130 status = 0;
131 for( j = 0; j < M; j++ ) {
132 ptr1 = matrix_adr( L_Q16, j, 0, M );
133 tmp_32 = 0;
134 for( i = 0; i < j; i++ ) {
135 v_Q0[ i ] = silk_SMULWW( D_Q0[ i ], ptr1[ i ] ); /* Q0 */
136 tmp_32 = silk_SMLAWW( tmp_32, v_Q0[ i ], ptr1[ i ] ); /* Q0 */
137 }
138 tmp_32 = silk_SUB32( matrix_ptr( A, j, j, M ), tmp_32 );
139
140 if( tmp_32 < diag_min_value ) {
141 tmp_32 = silk_SUB32( silk_SMULBB( loop_count + 1, diag_min_value ), tmp_32 );
142 /* Matrix not positive semi-definite, or ill conditioned */
143 for( i = 0; i < M; i++ ) {
144 matrix_ptr( A, i, i, M ) = silk_ADD32( matrix_ptr( A, i, i, M ), tmp_32 );
145 }
146 status = 1;
147 break;
148 }
149 D_Q0[ j ] = tmp_32; /* always < max(Correlation) */
150
151 /* two-step division */
152 one_div_diag_Q36 = silk_INVERSE32_varQ( tmp_32, 36 ); /* Q36 */
153 one_div_diag_Q40 = silk_LSHIFT( one_div_diag_Q36, 4 ); /* Q40 */
154 err = silk_SUB32( (opus_int32)1 << 24, silk_SMULWW( tmp_32, one_div_diag_Q40 ) ); /* Q24 */
155 one_div_diag_Q48 = silk_SMULWW( err, one_div_diag_Q40 ); /* Q48 */
156
157 /* Save 1/Ds */
158 inv_D[ j ].Q36_part = one_div_diag_Q36;
159 inv_D[ j ].Q48_part = one_div_diag_Q48;
160
161 matrix_ptr( L_Q16, j, j, M ) = 65536; /* 1.0 in Q16 */
162 ptr1 = matrix_adr( A, j, 0, M );
163 ptr2 = matrix_adr( L_Q16, j + 1, 0, M );
164 for( i = j + 1; i < M; i++ ) {
165 tmp_32 = 0;
166 for( k = 0; k < j; k++ ) {
167 tmp_32 = silk_SMLAWW( tmp_32, v_Q0[ k ], ptr2[ k ] ); /* Q0 */
168 }
169 tmp_32 = silk_SUB32( ptr1[ i ], tmp_32 ); /* always < max(Correlation) */
170
171 /* tmp_32 / D_Q0[j] : Divide to Q16 */
172 matrix_ptr( L_Q16, i, j, M ) = silk_ADD32( silk_SMMUL( tmp_32, one_div_diag_Q48 ),
173 silk_RSHIFT( silk_SMULWW( tmp_32, one_div_diag_Q36 ), 4 ) );
174
175 /* go to next column */
176 ptr2 += M;
177 }
178 }
179 }
180
181 silk_assert( status == 0 );
182 }
183
silk_LS_divide_Q16_FIX(opus_int32 T[],inv_D_t * inv_D,opus_int M)184 static inline void silk_LS_divide_Q16_FIX(
185 opus_int32 T[], /* I/O Numenator vector */
186 inv_D_t *inv_D, /* I 1 / D vector */
187 opus_int M /* I dimension */
188 )
189 {
190 opus_int i;
191 opus_int32 tmp_32;
192 opus_int32 one_div_diag_Q36, one_div_diag_Q48;
193
194 for( i = 0; i < M; i++ ) {
195 one_div_diag_Q36 = inv_D[ i ].Q36_part;
196 one_div_diag_Q48 = inv_D[ i ].Q48_part;
197
198 tmp_32 = T[ i ];
199 T[ i ] = silk_ADD32( silk_SMMUL( tmp_32, one_div_diag_Q48 ), silk_RSHIFT( silk_SMULWW( tmp_32, one_div_diag_Q36 ), 4 ) );
200 }
201 }
202
203 /* Solve Lx = b, when L is lower triangular and has ones on the diagonal */
silk_LS_SolveFirst_FIX(const opus_int32 * L_Q16,opus_int M,const opus_int32 * b,opus_int32 * x_Q16)204 static inline void silk_LS_SolveFirst_FIX(
205 const opus_int32 *L_Q16, /* I Pointer to Lower Triangular Matrix */
206 opus_int M, /* I Dim of Matrix equation */
207 const opus_int32 *b, /* I b Vector */
208 opus_int32 *x_Q16 /* O x Vector */
209 )
210 {
211 opus_int i, j;
212 const opus_int32 *ptr32;
213 opus_int32 tmp_32;
214
215 for( i = 0; i < M; i++ ) {
216 ptr32 = matrix_adr( L_Q16, i, 0, M );
217 tmp_32 = 0;
218 for( j = 0; j < i; j++ ) {
219 tmp_32 = silk_SMLAWW( tmp_32, ptr32[ j ], x_Q16[ j ] );
220 }
221 x_Q16[ i ] = silk_SUB32( b[ i ], tmp_32 );
222 }
223 }
224
225 /* Solve L^t*x = b, where L is lower triangular with ones on the diagonal */
silk_LS_SolveLast_FIX(const opus_int32 * L_Q16,const opus_int M,const opus_int32 * b,opus_int32 * x_Q16)226 static inline void silk_LS_SolveLast_FIX(
227 const opus_int32 *L_Q16, /* I Pointer to Lower Triangular Matrix */
228 const opus_int M, /* I Dim of Matrix equation */
229 const opus_int32 *b, /* I b Vector */
230 opus_int32 *x_Q16 /* O x Vector */
231 )
232 {
233 opus_int i, j;
234 const opus_int32 *ptr32;
235 opus_int32 tmp_32;
236
237 for( i = M - 1; i >= 0; i-- ) {
238 ptr32 = matrix_adr( L_Q16, 0, i, M );
239 tmp_32 = 0;
240 for( j = M - 1; j > i; j-- ) {
241 tmp_32 = silk_SMLAWW( tmp_32, ptr32[ silk_SMULBB( j, M ) ], x_Q16[ j ] );
242 }
243 x_Q16[ i ] = silk_SUB32( b[ i ], tmp_32 );
244 }
245 }
246