ef69fb45879ce71cf7ba7ed7a8ba33aa2e1ad9c7
[opus.git] / silk / fixed / silk_solve_LS_FIX.c
1 /***********************************************************************\r
2 Copyright (c) 2006-2011, Skype Limited. All rights reserved. \r
3 Redistribution and use in source and binary forms, with or without \r
4 modification, (subject to the limitations in the disclaimer below) \r
5 are permitted provided that the following conditions are met:\r
6 - Redistributions of source code must retain the above copyright notice,\r
7 this list of conditions and the following disclaimer.\r
8 - Redistributions in binary form must reproduce the above copyright \r
9 notice, this list of conditions and the following disclaimer in the \r
10 documentation and/or other materials provided with the distribution.\r
11 - Neither the name of Skype Limited, nor the names of specific \r
12 contributors, may be used to endorse or promote products derived from \r
13 this software without specific prior written permission.\r
14 NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED \r
15 BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND \r
16 CONTRIBUTORS ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,\r
17 BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND \r
18 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE \r
19 COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, \r
20 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT\r
21 NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF \r
22 USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON \r
23 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT \r
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE \r
25 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\r
26 ***********************************************************************/\r
27 \r
28 #include "silk_main_FIX.h"\r
29 #include "silk_tuning_parameters.h"\r
30 \r
31 /*****************************/\r
32 /* Internal function headers */\r
33 /*****************************/\r
34 \r
35 typedef struct {\r
36     opus_int32 Q36_part;\r
37     opus_int32 Q48_part;\r
38 } inv_D_t;\r
39 \r
40 /* Factorize square matrix A into LDL form */\r
41 SKP_INLINE void silk_LDL_factorize_FIX(\r
42     opus_int32           *A,         /* I/O Pointer to Symetric Square Matrix */\r
43     opus_int             M,          /* I   Size of Matrix */\r
44     opus_int32           *L_Q16,     /* I/O Pointer to Square Upper triangular Matrix */\r
45     inv_D_t             *inv_D      /* I/O Pointer to vector holding inverted diagonal elements of D */\r
46 );\r
47 \r
48 /* Solve Lx = b, when L is lower triangular and has ones on the diagonal */\r
49 SKP_INLINE void silk_LS_SolveFirst_FIX(\r
50     const opus_int32     *L_Q16,     /* I Pointer to Lower Triangular Matrix */\r
51     opus_int             M,          /* I Dim of Matrix equation */\r
52     const opus_int32     *b,         /* I b Vector */\r
53     opus_int32           *x_Q16      /* O x Vector */  \r
54 );\r
55 \r
56 /* Solve L^t*x = b, where L is lower triangular with ones on the diagonal */\r
57 SKP_INLINE void silk_LS_SolveLast_FIX(\r
58     const opus_int32     *L_Q16,     /* I Pointer to Lower Triangular Matrix */\r
59     const opus_int       M,          /* I Dim of Matrix equation */\r
60     const opus_int32     *b,         /* I b Vector */\r
61     opus_int32           *x_Q16      /* O x Vector */  \r
62 );\r
63 \r
64 SKP_INLINE void silk_LS_divide_Q16_FIX(\r
65     opus_int32           T[],    /* I/O Numenator vector */\r
66     inv_D_t             *inv_D, /* I   1 / D vector     */\r
67     opus_int             M       /* I   dimension        */\r
68 );\r
69 \r
70 /* Solves Ax = b, assuming A is symmetric */\r
71 void silk_solve_LDL_FIX(\r
72     opus_int32                       *A,                 /* I    Pointer to symetric square matrix A         */\r
73     opus_int                         M,                  /* I    Size of matrix                              */\r
74     const opus_int32                 *b,                 /* I    Pointer to b vector                         */\r
75     opus_int32                       *x_Q16              /* O    Pointer to x solution vector                */\r
76 )\r
77 {\r
78     opus_int32 L_Q16[  MAX_MATRIX_SIZE * MAX_MATRIX_SIZE ]; \r
79     opus_int32 Y[      MAX_MATRIX_SIZE ];\r
80     inv_D_t   inv_D[  MAX_MATRIX_SIZE ];\r
81 \r
82     SKP_assert( M <= MAX_MATRIX_SIZE );\r
83 \r
84     /***************************************************\r
85     Factorize A by LDL such that A = L*D*L',\r
86     where L is lower triangular with ones on diagonal\r
87     ****************************************************/\r
88     silk_LDL_factorize_FIX( A, M, L_Q16, inv_D );\r
89         \r
90     /****************************************************\r
91     * substitute D*L'*x = Y. ie:\r
92     L*D*L'*x = b => L*Y = b <=> Y = inv(L)*b\r
93     ******************************************************/\r
94     silk_LS_SolveFirst_FIX( L_Q16, M, b, Y );\r
95 \r
96     /****************************************************\r
97     D*L'*x = Y <=> L'*x = inv(D)*Y, because D is \r
98     diagonal just multiply with 1/d_i\r
99     ****************************************************/\r
100     silk_LS_divide_Q16_FIX( Y, inv_D, M );\r
101 \r
102     /****************************************************\r
103     x = inv(L') * inv(D) * Y\r
104     *****************************************************/\r
105     silk_LS_SolveLast_FIX( L_Q16, M, Y, x_Q16 );\r
106 }\r
107 \r
108 SKP_INLINE void silk_LDL_factorize_FIX(\r
109     opus_int32           *A,         /* I   Pointer to Symetric Square Matrix */\r
110     opus_int             M,          /* I   Size of Matrix */\r
111     opus_int32           *L_Q16,     /* I/O Pointer to Square Upper triangular Matrix */\r
112     inv_D_t             *inv_D      /* I/O Pointer to vector holding inverted diagonal elements of D */\r
113 )\r
114 {\r
115     opus_int   i, j, k, status, loop_count;\r
116     const opus_int32 *ptr1, *ptr2;\r
117     opus_int32 diag_min_value, tmp_32, err;\r
118     opus_int32 v_Q0[ MAX_MATRIX_SIZE ], D_Q0[ MAX_MATRIX_SIZE ];\r
119     opus_int32 one_div_diag_Q36, one_div_diag_Q40, one_div_diag_Q48;\r
120 \r
121     SKP_assert( M <= MAX_MATRIX_SIZE );\r
122 \r
123     status = 1;\r
124     diag_min_value = SKP_max_32( SKP_SMMUL( SKP_ADD_SAT32( A[ 0 ], A[ SKP_SMULBB( M, M ) - 1 ] ), SILK_FIX_CONST( FIND_LTP_COND_FAC, 31 ) ), 1 << 9 );\r
125     for( loop_count = 0; loop_count < M && status == 1; loop_count++ ) {\r
126         status = 0;\r
127         for( j = 0; j < M; j++ ) {\r
128             ptr1 = matrix_adr( L_Q16, j, 0, M );\r
129             tmp_32 = 0;\r
130             for( i = 0; i < j; i++ ) {\r
131                 v_Q0[ i ] = SKP_SMULWW(         D_Q0[ i ], ptr1[ i ] ); /* Q0 */\r
132                 tmp_32    = SKP_SMLAWW( tmp_32, v_Q0[ i ], ptr1[ i ] ); /* Q0 */\r
133             }\r
134             tmp_32 = SKP_SUB32( matrix_ptr( A, j, j, M ), tmp_32 );\r
135 \r
136             if( tmp_32 < diag_min_value ) {\r
137                 tmp_32 = SKP_SUB32( SKP_SMULBB( loop_count + 1, diag_min_value ), tmp_32 );\r
138                 /* Matrix not positive semi-definite, or ill conditioned */\r
139                 for( i = 0; i < M; i++ ) {\r
140                     matrix_ptr( A, i, i, M ) = SKP_ADD32( matrix_ptr( A, i, i, M ), tmp_32 );\r
141                 }\r
142                 status = 1;\r
143                 break;\r
144             }\r
145             D_Q0[ j ] = tmp_32;                         /* always < max(Correlation) */\r
146         \r
147             /* two-step division */\r
148             one_div_diag_Q36 = silk_INVERSE32_varQ( tmp_32, 36 );                    /* Q36 */\r
149             one_div_diag_Q40 = SKP_LSHIFT( one_div_diag_Q36, 4 );                   /* Q40 */\r
150             err = SKP_SUB32( 1 << 24, SKP_SMULWW( tmp_32, one_div_diag_Q40 ) );     /* Q24 */\r
151             one_div_diag_Q48 = SKP_SMULWW( err, one_div_diag_Q40 );                 /* Q48 */\r
152 \r
153             /* Save 1/Ds */\r
154             inv_D[ j ].Q36_part = one_div_diag_Q36;\r
155             inv_D[ j ].Q48_part = one_div_diag_Q48;\r
156 \r
157             matrix_ptr( L_Q16, j, j, M ) = 65536; /* 1.0 in Q16 */\r
158             ptr1 = matrix_adr( A, j, 0, M );\r
159             ptr2 = matrix_adr( L_Q16, j + 1, 0, M );\r
160             for( i = j + 1; i < M; i++ ) { \r
161                 tmp_32 = 0;\r
162                 for( k = 0; k < j; k++ ) {\r
163                     tmp_32 = SKP_SMLAWW( tmp_32, v_Q0[ k ], ptr2[ k ] ); /* Q0 */\r
164                 }\r
165                 tmp_32 = SKP_SUB32( ptr1[ i ], tmp_32 ); /* always < max(Correlation) */\r
166 \r
167                 /* tmp_32 / D_Q0[j] : Divide to Q16 */\r
168                 matrix_ptr( L_Q16, i, j, M ) = SKP_ADD32( SKP_SMMUL( tmp_32, one_div_diag_Q48 ),\r
169                     SKP_RSHIFT( SKP_SMULWW( tmp_32, one_div_diag_Q36 ), 4 ) );\r
170 \r
171                 /* go to next column */\r
172                 ptr2 += M; \r
173             }\r
174         }\r
175     }\r
176 \r
177     SKP_assert( status == 0 );\r
178 }\r
179 \r
180 SKP_INLINE void silk_LS_divide_Q16_FIX(\r
181     opus_int32 T[],      /* I/O Numenator vector */\r
182     inv_D_t *inv_D,     /* I   1 / D vector     */\r
183     opus_int M           /* I   Order */\r
184 )\r
185 {\r
186     opus_int   i;\r
187     opus_int32 tmp_32;\r
188     opus_int32 one_div_diag_Q36, one_div_diag_Q48;\r
189 \r
190     for( i = 0; i < M; i++ ) {\r
191         one_div_diag_Q36 = inv_D[ i ].Q36_part;\r
192         one_div_diag_Q48 = inv_D[ i ].Q48_part;\r
193 \r
194         tmp_32 = T[ i ];\r
195         T[ i ] = SKP_ADD32( SKP_SMMUL( tmp_32, one_div_diag_Q48 ), SKP_RSHIFT( SKP_SMULWW( tmp_32, one_div_diag_Q36 ), 4 ) );\r
196     }\r
197 }\r
198 \r
199 /* Solve Lx = b, when L is lower triangular and has ones on the diagonal */\r
200 SKP_INLINE void silk_LS_SolveFirst_FIX(\r
201     const opus_int32     *L_Q16, /* I Pointer to Lower Triangular Matrix */\r
202     opus_int             M,      /* I Dim of Matrix equation */\r
203     const opus_int32     *b,     /* I b Vector */\r
204     opus_int32           *x_Q16  /* O x Vector */  \r
205 )\r
206 {\r
207     opus_int i, j;\r
208     const opus_int32 *ptr32;\r
209     opus_int32 tmp_32;\r
210 \r
211     for( i = 0; i < M; i++ ) {\r
212         ptr32 = matrix_adr( L_Q16, i, 0, M );\r
213         tmp_32 = 0;\r
214         for( j = 0; j < i; j++ ) {\r
215             tmp_32 = SKP_SMLAWW( tmp_32, ptr32[ j ], x_Q16[ j ] );\r
216         }\r
217         x_Q16[ i ] = SKP_SUB32( b[ i ], tmp_32 );\r
218     }\r
219 }\r
220 \r
221 /* Solve L^t*x = b, where L is lower triangular with ones on the diagonal */\r
222 SKP_INLINE void silk_LS_SolveLast_FIX(\r
223     const opus_int32     *L_Q16,     /* I Pointer to Lower Triangular Matrix */\r
224     const opus_int       M,          /* I Dim of Matrix equation */\r
225     const opus_int32     *b,         /* I b Vector */\r
226     opus_int32           *x_Q16      /* O x Vector */  \r
227 )\r
228 {\r
229     opus_int i, j;\r
230     const opus_int32 *ptr32;\r
231     opus_int32 tmp_32;\r
232 \r
233     for( i = M - 1; i >= 0; i-- ) {\r
234         ptr32 = matrix_adr( L_Q16, 0, i, M );\r
235         tmp_32 = 0;\r
236         for( j = M - 1; j > i; j-- ) {\r
237             tmp_32 = SKP_SMLAWW( tmp_32, ptr32[ SKP_SMULBB( j, M ) ], x_Q16[ j ] );\r
238         }\r
239         x_Q16[ i ] = SKP_SUB32( b[ i ], tmp_32 );\r
240     }\r
241 }\r