Chris@69: /*********************************************************************** Chris@69: Copyright (c) 2006-2011, Skype Limited. All rights reserved. Chris@69: Redistribution and use in source and binary forms, with or without Chris@69: modification, are permitted provided that the following conditions Chris@69: are met: Chris@69: - Redistributions of source code must retain the above copyright notice, Chris@69: this list of conditions and the following disclaimer. Chris@69: - Redistributions in binary form must reproduce the above copyright Chris@69: notice, this list of conditions and the following disclaimer in the Chris@69: documentation and/or other materials provided with the distribution. Chris@69: - Neither the name of Internet Society, IETF or IETF Trust, nor the Chris@69: names of specific contributors, may be used to endorse or promote Chris@69: products derived from this software without specific prior written Chris@69: permission. Chris@69: THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" Chris@69: AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE Chris@69: IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE Chris@69: ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE Chris@69: LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR Chris@69: CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF Chris@69: SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS Chris@69: INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN Chris@69: CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) Chris@69: ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE Chris@69: POSSIBILITY OF SUCH DAMAGE. Chris@69: ***********************************************************************/ Chris@69: Chris@69: #ifdef HAVE_CONFIG_H Chris@69: #include "config.h" Chris@69: #endif Chris@69: Chris@69: /********************************************************************** Chris@69: * Correlation Matrix Computations for LS estimate. Chris@69: **********************************************************************/ Chris@69: Chris@69: #include "main_FIX.h" Chris@69: Chris@69: /* Calculates correlation vector X'*t */ Chris@69: void silk_corrVector_FIX( Chris@69: const opus_int16 *x, /* I x vector [L + order - 1] used to form data matrix X */ Chris@69: const opus_int16 *t, /* I Target vector [L] */ Chris@69: const opus_int L, /* I Length of vectors */ Chris@69: const opus_int order, /* I Max lag for correlation */ Chris@69: opus_int32 *Xt, /* O Pointer to X'*t correlation vector [order] */ Chris@69: const opus_int rshifts, /* I Right shifts of correlations */ Chris@69: int arch /* I Run-time architecture */ Chris@69: ) Chris@69: { Chris@69: opus_int lag, i; Chris@69: const opus_int16 *ptr1, *ptr2; Chris@69: opus_int32 inner_prod; Chris@69: Chris@69: ptr1 = &x[ order - 1 ]; /* Points to first sample of column 0 of X: X[:,0] */ Chris@69: ptr2 = t; Chris@69: /* Calculate X'*t */ Chris@69: if( rshifts > 0 ) { Chris@69: /* Right shifting used */ Chris@69: for( lag = 0; lag < order; lag++ ) { Chris@69: inner_prod = 0; Chris@69: for( i = 0; i < L; i++ ) { Chris@69: inner_prod = silk_ADD_RSHIFT32( inner_prod, silk_SMULBB( ptr1[ i ], ptr2[i] ), rshifts ); Chris@69: } Chris@69: Xt[ lag ] = inner_prod; /* X[:,lag]'*t */ Chris@69: ptr1--; /* Go to next column of X */ Chris@69: } Chris@69: } else { Chris@69: silk_assert( rshifts == 0 ); Chris@69: for( lag = 0; lag < order; lag++ ) { Chris@69: Xt[ lag ] = silk_inner_prod_aligned( ptr1, ptr2, L, arch ); /* X[:,lag]'*t */ Chris@69: ptr1--; /* Go to next column of X */ Chris@69: } Chris@69: } Chris@69: } Chris@69: Chris@69: /* Calculates correlation matrix X'*X */ Chris@69: void silk_corrMatrix_FIX( Chris@69: const opus_int16 *x, /* I x vector [L + order - 1] used to form data matrix X */ Chris@69: const opus_int L, /* I Length of vectors */ Chris@69: const opus_int order, /* I Max lag for correlation */ Chris@69: opus_int32 *XX, /* O Pointer to X'*X correlation matrix [ order x order ] */ Chris@69: opus_int32 *nrg, /* O Energy of x vector */ Chris@69: opus_int *rshifts, /* O Right shifts of correlations and energy */ Chris@69: int arch /* I Run-time architecture */ Chris@69: ) Chris@69: { Chris@69: opus_int i, j, lag; Chris@69: opus_int32 energy; Chris@69: const opus_int16 *ptr1, *ptr2; Chris@69: Chris@69: /* Calculate energy to find shift used to fit in 32 bits */ Chris@69: silk_sum_sqr_shift( nrg, rshifts, x, L + order - 1 ); Chris@69: energy = *nrg; Chris@69: Chris@69: /* Calculate energy of first column (0) of X: X[:,0]'*X[:,0] */ Chris@69: /* Remove contribution of first order - 1 samples */ Chris@69: for( i = 0; i < order - 1; i++ ) { Chris@69: energy -= silk_RSHIFT32( silk_SMULBB( x[ i ], x[ i ] ), *rshifts ); Chris@69: } Chris@69: Chris@69: /* Calculate energy of remaining columns of X: X[:,j]'*X[:,j] */ Chris@69: /* Fill out the diagonal of the correlation matrix */ Chris@69: matrix_ptr( XX, 0, 0, order ) = energy; Chris@69: silk_assert( energy >= 0 ); Chris@69: ptr1 = &x[ order - 1 ]; /* First sample of column 0 of X */ Chris@69: for( j = 1; j < order; j++ ) { Chris@69: energy = silk_SUB32( energy, silk_RSHIFT32( silk_SMULBB( ptr1[ L - j ], ptr1[ L - j ] ), *rshifts ) ); Chris@69: energy = silk_ADD32( energy, silk_RSHIFT32( silk_SMULBB( ptr1[ -j ], ptr1[ -j ] ), *rshifts ) ); Chris@69: matrix_ptr( XX, j, j, order ) = energy; Chris@69: silk_assert( energy >= 0 ); Chris@69: } Chris@69: Chris@69: ptr2 = &x[ order - 2 ]; /* First sample of column 1 of X */ Chris@69: /* Calculate the remaining elements of the correlation matrix */ Chris@69: if( *rshifts > 0 ) { Chris@69: /* Right shifting used */ Chris@69: for( lag = 1; lag < order; lag++ ) { Chris@69: /* Inner product of column 0 and column lag: X[:,0]'*X[:,lag] */ Chris@69: energy = 0; Chris@69: for( i = 0; i < L; i++ ) { Chris@69: energy += silk_RSHIFT32( silk_SMULBB( ptr1[ i ], ptr2[i] ), *rshifts ); Chris@69: } Chris@69: /* Calculate remaining off diagonal: X[:,j]'*X[:,j + lag] */ Chris@69: matrix_ptr( XX, lag, 0, order ) = energy; Chris@69: matrix_ptr( XX, 0, lag, order ) = energy; Chris@69: for( j = 1; j < ( order - lag ); j++ ) { Chris@69: energy = silk_SUB32( energy, silk_RSHIFT32( silk_SMULBB( ptr1[ L - j ], ptr2[ L - j ] ), *rshifts ) ); Chris@69: energy = silk_ADD32( energy, silk_RSHIFT32( silk_SMULBB( ptr1[ -j ], ptr2[ -j ] ), *rshifts ) ); Chris@69: matrix_ptr( XX, lag + j, j, order ) = energy; Chris@69: matrix_ptr( XX, j, lag + j, order ) = energy; Chris@69: } Chris@69: ptr2--; /* Update pointer to first sample of next column (lag) in X */ Chris@69: } Chris@69: } else { Chris@69: for( lag = 1; lag < order; lag++ ) { Chris@69: /* Inner product of column 0 and column lag: X[:,0]'*X[:,lag] */ Chris@69: energy = silk_inner_prod_aligned( ptr1, ptr2, L, arch ); Chris@69: matrix_ptr( XX, lag, 0, order ) = energy; Chris@69: matrix_ptr( XX, 0, lag, order ) = energy; Chris@69: /* Calculate remaining off diagonal: X[:,j]'*X[:,j + lag] */ Chris@69: for( j = 1; j < ( order - lag ); j++ ) { Chris@69: energy = silk_SUB32( energy, silk_SMULBB( ptr1[ L - j ], ptr2[ L - j ] ) ); Chris@69: energy = silk_SMLABB( energy, ptr1[ -j ], ptr2[ -j ] ); Chris@69: matrix_ptr( XX, lag + j, j, order ) = energy; Chris@69: matrix_ptr( XX, j, lag + j, order ) = energy; Chris@69: } Chris@69: ptr2--;/* Update pointer to first sample of next column (lag) in X */ Chris@69: } Chris@69: } Chris@69: } Chris@69: