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