Reformatting changes with an update to the MSVC project files
[opus.git] / silk / fixed / solve_LS_FIX.c
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, (subject to the limitations in the disclaimer below)
5 are permitted provided that the following conditions 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 Skype Limited, nor the names of specific
12 contributors, may be used to endorse or promote products derived from
13 this software without specific prior written permission.
14 NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED
15 BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
16 CONTRIBUTORS ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
17 BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
18 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
19 COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
20 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
21 NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
22 USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
23 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
25 OF THIS SOFTWARE, EVEN IF ADVISED OF THE 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 */
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
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( 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
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 */
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 */
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 }