| 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 |