Alexandre Lision | 744f742 | 2013-09-25 11:39:37 -0400 | [diff] [blame] | 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, are permitted provided that the following conditions |
| 5 | 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 Internet Society, IETF or IETF Trust, nor the |
| 12 | names of specific contributors, may be used to endorse or promote |
| 13 | products derived from this software without specific prior written |
| 14 | permission. |
| 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” |
| 16 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| 17 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| 18 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| 19 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| 20 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| 21 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| 22 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| 23 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| 24 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 25 | POSSIBILITY OF SUCH DAMAGE. |
| 26 | ***********************************************************************/ |
| 27 | |
| 28 | #ifdef HAVE_CONFIG_H |
| 29 | #include "config.h" |
| 30 | #endif |
| 31 | |
| 32 | #include "SigProc_FIX.h" |
| 33 | #include "resampler_private.h" |
| 34 | |
| 35 | static inline opus_int16 *silk_resampler_private_down_FIR_INTERPOL( |
| 36 | opus_int16 *out, |
| 37 | opus_int32 *buf, |
| 38 | const opus_int16 *FIR_Coefs, |
| 39 | opus_int FIR_Order, |
| 40 | opus_int FIR_Fracs, |
| 41 | opus_int32 max_index_Q16, |
| 42 | opus_int32 index_increment_Q16 |
| 43 | ) |
| 44 | { |
| 45 | opus_int32 index_Q16, res_Q6; |
| 46 | opus_int32 *buf_ptr; |
| 47 | opus_int32 interpol_ind; |
| 48 | const opus_int16 *interpol_ptr; |
| 49 | |
| 50 | switch( FIR_Order ) { |
| 51 | case RESAMPLER_DOWN_ORDER_FIR0: |
| 52 | for( index_Q16 = 0; index_Q16 < max_index_Q16; index_Q16 += index_increment_Q16 ) { |
| 53 | /* Integer part gives pointer to buffered input */ |
| 54 | buf_ptr = buf + silk_RSHIFT( index_Q16, 16 ); |
| 55 | |
| 56 | /* Fractional part gives interpolation coefficients */ |
| 57 | interpol_ind = silk_SMULWB( index_Q16 & 0xFFFF, FIR_Fracs ); |
| 58 | |
| 59 | /* Inner product */ |
| 60 | interpol_ptr = &FIR_Coefs[ RESAMPLER_DOWN_ORDER_FIR0 / 2 * interpol_ind ]; |
| 61 | res_Q6 = silk_SMULWB( buf_ptr[ 0 ], interpol_ptr[ 0 ] ); |
| 62 | res_Q6 = silk_SMLAWB( res_Q6, buf_ptr[ 1 ], interpol_ptr[ 1 ] ); |
| 63 | res_Q6 = silk_SMLAWB( res_Q6, buf_ptr[ 2 ], interpol_ptr[ 2 ] ); |
| 64 | res_Q6 = silk_SMLAWB( res_Q6, buf_ptr[ 3 ], interpol_ptr[ 3 ] ); |
| 65 | res_Q6 = silk_SMLAWB( res_Q6, buf_ptr[ 4 ], interpol_ptr[ 4 ] ); |
| 66 | res_Q6 = silk_SMLAWB( res_Q6, buf_ptr[ 5 ], interpol_ptr[ 5 ] ); |
| 67 | res_Q6 = silk_SMLAWB( res_Q6, buf_ptr[ 6 ], interpol_ptr[ 6 ] ); |
| 68 | res_Q6 = silk_SMLAWB( res_Q6, buf_ptr[ 7 ], interpol_ptr[ 7 ] ); |
| 69 | res_Q6 = silk_SMLAWB( res_Q6, buf_ptr[ 8 ], interpol_ptr[ 8 ] ); |
| 70 | interpol_ptr = &FIR_Coefs[ RESAMPLER_DOWN_ORDER_FIR0 / 2 * ( FIR_Fracs - 1 - interpol_ind ) ]; |
| 71 | res_Q6 = silk_SMLAWB( res_Q6, buf_ptr[ 17 ], interpol_ptr[ 0 ] ); |
| 72 | res_Q6 = silk_SMLAWB( res_Q6, buf_ptr[ 16 ], interpol_ptr[ 1 ] ); |
| 73 | res_Q6 = silk_SMLAWB( res_Q6, buf_ptr[ 15 ], interpol_ptr[ 2 ] ); |
| 74 | res_Q6 = silk_SMLAWB( res_Q6, buf_ptr[ 14 ], interpol_ptr[ 3 ] ); |
| 75 | res_Q6 = silk_SMLAWB( res_Q6, buf_ptr[ 13 ], interpol_ptr[ 4 ] ); |
| 76 | res_Q6 = silk_SMLAWB( res_Q6, buf_ptr[ 12 ], interpol_ptr[ 5 ] ); |
| 77 | res_Q6 = silk_SMLAWB( res_Q6, buf_ptr[ 11 ], interpol_ptr[ 6 ] ); |
| 78 | res_Q6 = silk_SMLAWB( res_Q6, buf_ptr[ 10 ], interpol_ptr[ 7 ] ); |
| 79 | res_Q6 = silk_SMLAWB( res_Q6, buf_ptr[ 9 ], interpol_ptr[ 8 ] ); |
| 80 | |
| 81 | /* Scale down, saturate and store in output array */ |
| 82 | *out++ = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND( res_Q6, 6 ) ); |
| 83 | } |
| 84 | break; |
| 85 | case RESAMPLER_DOWN_ORDER_FIR1: |
| 86 | for( index_Q16 = 0; index_Q16 < max_index_Q16; index_Q16 += index_increment_Q16 ) { |
| 87 | /* Integer part gives pointer to buffered input */ |
| 88 | buf_ptr = buf + silk_RSHIFT( index_Q16, 16 ); |
| 89 | |
| 90 | /* Inner product */ |
| 91 | res_Q6 = silk_SMULWB( silk_ADD32( buf_ptr[ 0 ], buf_ptr[ 23 ] ), FIR_Coefs[ 0 ] ); |
| 92 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 1 ], buf_ptr[ 22 ] ), FIR_Coefs[ 1 ] ); |
| 93 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 2 ], buf_ptr[ 21 ] ), FIR_Coefs[ 2 ] ); |
| 94 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 3 ], buf_ptr[ 20 ] ), FIR_Coefs[ 3 ] ); |
| 95 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 4 ], buf_ptr[ 19 ] ), FIR_Coefs[ 4 ] ); |
| 96 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 5 ], buf_ptr[ 18 ] ), FIR_Coefs[ 5 ] ); |
| 97 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 6 ], buf_ptr[ 17 ] ), FIR_Coefs[ 6 ] ); |
| 98 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 7 ], buf_ptr[ 16 ] ), FIR_Coefs[ 7 ] ); |
| 99 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 8 ], buf_ptr[ 15 ] ), FIR_Coefs[ 8 ] ); |
| 100 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 9 ], buf_ptr[ 14 ] ), FIR_Coefs[ 9 ] ); |
| 101 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 10 ], buf_ptr[ 13 ] ), FIR_Coefs[ 10 ] ); |
| 102 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 11 ], buf_ptr[ 12 ] ), FIR_Coefs[ 11 ] ); |
| 103 | |
| 104 | /* Scale down, saturate and store in output array */ |
| 105 | *out++ = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND( res_Q6, 6 ) ); |
| 106 | } |
| 107 | break; |
| 108 | case RESAMPLER_DOWN_ORDER_FIR2: |
| 109 | for( index_Q16 = 0; index_Q16 < max_index_Q16; index_Q16 += index_increment_Q16 ) { |
| 110 | /* Integer part gives pointer to buffered input */ |
| 111 | buf_ptr = buf + silk_RSHIFT( index_Q16, 16 ); |
| 112 | |
| 113 | /* Inner product */ |
| 114 | res_Q6 = silk_SMULWB( silk_ADD32( buf_ptr[ 0 ], buf_ptr[ 35 ] ), FIR_Coefs[ 0 ] ); |
| 115 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 1 ], buf_ptr[ 34 ] ), FIR_Coefs[ 1 ] ); |
| 116 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 2 ], buf_ptr[ 33 ] ), FIR_Coefs[ 2 ] ); |
| 117 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 3 ], buf_ptr[ 32 ] ), FIR_Coefs[ 3 ] ); |
| 118 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 4 ], buf_ptr[ 31 ] ), FIR_Coefs[ 4 ] ); |
| 119 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 5 ], buf_ptr[ 30 ] ), FIR_Coefs[ 5 ] ); |
| 120 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 6 ], buf_ptr[ 29 ] ), FIR_Coefs[ 6 ] ); |
| 121 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 7 ], buf_ptr[ 28 ] ), FIR_Coefs[ 7 ] ); |
| 122 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 8 ], buf_ptr[ 27 ] ), FIR_Coefs[ 8 ] ); |
| 123 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 9 ], buf_ptr[ 26 ] ), FIR_Coefs[ 9 ] ); |
| 124 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 10 ], buf_ptr[ 25 ] ), FIR_Coefs[ 10 ] ); |
| 125 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 11 ], buf_ptr[ 24 ] ), FIR_Coefs[ 11 ] ); |
| 126 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 12 ], buf_ptr[ 23 ] ), FIR_Coefs[ 12 ] ); |
| 127 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 13 ], buf_ptr[ 22 ] ), FIR_Coefs[ 13 ] ); |
| 128 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 14 ], buf_ptr[ 21 ] ), FIR_Coefs[ 14 ] ); |
| 129 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 15 ], buf_ptr[ 20 ] ), FIR_Coefs[ 15 ] ); |
| 130 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 16 ], buf_ptr[ 19 ] ), FIR_Coefs[ 16 ] ); |
| 131 | res_Q6 = silk_SMLAWB( res_Q6, silk_ADD32( buf_ptr[ 17 ], buf_ptr[ 18 ] ), FIR_Coefs[ 17 ] ); |
| 132 | |
| 133 | /* Scale down, saturate and store in output array */ |
| 134 | *out++ = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND( res_Q6, 6 ) ); |
| 135 | } |
| 136 | break; |
| 137 | default: |
| 138 | silk_assert( 0 ); |
| 139 | } |
| 140 | return out; |
| 141 | } |
| 142 | |
| 143 | /* Resample with a 2nd order AR filter followed by FIR interpolation */ |
| 144 | void silk_resampler_private_down_FIR( |
| 145 | void *SS, /* I/O Resampler state */ |
| 146 | opus_int16 out[], /* O Output signal */ |
| 147 | const opus_int16 in[], /* I Input signal */ |
| 148 | opus_int32 inLen /* I Number of input samples */ |
| 149 | ) |
| 150 | { |
| 151 | silk_resampler_state_struct *S = (silk_resampler_state_struct *)SS; |
| 152 | opus_int32 nSamplesIn; |
| 153 | opus_int32 max_index_Q16, index_increment_Q16; |
| 154 | opus_int32 buf[ RESAMPLER_MAX_BATCH_SIZE_IN + SILK_RESAMPLER_MAX_FIR_ORDER ]; |
| 155 | const opus_int16 *FIR_Coefs; |
| 156 | |
| 157 | /* Copy buffered samples to start of buffer */ |
| 158 | silk_memcpy( buf, S->sFIR.i32, S->FIR_Order * sizeof( opus_int32 ) ); |
| 159 | |
| 160 | FIR_Coefs = &S->Coefs[ 2 ]; |
| 161 | |
| 162 | /* Iterate over blocks of frameSizeIn input samples */ |
| 163 | index_increment_Q16 = S->invRatio_Q16; |
| 164 | while( 1 ) { |
| 165 | nSamplesIn = silk_min( inLen, S->batchSize ); |
| 166 | |
| 167 | /* Second-order AR filter (output in Q8) */ |
| 168 | silk_resampler_private_AR2( S->sIIR, &buf[ S->FIR_Order ], in, S->Coefs, nSamplesIn ); |
| 169 | |
| 170 | max_index_Q16 = silk_LSHIFT32( nSamplesIn, 16 ); |
| 171 | |
| 172 | /* Interpolate filtered signal */ |
| 173 | out = silk_resampler_private_down_FIR_INTERPOL( out, buf, FIR_Coefs, S->FIR_Order, |
| 174 | S->FIR_Fracs, max_index_Q16, index_increment_Q16 ); |
| 175 | |
| 176 | in += nSamplesIn; |
| 177 | inLen -= nSamplesIn; |
| 178 | |
| 179 | if( inLen > 1 ) { |
| 180 | /* More iterations to do; copy last part of filtered signal to beginning of buffer */ |
| 181 | silk_memcpy( buf, &buf[ nSamplesIn ], S->FIR_Order * sizeof( opus_int32 ) ); |
| 182 | } else { |
| 183 | break; |
| 184 | } |
| 185 | } |
| 186 | |
| 187 | /* Copy last part of filtered signal to the state for the next call */ |
| 188 | silk_memcpy( S->sFIR.i32, &buf[ nSamplesIn ], S->FIR_Order * sizeof( opus_int32 ) ); |
| 189 | } |