1885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org/*********************************************************************** 2885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgCopyright (c) 2006-2011, Skype Limited. All rights reserved. 3885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgRedistribution and use in source and binary forms, with or without 4885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgmodification, are permitted provided that the following conditions 5885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgare met: 6885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org- Redistributions of source code must retain the above copyright notice, 7885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgthis list of conditions and the following disclaimer. 8885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org- Redistributions in binary form must reproduce the above copyright 9885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgnotice, this list of conditions and the following disclaimer in the 10885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgdocumentation and/or other materials provided with the distribution. 11e3ea049fcaee2247e45f0ce793d4313babb4ef69tlegrand@chromium.org- Neither the name of Internet Society, IETF or IETF Trust, nor the 12885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgnames of specific contributors, may be used to endorse or promote 13885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgproducts derived from this software without specific prior written 14885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgpermission. 15e3ea049fcaee2247e45f0ce793d4313babb4ef69tlegrand@chromium.orgTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgLIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgCONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgSUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgINTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgCONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgPOSSIBILITY OF SUCH DAMAGE. 26885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org***********************************************************************/ 27885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org 28885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org#ifdef HAVE_CONFIG_H 29885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org#include "config.h" 30885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org#endif 31885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org 32885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org/********************************************************************** 33885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org * Correlation Matrix Computations for LS estimate. 34885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org **********************************************************************/ 35885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org 36885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org#include "main_FIX.h" 37885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org 38885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org/* Calculates correlation vector X'*t */ 39885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgvoid silk_corrVector_FIX( 40885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org const opus_int16 *x, /* I x vector [L + order - 1] used to form data matrix X */ 41885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org const opus_int16 *t, /* I Target vector [L] */ 42885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org const opus_int L, /* I Length of vectors */ 43885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org const opus_int order, /* I Max lag for correlation */ 44885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org opus_int32 *Xt, /* O Pointer to X'*t correlation vector [order] */ 45885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org const opus_int rshifts /* I Right shifts of correlations */ 46885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org) 47885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org{ 48885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org opus_int lag, i; 49885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org const opus_int16 *ptr1, *ptr2; 50885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org opus_int32 inner_prod; 51885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org 52885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org ptr1 = &x[ order - 1 ]; /* Points to first sample of column 0 of X: X[:,0] */ 53885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org ptr2 = t; 54885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org /* Calculate X'*t */ 55885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org if( rshifts > 0 ) { 56885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org /* Right shifting used */ 57885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org for( lag = 0; lag < order; lag++ ) { 58885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org inner_prod = 0; 59885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org for( i = 0; i < L; i++ ) { 60885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org inner_prod += silk_RSHIFT32( silk_SMULBB( ptr1[ i ], ptr2[i] ), rshifts ); 61885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org } 62885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org Xt[ lag ] = inner_prod; /* X[:,lag]'*t */ 63885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org ptr1--; /* Go to next column of X */ 64885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org } 65885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org } else { 66885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org silk_assert( rshifts == 0 ); 67885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org for( lag = 0; lag < order; lag++ ) { 68885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org Xt[ lag ] = silk_inner_prod_aligned( ptr1, ptr2, L ); /* X[:,lag]'*t */ 69885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org ptr1--; /* Go to next column of X */ 70885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org } 71885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org } 72885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org} 73885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org 74885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org/* Calculates correlation matrix X'*X */ 75885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.orgvoid silk_corrMatrix_FIX( 76885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org const opus_int16 *x, /* I x vector [L + order - 1] used to form data matrix X */ 77885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org const opus_int L, /* I Length of vectors */ 78885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org const opus_int order, /* I Max lag for correlation */ 79885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org const opus_int head_room, /* I Desired headroom */ 80885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org opus_int32 *XX, /* O Pointer to X'*X correlation matrix [ order x order ] */ 81885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org opus_int *rshifts /* I/O Right shifts of correlations */ 82885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org) 83885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org{ 84885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org opus_int i, j, lag, rshifts_local, head_room_rshifts; 85885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org opus_int32 energy; 86885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org const opus_int16 *ptr1, *ptr2; 87885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org 88885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org /* Calculate energy to find shift used to fit in 32 bits */ 89885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org silk_sum_sqr_shift( &energy, &rshifts_local, x, L + order - 1 ); 90885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org /* Add shifts to get the desired head room */ 91885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org head_room_rshifts = silk_max( head_room - silk_CLZ32( energy ), 0 ); 92885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org 93885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org energy = silk_RSHIFT32( energy, head_room_rshifts ); 94885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org rshifts_local += head_room_rshifts; 95885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org 96885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org /* Calculate energy of first column (0) of X: X[:,0]'*X[:,0] */ 97885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org /* Remove contribution of first order - 1 samples */ 98885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org for( i = 0; i < order - 1; i++ ) { 99885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org energy -= silk_RSHIFT32( silk_SMULBB( x[ i ], x[ i ] ), rshifts_local ); 100885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org } 101885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org if( rshifts_local < *rshifts ) { 102885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org /* Adjust energy */ 103885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org energy = silk_RSHIFT32( energy, *rshifts - rshifts_local ); 104885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org rshifts_local = *rshifts; 105885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org } 106885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org 107885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org /* Calculate energy of remaining columns of X: X[:,j]'*X[:,j] */ 108885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org /* Fill out the diagonal of the correlation matrix */ 109885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org matrix_ptr( XX, 0, 0, order ) = energy; 110885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org ptr1 = &x[ order - 1 ]; /* First sample of column 0 of X */ 111885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org for( j = 1; j < order; j++ ) { 112885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org energy = silk_SUB32( energy, silk_RSHIFT32( silk_SMULBB( ptr1[ L - j ], ptr1[ L - j ] ), rshifts_local ) ); 113885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org energy = silk_ADD32( energy, silk_RSHIFT32( silk_SMULBB( ptr1[ -j ], ptr1[ -j ] ), rshifts_local ) ); 114885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org matrix_ptr( XX, j, j, order ) = energy; 115885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org } 116885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org 117885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org ptr2 = &x[ order - 2 ]; /* First sample of column 1 of X */ 118885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org /* Calculate the remaining elements of the correlation matrix */ 119885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org if( rshifts_local > 0 ) { 120885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org /* Right shifting used */ 121885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org for( lag = 1; lag < order; lag++ ) { 122885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org /* Inner product of column 0 and column lag: X[:,0]'*X[:,lag] */ 123885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org energy = 0; 124885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org for( i = 0; i < L; i++ ) { 125885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org energy += silk_RSHIFT32( silk_SMULBB( ptr1[ i ], ptr2[i] ), rshifts_local ); 126885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org } 127885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org /* Calculate remaining off diagonal: X[:,j]'*X[:,j + lag] */ 128885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org matrix_ptr( XX, lag, 0, order ) = energy; 129885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org matrix_ptr( XX, 0, lag, order ) = energy; 130885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org for( j = 1; j < ( order - lag ); j++ ) { 131885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org energy = silk_SUB32( energy, silk_RSHIFT32( silk_SMULBB( ptr1[ L - j ], ptr2[ L - j ] ), rshifts_local ) ); 132885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org energy = silk_ADD32( energy, silk_RSHIFT32( silk_SMULBB( ptr1[ -j ], ptr2[ -j ] ), rshifts_local ) ); 133885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org matrix_ptr( XX, lag + j, j, order ) = energy; 134885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org matrix_ptr( XX, j, lag + j, order ) = energy; 135885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org } 136885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org ptr2--; /* Update pointer to first sample of next column (lag) in X */ 137885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org } 138885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org } else { 139885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org for( lag = 1; lag < order; lag++ ) { 140885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org /* Inner product of column 0 and column lag: X[:,0]'*X[:,lag] */ 141885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org energy = silk_inner_prod_aligned( ptr1, ptr2, L ); 142885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org matrix_ptr( XX, lag, 0, order ) = energy; 143885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org matrix_ptr( XX, 0, lag, order ) = energy; 144885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org /* Calculate remaining off diagonal: X[:,j]'*X[:,j + lag] */ 145885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org for( j = 1; j < ( order - lag ); j++ ) { 146885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org energy = silk_SUB32( energy, silk_SMULBB( ptr1[ L - j ], ptr2[ L - j ] ) ); 147885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org energy = silk_SMLABB( energy, ptr1[ -j ], ptr2[ -j ] ); 148885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org matrix_ptr( XX, lag + j, j, order ) = energy; 149885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org matrix_ptr( XX, j, lag + j, order ) = energy; 150885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org } 151885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org ptr2--;/* Update pointer to first sample of next column (lag) in X */ 152885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org } 153885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org } 154885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org *rshifts = rshifts_local; 155885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org} 156885f2ff5a7a7d6a73432d26a6c0ae9147e6b452sergeyu@chromium.org 157