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