OLD | NEW |
1 /*********************************************************************** | 1 /*********************************************************************** |
2 Copyright (c) 2006-2011, Skype Limited. All rights reserved. | 2 Copyright (c) 2006-2011, Skype Limited. All rights reserved. |
3 Redistribution and use in source and binary forms, with or without | 3 Redistribution and use in source and binary forms, with or without |
4 modification, are permitted provided that the following conditions | 4 modification, are permitted provided that the following conditions |
5 are met: | 5 are met: |
6 - Redistributions of source code must retain the above copyright notice, | 6 - Redistributions of source code must retain the above copyright notice, |
7 this list of conditions and the following disclaimer. | 7 this list of conditions and the following disclaimer. |
8 - Redistributions in binary form must reproduce the above copyright | 8 - Redistributions in binary form must reproduce the above copyright |
9 notice, this list of conditions and the following disclaimer in the | 9 notice, this list of conditions and the following disclaimer in the |
10 documentation and/or other materials provided with the distribution. | 10 documentation and/or other materials provided with the distribution. |
(...skipping 25 matching lines...) Expand all Loading... |
36 /*****************************/ | 36 /*****************************/ |
37 /* Internal function headers */ | 37 /* Internal function headers */ |
38 /*****************************/ | 38 /*****************************/ |
39 | 39 |
40 typedef struct { | 40 typedef struct { |
41 opus_int32 Q36_part; | 41 opus_int32 Q36_part; |
42 opus_int32 Q48_part; | 42 opus_int32 Q48_part; |
43 } inv_D_t; | 43 } inv_D_t; |
44 | 44 |
45 /* Factorize square matrix A into LDL form */ | 45 /* Factorize square matrix A into LDL form */ |
46 static inline void silk_LDL_factorize_FIX( | 46 static OPUS_INLINE void silk_LDL_factorize_FIX( |
47 opus_int32 *A, /* I/O Pointer to Symetric Square Matrix
*/ | 47 opus_int32 *A, /* I/O Pointer to Symetric Square Matrix
*/ |
48 opus_int M, /* I Size of Matrix
*/ | 48 opus_int M, /* I Size of Matrix
*/ |
49 opus_int32 *L_Q16, /* I/O Pointer to Square Upper triangular Ma
trix */ | 49 opus_int32 *L_Q16, /* I/O Pointer to Square Upper triangular Ma
trix */ |
50 inv_D_t *inv_D /* I/O Pointer to vector holding inverted di
agonal elements of D */ | 50 inv_D_t *inv_D /* I/O Pointer to vector holding inverted di
agonal elements of D */ |
51 ); | 51 ); |
52 | 52 |
53 /* Solve Lx = b, when L is lower triangular and has ones on the diagonal */ | 53 /* Solve Lx = b, when L is lower triangular and has ones on the diagonal */ |
54 static inline void silk_LS_SolveFirst_FIX( | 54 static OPUS_INLINE void silk_LS_SolveFirst_FIX( |
55 const opus_int32 *L_Q16, /* I Pointer to Lower Triangular Matrix
*/ | 55 const opus_int32 *L_Q16, /* I Pointer to Lower Triangular Matrix
*/ |
56 opus_int M, /* I Dim of Matrix equation
*/ | 56 opus_int M, /* I Dim of Matrix equation
*/ |
57 const opus_int32 *b, /* I b Vector
*/ | 57 const opus_int32 *b, /* I b Vector
*/ |
58 opus_int32 *x_Q16 /* O x Vector
*/ | 58 opus_int32 *x_Q16 /* O x Vector
*/ |
59 ); | 59 ); |
60 | 60 |
61 /* Solve L^t*x = b, where L is lower triangular with ones on the diagonal */ | 61 /* Solve L^t*x = b, where L is lower triangular with ones on the diagonal */ |
62 static inline void silk_LS_SolveLast_FIX( | 62 static OPUS_INLINE void silk_LS_SolveLast_FIX( |
63 const opus_int32 *L_Q16, /* I Pointer to Lower Triangular Matrix
*/ | 63 const opus_int32 *L_Q16, /* I Pointer to Lower Triangular Matrix
*/ |
64 const opus_int M, /* I Dim of Matrix equation
*/ | 64 const opus_int M, /* I Dim of Matrix equation
*/ |
65 const opus_int32 *b, /* I b Vector
*/ | 65 const opus_int32 *b, /* I b Vector
*/ |
66 opus_int32 *x_Q16 /* O x Vector
*/ | 66 opus_int32 *x_Q16 /* O x Vector
*/ |
67 ); | 67 ); |
68 | 68 |
69 static inline void silk_LS_divide_Q16_FIX( | 69 static OPUS_INLINE void silk_LS_divide_Q16_FIX( |
70 opus_int32 T[], /* I/O Numenator vector
*/ | 70 opus_int32 T[], /* I/O Numenator vector
*/ |
71 inv_D_t *inv_D, /* I 1 / D vector
*/ | 71 inv_D_t *inv_D, /* I 1 / D vector
*/ |
72 opus_int M /* I dimension
*/ | 72 opus_int M /* I dimension
*/ |
73 ); | 73 ); |
74 | 74 |
75 /* Solves Ax = b, assuming A is symmetric */ | 75 /* Solves Ax = b, assuming A is symmetric */ |
76 void silk_solve_LDL_FIX( | 76 void silk_solve_LDL_FIX( |
77 opus_int32 *A, /* I
Pointer to symetric square matrix A
*/ | 77 opus_int32 *A, /* I
Pointer to symetric square matrix A
*/ |
78 opus_int M, /* I
Size of matrix
*/ | 78 opus_int M, /* I
Size of matrix
*/ |
79 const opus_int32 *b, /* I
Pointer to b vector
*/ | 79 const opus_int32 *b, /* I
Pointer to b vector
*/ |
(...skipping 26 matching lines...) Expand all Loading... |
106 ****************************************************/ | 106 ****************************************************/ |
107 silk_LS_divide_Q16_FIX( Y, inv_D, M ); | 107 silk_LS_divide_Q16_FIX( Y, inv_D, M ); |
108 | 108 |
109 /**************************************************** | 109 /**************************************************** |
110 x = inv(L') * inv(D) * Y | 110 x = inv(L') * inv(D) * Y |
111 *****************************************************/ | 111 *****************************************************/ |
112 silk_LS_SolveLast_FIX( L_Q16, M, Y, x_Q16 ); | 112 silk_LS_SolveLast_FIX( L_Q16, M, Y, x_Q16 ); |
113 RESTORE_STACK; | 113 RESTORE_STACK; |
114 } | 114 } |
115 | 115 |
116 static inline void silk_LDL_factorize_FIX( | 116 static OPUS_INLINE void silk_LDL_factorize_FIX( |
117 opus_int32 *A, /* I/O Pointer to Symetric Square Matrix
*/ | 117 opus_int32 *A, /* I/O Pointer to Symetric Square Matrix
*/ |
118 opus_int M, /* I Size of Matrix
*/ | 118 opus_int M, /* I Size of Matrix
*/ |
119 opus_int32 *L_Q16, /* I/O Pointer to Square Upper triangular Ma
trix */ | 119 opus_int32 *L_Q16, /* I/O Pointer to Square Upper triangular Ma
trix */ |
120 inv_D_t *inv_D /* I/O Pointer to vector holding inverted di
agonal elements of D */ | 120 inv_D_t *inv_D /* I/O Pointer to vector holding inverted di
agonal elements of D */ |
121 ) | 121 ) |
122 { | 122 { |
123 opus_int i, j, k, status, loop_count; | 123 opus_int i, j, k, status, loop_count; |
124 const opus_int32 *ptr1, *ptr2; | 124 const opus_int32 *ptr1, *ptr2; |
125 opus_int32 diag_min_value, tmp_32, err; | 125 opus_int32 diag_min_value, tmp_32, err; |
126 opus_int32 v_Q0[ MAX_MATRIX_SIZE ], D_Q0[ MAX_MATRIX_SIZE ]; | 126 opus_int32 v_Q0[ MAX_MATRIX_SIZE ], D_Q0[ MAX_MATRIX_SIZE ]; |
(...skipping 51 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
178 | 178 |
179 /* go to next column */ | 179 /* go to next column */ |
180 ptr2 += M; | 180 ptr2 += M; |
181 } | 181 } |
182 } | 182 } |
183 } | 183 } |
184 | 184 |
185 silk_assert( status == 0 ); | 185 silk_assert( status == 0 ); |
186 } | 186 } |
187 | 187 |
188 static inline void silk_LS_divide_Q16_FIX( | 188 static OPUS_INLINE void silk_LS_divide_Q16_FIX( |
189 opus_int32 T[], /* I/O Numenator vector
*/ | 189 opus_int32 T[], /* I/O Numenator vector
*/ |
190 inv_D_t *inv_D, /* I 1 / D vector
*/ | 190 inv_D_t *inv_D, /* I 1 / D vector
*/ |
191 opus_int M /* I dimension
*/ | 191 opus_int M /* I dimension
*/ |
192 ) | 192 ) |
193 { | 193 { |
194 opus_int i; | 194 opus_int i; |
195 opus_int32 tmp_32; | 195 opus_int32 tmp_32; |
196 opus_int32 one_div_diag_Q36, one_div_diag_Q48; | 196 opus_int32 one_div_diag_Q36, one_div_diag_Q48; |
197 | 197 |
198 for( i = 0; i < M; i++ ) { | 198 for( i = 0; i < M; i++ ) { |
199 one_div_diag_Q36 = inv_D[ i ].Q36_part; | 199 one_div_diag_Q36 = inv_D[ i ].Q36_part; |
200 one_div_diag_Q48 = inv_D[ i ].Q48_part; | 200 one_div_diag_Q48 = inv_D[ i ].Q48_part; |
201 | 201 |
202 tmp_32 = T[ i ]; | 202 tmp_32 = T[ i ]; |
203 T[ i ] = silk_ADD32( silk_SMMUL( tmp_32, one_div_diag_Q48 ), silk_RSHIFT
( silk_SMULWW( tmp_32, one_div_diag_Q36 ), 4 ) ); | 203 T[ i ] = silk_ADD32( silk_SMMUL( tmp_32, one_div_diag_Q48 ), silk_RSHIFT
( silk_SMULWW( tmp_32, one_div_diag_Q36 ), 4 ) ); |
204 } | 204 } |
205 } | 205 } |
206 | 206 |
207 /* Solve Lx = b, when L is lower triangular and has ones on the diagonal */ | 207 /* Solve Lx = b, when L is lower triangular and has ones on the diagonal */ |
208 static inline void silk_LS_SolveFirst_FIX( | 208 static OPUS_INLINE void silk_LS_SolveFirst_FIX( |
209 const opus_int32 *L_Q16, /* I Pointer to Lower Triangular Matrix
*/ | 209 const opus_int32 *L_Q16, /* I Pointer to Lower Triangular Matrix
*/ |
210 opus_int M, /* I Dim of Matrix equation
*/ | 210 opus_int M, /* I Dim of Matrix equation
*/ |
211 const opus_int32 *b, /* I b Vector
*/ | 211 const opus_int32 *b, /* I b Vector
*/ |
212 opus_int32 *x_Q16 /* O x Vector
*/ | 212 opus_int32 *x_Q16 /* O x Vector
*/ |
213 ) | 213 ) |
214 { | 214 { |
215 opus_int i, j; | 215 opus_int i, j; |
216 const opus_int32 *ptr32; | 216 const opus_int32 *ptr32; |
217 opus_int32 tmp_32; | 217 opus_int32 tmp_32; |
218 | 218 |
219 for( i = 0; i < M; i++ ) { | 219 for( i = 0; i < M; i++ ) { |
220 ptr32 = matrix_adr( L_Q16, i, 0, M ); | 220 ptr32 = matrix_adr( L_Q16, i, 0, M ); |
221 tmp_32 = 0; | 221 tmp_32 = 0; |
222 for( j = 0; j < i; j++ ) { | 222 for( j = 0; j < i; j++ ) { |
223 tmp_32 = silk_SMLAWW( tmp_32, ptr32[ j ], x_Q16[ j ] ); | 223 tmp_32 = silk_SMLAWW( tmp_32, ptr32[ j ], x_Q16[ j ] ); |
224 } | 224 } |
225 x_Q16[ i ] = silk_SUB32( b[ i ], tmp_32 ); | 225 x_Q16[ i ] = silk_SUB32( b[ i ], tmp_32 ); |
226 } | 226 } |
227 } | 227 } |
228 | 228 |
229 /* Solve L^t*x = b, where L is lower triangular with ones on the diagonal */ | 229 /* Solve L^t*x = b, where L is lower triangular with ones on the diagonal */ |
230 static inline void silk_LS_SolveLast_FIX( | 230 static OPUS_INLINE void silk_LS_SolveLast_FIX( |
231 const opus_int32 *L_Q16, /* I Pointer to Lower Triangular Matrix
*/ | 231 const opus_int32 *L_Q16, /* I Pointer to Lower Triangular Matrix
*/ |
232 const opus_int M, /* I Dim of Matrix equation
*/ | 232 const opus_int M, /* I Dim of Matrix equation
*/ |
233 const opus_int32 *b, /* I b Vector
*/ | 233 const opus_int32 *b, /* I b Vector
*/ |
234 opus_int32 *x_Q16 /* O x Vector
*/ | 234 opus_int32 *x_Q16 /* O x Vector
*/ |
235 ) | 235 ) |
236 { | 236 { |
237 opus_int i, j; | 237 opus_int i, j; |
238 const opus_int32 *ptr32; | 238 const opus_int32 *ptr32; |
239 opus_int32 tmp_32; | 239 opus_int32 tmp_32; |
240 | 240 |
241 for( i = M - 1; i >= 0; i-- ) { | 241 for( i = M - 1; i >= 0; i-- ) { |
242 ptr32 = matrix_adr( L_Q16, 0, i, M ); | 242 ptr32 = matrix_adr( L_Q16, 0, i, M ); |
243 tmp_32 = 0; | 243 tmp_32 = 0; |
244 for( j = M - 1; j > i; j-- ) { | 244 for( j = M - 1; j > i; j-- ) { |
245 tmp_32 = silk_SMLAWW( tmp_32, ptr32[ silk_SMULBB( j, M ) ], x_Q16[ j
] ); | 245 tmp_32 = silk_SMLAWW( tmp_32, ptr32[ silk_SMULBB( j, M ) ], x_Q16[ j
] ); |
246 } | 246 } |
247 x_Q16[ i ] = silk_SUB32( b[ i ], tmp_32 ); | 247 x_Q16[ i ] = silk_SUB32( b[ i ], tmp_32 ); |
248 } | 248 } |
249 } | 249 } |
OLD | NEW |