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