Cleaner way to take into account the prediction for stereo width
[opus.git] / silk / burg_modified.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 "SigProc_FIX.h"
33
34 #define MAX_FRAME_SIZE              384 /* subfr_length * nb_subfr = ( 0.005 * 16000 + 16 ) * 4 = 384*/
35 #define MAX_NB_SUBFR                4
36
37 #define QA                          25
38 #define N_BITS_HEAD_ROOM            2
39 #define MIN_RSHIFTS                 -16
40 #define MAX_RSHIFTS                 (32 - QA)
41
42 /* Compute reflection coefficients from input signal */
43 void silk_burg_modified(
44     opus_int32       *res_nrg,           /* O    residual energy                                                 */
45     opus_int         *res_nrg_Q,         /* O    residual energy Q value                                         */
46     opus_int32       A_Q16[],            /* O    prediction coefficients (length order)                          */
47     const opus_int16 x[],                /* I    input signal, length: nb_subfr * ( D + subfr_length )           */
48     const opus_int   subfr_length,       /* I    input signal subframe length (including D preceeding samples)   */
49     const opus_int   nb_subfr,           /* I    number of subframes stacked in x                                */
50     const opus_int32 WhiteNoiseFrac_Q32, /* I    fraction added to zero-lag autocorrelation                      */
51     const opus_int   D                   /* I    order                                                           */
52 )
53 {
54     opus_int         k, n, s, lz, rshifts, rshifts_extra;
55     opus_int32       C0, num, nrg, rc_Q31, Atmp_QA, Atmp1, tmp1, tmp2, x1, x2;
56     const opus_int16 *x_ptr;
57
58     opus_int32       C_first_row[ SILK_MAX_ORDER_LPC ];
59     opus_int32       C_last_row[  SILK_MAX_ORDER_LPC ];
60     opus_int32       Af_QA[       SILK_MAX_ORDER_LPC ];
61
62     opus_int32       CAf[ SILK_MAX_ORDER_LPC + 1 ];
63     opus_int32       CAb[ SILK_MAX_ORDER_LPC + 1 ];
64
65     silk_assert( subfr_length * nb_subfr <= MAX_FRAME_SIZE );
66     silk_assert( nb_subfr <= MAX_NB_SUBFR );
67
68
69     /* Compute autocorrelations, added over subframes */
70     silk_sum_sqr_shift( &C0, &rshifts, x, nb_subfr * subfr_length );
71     if( rshifts > MAX_RSHIFTS ) {
72         C0 = silk_LSHIFT32( C0, rshifts - MAX_RSHIFTS );
73         silk_assert( C0 > 0 );
74         rshifts = MAX_RSHIFTS;
75     } else {
76         lz = silk_CLZ32( C0 ) - 1;
77         rshifts_extra = N_BITS_HEAD_ROOM - lz;
78         if( rshifts_extra > 0 ) {
79             rshifts_extra = silk_min( rshifts_extra, MAX_RSHIFTS - rshifts );
80             C0 = silk_RSHIFT32( C0, rshifts_extra );
81         } else {
82             rshifts_extra = silk_max( rshifts_extra, MIN_RSHIFTS - rshifts );
83             C0 = silk_LSHIFT32( C0, -rshifts_extra );
84         }
85         rshifts += rshifts_extra;
86     }
87     silk_memset( C_first_row, 0, SILK_MAX_ORDER_LPC * sizeof( opus_int32 ) );
88     if( rshifts > 0 ) {
89         for( s = 0; s < nb_subfr; s++ ) {
90             x_ptr = x + s * subfr_length;
91             for( n = 1; n < D + 1; n++ ) {
92                 C_first_row[ n - 1 ] += (opus_int32)silk_RSHIFT64(
93                     silk_inner_prod16_aligned_64( x_ptr, x_ptr + n, subfr_length - n ), rshifts );
94             }
95         }
96     } else {
97         for( s = 0; s < nb_subfr; s++ ) {
98             x_ptr = x + s * subfr_length;
99             for( n = 1; n < D + 1; n++ ) {
100                 C_first_row[ n - 1 ] += silk_LSHIFT32(
101                     silk_inner_prod_aligned( x_ptr, x_ptr + n, subfr_length - n ), -rshifts );
102             }
103         }
104     }
105     silk_memcpy( C_last_row, C_first_row, SILK_MAX_ORDER_LPC * sizeof( opus_int32 ) );
106
107     /* Initialize */
108     CAb[ 0 ] = CAf[ 0 ] = C0 + silk_SMMUL( WhiteNoiseFrac_Q32, C0 ) + 1;         /* Q(-rshifts)*/
109
110     for( n = 0; n < D; n++ ) {
111         /* Update first row of correlation matrix (without first element) */
112         /* Update last row of correlation matrix (without last element, stored in reversed order) */
113         /* Update C * Af */
114         /* Update C * flipud(Af) (stored in reversed order) */
115         if( rshifts > -2 ) {
116             for( s = 0; s < nb_subfr; s++ ) {
117                 x_ptr = x + s * subfr_length;
118                 x1  = -silk_LSHIFT32( (opus_int32)x_ptr[ n ],                    16 - rshifts );      /* Q(16-rshifts)*/
119                 x2  = -silk_LSHIFT32( (opus_int32)x_ptr[ subfr_length - n - 1 ], 16 - rshifts );      /* Q(16-rshifts)*/
120                 tmp1 = silk_LSHIFT32( (opus_int32)x_ptr[ n ],                    QA - 16 );           /* Q(QA-16)*/
121                 tmp2 = silk_LSHIFT32( (opus_int32)x_ptr[ subfr_length - n - 1 ], QA - 16 );           /* Q(QA-16)*/
122                 for( k = 0; k < n; k++ ) {
123                     C_first_row[ k ] = silk_SMLAWB( C_first_row[ k ], x1, x_ptr[ n - k - 1 ]            ); /* Q( -rshifts )*/
124                     C_last_row[ k ]  = silk_SMLAWB( C_last_row[ k ],  x2, x_ptr[ subfr_length - n + k ] ); /* Q( -rshifts )*/
125                     Atmp_QA = Af_QA[ k ];
126                     tmp1 = silk_SMLAWB( tmp1, Atmp_QA, x_ptr[ n - k - 1 ]            );              /* Q(QA-16)*/
127                     tmp2 = silk_SMLAWB( tmp2, Atmp_QA, x_ptr[ subfr_length - n + k ] );              /* Q(QA-16)*/
128                 }
129                 tmp1 = silk_LSHIFT32( -tmp1, 32 - QA - rshifts );                                    /* Q(16-rshifts)*/
130                 tmp2 = silk_LSHIFT32( -tmp2, 32 - QA - rshifts );                                    /* Q(16-rshifts)*/
131                 for( k = 0; k <= n; k++ ) {
132                     CAf[ k ] = silk_SMLAWB( CAf[ k ], tmp1, x_ptr[ n - k ]                    );     /* Q( -rshift )*/
133                     CAb[ k ] = silk_SMLAWB( CAb[ k ], tmp2, x_ptr[ subfr_length - n + k - 1 ] );     /* Q( -rshift )*/
134                 }
135             }
136         } else {
137             for( s = 0; s < nb_subfr; s++ ) {
138                 x_ptr = x + s * subfr_length;
139                 x1  = -silk_LSHIFT32( (opus_int32)x_ptr[ n ],                    -rshifts );          /* Q( -rshifts )*/
140                 x2  = -silk_LSHIFT32( (opus_int32)x_ptr[ subfr_length - n - 1 ], -rshifts );          /* Q( -rshifts )*/
141                 tmp1 = silk_LSHIFT32( (opus_int32)x_ptr[ n ],                    17 );                /* Q17*/
142                 tmp2 = silk_LSHIFT32( (opus_int32)x_ptr[ subfr_length - n - 1 ], 17 );                /* Q17*/
143                 for( k = 0; k < n; k++ ) {
144                     C_first_row[ k ] = silk_MLA( C_first_row[ k ], x1, x_ptr[ n - k - 1 ]            ); /* Q( -rshifts )*/
145                     C_last_row[ k ]  = silk_MLA( C_last_row[ k ],  x2, x_ptr[ subfr_length - n + k ] ); /* Q( -rshifts )*/
146                     Atmp1 = silk_RSHIFT_ROUND( Af_QA[ k ], QA - 17 );                                /* Q17*/
147                     tmp1 = silk_MLA( tmp1, x_ptr[ n - k - 1 ],            Atmp1 );                   /* Q17*/
148                     tmp2 = silk_MLA( tmp2, x_ptr[ subfr_length - n + k ], Atmp1 );                   /* Q17*/
149                 }
150                 tmp1 = -tmp1;                                                                       /* Q17*/
151                 tmp2 = -tmp2;                                                                       /* Q17*/
152                 for( k = 0; k <= n; k++ ) {
153                     CAf[ k ] = silk_SMLAWW( CAf[ k ], tmp1,
154                         silk_LSHIFT32( (opus_int32)x_ptr[ n - k ], -rshifts - 1 ) );                  /* Q( -rshift )*/
155                     CAb[ k ] = silk_SMLAWW( CAb[ k ], tmp2,
156                         silk_LSHIFT32( (opus_int32)x_ptr[ subfr_length - n + k - 1 ], -rshifts - 1 ) );/* Q( -rshift )*/
157                 }
158             }
159         }
160
161         /* Calculate nominator and denominator for the next order reflection (parcor) coefficient */
162         tmp1 = C_first_row[ n ];                                                            /* Q( -rshifts )*/
163         tmp2 = C_last_row[ n ];                                                             /* Q( -rshifts )*/
164         num  = 0;                                                                           /* Q( -rshifts )*/
165         nrg  = silk_ADD32( CAb[ 0 ], CAf[ 0 ] );                                             /* Q( 1-rshifts )*/
166         for( k = 0; k < n; k++ ) {
167             Atmp_QA = Af_QA[ k ];
168             lz = silk_CLZ32( silk_abs( Atmp_QA ) ) - 1;
169             lz = silk_min( 32 - QA, lz );
170             Atmp1 = silk_LSHIFT32( Atmp_QA, lz );                                            /* Q( QA + lz )*/
171
172             tmp1 = silk_ADD_LSHIFT32( tmp1, silk_SMMUL( C_last_row[  n - k - 1 ], Atmp1 ), 32 - QA - lz );    /* Q( -rshifts )*/
173             tmp2 = silk_ADD_LSHIFT32( tmp2, silk_SMMUL( C_first_row[ n - k - 1 ], Atmp1 ), 32 - QA - lz );    /* Q( -rshifts )*/
174             num  = silk_ADD_LSHIFT32( num,  silk_SMMUL( CAb[ n - k ],             Atmp1 ), 32 - QA - lz );    /* Q( -rshifts )*/
175             nrg  = silk_ADD_LSHIFT32( nrg,  silk_SMMUL( silk_ADD32( CAb[ k + 1 ], CAf[ k + 1 ] ),
176                                                                                 Atmp1 ), 32 - QA - lz );    /* Q( 1-rshifts )*/
177         }
178         CAf[ n + 1 ] = tmp1;                                                                /* Q( -rshifts )*/
179         CAb[ n + 1 ] = tmp2;                                                                /* Q( -rshifts )*/
180         num = silk_ADD32( num, tmp2 );                                                       /* Q( -rshifts )*/
181         num = silk_LSHIFT32( -num, 1 );                                                      /* Q( 1-rshifts )*/
182
183         /* Calculate the next order reflection (parcor) coefficient */
184         if( silk_abs( num ) < nrg ) {
185             rc_Q31 = silk_DIV32_varQ( num, nrg, 31 );
186         } else {
187             /* Negative energy or ratio too high; set remaining coefficients to zero and exit loop */
188             silk_memset( &Af_QA[ n ], 0, ( D - n ) * sizeof( opus_int32 ) );
189             silk_assert( 0 );
190             break;
191         }
192
193         /* Update the AR coefficients */
194         for( k = 0; k < (n + 1) >> 1; k++ ) {
195             tmp1 = Af_QA[ k ];                                                              /* QA*/
196             tmp2 = Af_QA[ n - k - 1 ];                                                      /* QA*/
197             Af_QA[ k ]         = silk_ADD_LSHIFT32( tmp1, silk_SMMUL( tmp2, rc_Q31 ), 1 );    /* QA*/
198             Af_QA[ n - k - 1 ] = silk_ADD_LSHIFT32( tmp2, silk_SMMUL( tmp1, rc_Q31 ), 1 );    /* QA*/
199         }
200         Af_QA[ n ] = silk_RSHIFT32( rc_Q31, 31 - QA );                                       /* QA*/
201
202         /* Update C * Af and C * Ab */
203         for( k = 0; k <= n + 1; k++ ) {
204             tmp1 = CAf[ k ];                                                                /* Q( -rshifts )*/
205             tmp2 = CAb[ n - k + 1 ];                                                        /* Q( -rshifts )*/
206             CAf[ k ]         = silk_ADD_LSHIFT32( tmp1, silk_SMMUL( tmp2, rc_Q31 ), 1 );      /* Q( -rshifts )*/
207             CAb[ n - k + 1 ] = silk_ADD_LSHIFT32( tmp2, silk_SMMUL( tmp1, rc_Q31 ), 1 );      /* Q( -rshifts )*/
208         }
209     }
210
211     /* Return residual energy */
212     nrg  = CAf[ 0 ];                                                                        /* Q( -rshifts )*/
213     tmp1 = 1 << 16;                                                                         /* Q16*/
214     for( k = 0; k < D; k++ ) {
215         Atmp1 = silk_RSHIFT_ROUND( Af_QA[ k ], QA - 16 );                                    /* Q16*/
216         nrg  = silk_SMLAWW( nrg, CAf[ k + 1 ], Atmp1 );                                      /* Q( -rshifts )*/
217         tmp1 = silk_SMLAWW( tmp1, Atmp1, Atmp1 );                                            /* Q16*/
218         A_Q16[ k ] = -Atmp1;
219     }
220     *res_nrg = silk_SMLAWW( nrg, silk_SMMUL( WhiteNoiseFrac_Q32, C0 ), -tmp1 );               /* Q( -rshifts )*/
221     *res_nrg_Q = -rshifts;
222 }