cb1aeb76851b61da17c54bc69f3316d8655dd546
[opus.git] / silk / resampler.c
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, (subject to the limitations in the disclaimer below)
5 are permitted provided that the following conditions 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 Skype Limited, nor the names of specific
12 contributors, may be used to endorse or promote products derived from
13 this software without specific prior written permission.
14 NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED
15 BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
16 CONTRIBUTORS ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
17 BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
18 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
19 COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
20 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
21 NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
22 USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
23 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
25 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***********************************************************************/
27
28 #ifdef HAVE_CONFIG_H
29 #include "config.h"
30 #endif
31
32 /* Matrix of resampling methods used:
33  *                                        Fs_out (kHz)
34  *                        8      12     16     24     32     44.1   48
35  *
36  *               8        C      UF     U      UF     UF     UF     UF
37  *              12        AF     C      UF     U      UF     UF     UF
38  *              16        D      AF     C      UF     U      UF     UF
39  * Fs_in (kHz)  24        AIF    D      AF     C      UF     UF     U
40  *              32        UF     AF     D      AF     C      UF     UF
41  *              44.1      AMI    AMI    AMI    AMI    AMI    C      UF
42  *              48        DAF    DAF    AF     D      AF     UF     C
43  *
44  * default method: UF
45  *
46  * C   -> Copy (no resampling)
47  * D   -> Allpass-based 2x downsampling
48  * U   -> Allpass-based 2x upsampling
49  * DAF -> Allpass-based 2x downsampling followed by AR2 filter followed by FIR interpolation
50  * UF  -> Allpass-based 2x upsampling followed by FIR interpolation
51  * AMI -> ARMA4 filter followed by FIR interpolation
52  * AF  -> AR2 filter followed by FIR interpolation
53  *
54  * Input signals sampled above 48 kHz are first downsampled to at most 48 kHz.
55  * Output signals sampled above 48 kHz are upsampled from at most 48 kHz.
56  */
57
58 #include "resampler_private.h"
59
60 /* Greatest common divisor */
61 static opus_int32 gcd(
62     opus_int32 a,
63     opus_int32 b
64 )
65 {
66     opus_int32 tmp;
67     while( b > 0 ) {
68         tmp = a - b * silk_DIV32( a, b );
69         a   = b;
70         b   = tmp;
71     }
72     return a;
73 }
74
75 /* Initialize/reset the resampler state for a given pair of input/output sampling rates */
76 opus_int silk_resampler_init(
77     silk_resampler_state_struct    *S,                    /* I/O: Resampler state             */
78     opus_int32                            Fs_Hz_in,    /* I:    Input sampling rate (Hz)    */
79     opus_int32                            Fs_Hz_out    /* I:    Output sampling rate (Hz)    */
80 )
81 {
82     opus_int32 cycleLen, cyclesPerBatch, up2 = 0, down2 = 0;
83
84     /* Clear state */
85     silk_memset( S, 0, sizeof( silk_resampler_state_struct ) );
86
87     /* Input checking */
88 #if RESAMPLER_SUPPORT_ABOVE_48KHZ
89     if( Fs_Hz_in < 8000 || Fs_Hz_in > 192000 || Fs_Hz_out < 8000 || Fs_Hz_out > 192000 ) {
90 #else
91     if( Fs_Hz_in < 8000 || Fs_Hz_in >  48000 || Fs_Hz_out < 8000 || Fs_Hz_out >  48000 ) {
92 #endif
93         silk_assert( 0 );
94         return -1;
95     }
96
97 #if RESAMPLER_SUPPORT_ABOVE_48KHZ
98     /* Determine pre downsampling and post upsampling */
99     if( Fs_Hz_in > 96000 ) {
100         S->nPreDownsamplers = 2;
101         S->down_pre_function = silk_resampler_private_down4;
102     } else if( Fs_Hz_in > 48000 ) {
103         S->nPreDownsamplers = 1;
104         S->down_pre_function = silk_resampler_down2;
105     } else {
106         S->nPreDownsamplers = 0;
107         S->down_pre_function = NULL;
108     }
109
110     if( Fs_Hz_out > 96000 ) {
111         S->nPostUpsamplers = 2;
112         S->up_post_function = silk_resampler_private_up4;
113     } else if( Fs_Hz_out > 48000 ) {
114         S->nPostUpsamplers = 1;
115         S->up_post_function = silk_resampler_up2;
116     } else {
117         S->nPostUpsamplers = 0;
118         S->up_post_function = NULL;
119     }
120
121     if( S->nPreDownsamplers + S->nPostUpsamplers > 0 ) {
122         /* Ratio of output/input samples */
123         S->ratio_Q16 = silk_LSHIFT32( silk_DIV32( silk_LSHIFT32( Fs_Hz_out, 13 ), Fs_Hz_in ), 3 );
124         /* Make sure the ratio is rounded up */
125         while( silk_SMULWW( S->ratio_Q16, Fs_Hz_in ) < Fs_Hz_out ) S->ratio_Q16++;
126
127         /* Batch size is 10 ms */
128         S->batchSizePrePost = silk_DIV32_16( Fs_Hz_in, 100 );
129
130         /* Convert sampling rate to those after pre-downsampling and before post-upsampling */
131         Fs_Hz_in  = silk_RSHIFT( Fs_Hz_in,  S->nPreDownsamplers  );
132         Fs_Hz_out = silk_RSHIFT( Fs_Hz_out, S->nPostUpsamplers  );
133     }
134 #endif
135
136     /* Number of samples processed per batch */
137     /* First, try 10 ms frames */
138     S->batchSize = silk_DIV32_16( Fs_Hz_in, 100 );
139     if( ( silk_MUL( S->batchSize, 100 ) != Fs_Hz_in ) || ( Fs_Hz_in % 100 != 0 ) ) {
140         /* No integer number of input or output samples with 10 ms frames, use greatest common divisor */
141         cycleLen = silk_DIV32( Fs_Hz_in, gcd( Fs_Hz_in, Fs_Hz_out ) );
142         cyclesPerBatch = silk_DIV32( RESAMPLER_MAX_BATCH_SIZE_IN, cycleLen );
143         if( cyclesPerBatch == 0 ) {
144             /* cycleLen too big, let's just use the maximum batch size. Some distortion will result. */
145             S->batchSize = RESAMPLER_MAX_BATCH_SIZE_IN;
146             silk_assert( 0 );
147         } else {
148             S->batchSize = silk_MUL( cyclesPerBatch, cycleLen );
149         }
150     }
151
152
153     /* Find resampler with the right sampling ratio */
154     if( Fs_Hz_out > Fs_Hz_in ) {
155         /* Upsample */
156         if( Fs_Hz_out == silk_MUL( Fs_Hz_in, 2 ) ) {                             /* Fs_out : Fs_in = 2 : 1 */
157             /* Special case: directly use 2x upsampler */
158             S->resampler_function = silk_resampler_private_up2_HQ_wrapper;
159         } else {
160             /* Default resampler */
161             S->resampler_function = silk_resampler_private_IIR_FIR;
162             up2 = 1;
163             if( Fs_Hz_in > 24000 ) {
164                 /* Low-quality all-pass upsampler */
165                 S->up2_function = silk_resampler_up2;
166             } else {
167                 /* High-quality all-pass upsampler */
168                 S->up2_function = silk_resampler_private_up2_HQ;
169             }
170         }
171     } else if ( Fs_Hz_out < Fs_Hz_in ) {
172         /* Downsample */
173         if( silk_MUL( Fs_Hz_out, 4 ) == silk_MUL( Fs_Hz_in, 3 ) ) {               /* Fs_out : Fs_in = 3 : 4 */
174             S->FIR_Fracs = 3;
175             S->Coefs = silk_Resampler_3_4_COEFS;
176             S->resampler_function = silk_resampler_private_down_FIR;
177         } else if( silk_MUL( Fs_Hz_out, 3 ) == silk_MUL( Fs_Hz_in, 2 ) ) {        /* Fs_out : Fs_in = 2 : 3 */
178             S->FIR_Fracs = 2;
179             S->Coefs = silk_Resampler_2_3_COEFS;
180             S->resampler_function = silk_resampler_private_down_FIR;
181         } else if( silk_MUL( Fs_Hz_out, 2 ) == Fs_Hz_in ) {                      /* Fs_out : Fs_in = 1 : 2 */
182             S->FIR_Fracs = 1;
183             S->Coefs = silk_Resampler_1_2_COEFS;
184             S->resampler_function = silk_resampler_private_down_FIR;
185         } else if( silk_MUL( Fs_Hz_out, 8 ) == silk_MUL( Fs_Hz_in, 3 ) ) {        /* Fs_out : Fs_in = 3 : 8 */
186             S->FIR_Fracs = 3;
187             S->Coefs = silk_Resampler_3_8_COEFS;
188             S->resampler_function = silk_resampler_private_down_FIR;
189         } else if( silk_MUL( Fs_Hz_out, 3 ) == Fs_Hz_in ) {                      /* Fs_out : Fs_in = 1 : 3 */
190             S->FIR_Fracs = 1;
191             S->Coefs = silk_Resampler_1_3_COEFS;
192             S->resampler_function = silk_resampler_private_down_FIR;
193         } else if( silk_MUL( Fs_Hz_out, 4 ) == Fs_Hz_in ) {                      /* Fs_out : Fs_in = 1 : 4 */
194             S->FIR_Fracs = 1;
195             down2 = 1;
196             S->Coefs = silk_Resampler_1_2_COEFS;
197             S->resampler_function = silk_resampler_private_down_FIR;
198         } else if( silk_MUL( Fs_Hz_out, 6 ) == Fs_Hz_in ) {                      /* Fs_out : Fs_in = 1 : 6 */
199             S->FIR_Fracs = 1;
200             down2 = 1;
201             S->Coefs = silk_Resampler_1_3_COEFS;
202             S->resampler_function = silk_resampler_private_down_FIR;
203         } else if( silk_MUL( Fs_Hz_out, 441 ) == silk_MUL( Fs_Hz_in, 80 ) ) {     /* Fs_out : Fs_in = 80 : 441 */
204             S->Coefs = silk_Resampler_80_441_ARMA4_COEFS;
205             S->resampler_function = silk_resampler_private_IIR_FIR;
206         } else if( silk_MUL( Fs_Hz_out, 441 ) == silk_MUL( Fs_Hz_in, 120 ) ) {    /* Fs_out : Fs_in = 120 : 441 */
207             S->Coefs = silk_Resampler_120_441_ARMA4_COEFS;
208             S->resampler_function = silk_resampler_private_IIR_FIR;
209         } else if( silk_MUL( Fs_Hz_out, 441 ) == silk_MUL( Fs_Hz_in, 160 ) ) {    /* Fs_out : Fs_in = 160 : 441 */
210             S->Coefs = silk_Resampler_160_441_ARMA4_COEFS;
211             S->resampler_function = silk_resampler_private_IIR_FIR;
212         } else if( silk_MUL( Fs_Hz_out, 441 ) == silk_MUL( Fs_Hz_in, 240 ) ) {    /* Fs_out : Fs_in = 240 : 441 */
213             S->Coefs = silk_Resampler_240_441_ARMA4_COEFS;
214             S->resampler_function = silk_resampler_private_IIR_FIR;
215         } else if( silk_MUL( Fs_Hz_out, 441 ) == silk_MUL( Fs_Hz_in, 320 ) ) {    /* Fs_out : Fs_in = 320 : 441 */
216             S->Coefs = silk_Resampler_320_441_ARMA4_COEFS;
217             S->resampler_function = silk_resampler_private_IIR_FIR;
218         } else {
219             /* Default resampler */
220             S->resampler_function = silk_resampler_private_IIR_FIR;
221             up2 = 1;
222             if( Fs_Hz_in > 24000 ) {
223                 /* Low-quality all-pass upsampler */
224                 S->up2_function = silk_resampler_up2;
225             } else {
226                 /* High-quality all-pass upsampler */
227                 S->up2_function = silk_resampler_private_up2_HQ;
228             }
229         }
230     } else {
231         /* Input and output sampling rates are equal: copy */
232         S->resampler_function = silk_resampler_private_copy;
233     }
234
235     S->input2x = up2 | down2;
236
237     /* Ratio of input/output samples */
238     S->invRatio_Q16 = silk_LSHIFT32( silk_DIV32( silk_LSHIFT32( Fs_Hz_in, 14 + up2 - down2 ), Fs_Hz_out ), 2 );
239     /* Make sure the ratio is rounded up */
240     while( silk_SMULWW( S->invRatio_Q16, silk_LSHIFT32( Fs_Hz_out, down2 ) ) < silk_LSHIFT32( Fs_Hz_in, up2 ) ) {
241         S->invRatio_Q16++;
242     }
243
244     S->magic_number = 123456789;
245
246     return 0;
247 }
248
249 /* Clear the states of all resampling filters, without resetting sampling rate ratio */
250 opus_int silk_resampler_clear(
251     silk_resampler_state_struct    *S            /* I/O: Resampler state             */
252 )
253 {
254     /* Clear state */
255     silk_memset( S->sDown2, 0, sizeof( S->sDown2 ) );
256     silk_memset( S->sIIR,   0, sizeof( S->sIIR ) );
257     silk_memset( S->sFIR,   0, sizeof( S->sFIR ) );
258 #if RESAMPLER_SUPPORT_ABOVE_48KHZ
259     silk_memset( S->sDownPre, 0, sizeof( S->sDownPre ) );
260     silk_memset( S->sUpPost,  0, sizeof( S->sUpPost ) );
261 #endif
262     return 0;
263 }
264
265 /* Resampler: convert from one sampling rate to another                                 */
266 opus_int silk_resampler(
267     silk_resampler_state_struct    *S,                    /* I/O: Resampler state             */
268     opus_int16                            out[],        /* O:    Output signal                 */
269     const opus_int16                        in[],        /* I:    Input signal                */
270     opus_int32                            inLen        /* I:    Number of input samples        */
271 )
272 {
273     /* Verify that state was initialized and has not been corrupted */
274     if( S->magic_number != 123456789 ) {
275         silk_assert( 0 );
276         return -1;
277     }
278
279 #if RESAMPLER_SUPPORT_ABOVE_48KHZ
280     if( S->nPreDownsamplers + S->nPostUpsamplers > 0 ) {
281         /* The input and/or output sampling rate is above 48000 Hz */
282         opus_int32       nSamplesIn, nSamplesOut;
283         opus_int16        in_buf[ 480 ], out_buf[ 480 ];
284
285         while( inLen > 0 ) {
286             /* Number of input and output samples to process */
287             nSamplesIn = silk_min( inLen, S->batchSizePrePost );
288             nSamplesOut = silk_SMULWB( S->ratio_Q16, nSamplesIn );
289
290             silk_assert( silk_RSHIFT32( nSamplesIn,  S->nPreDownsamplers ) <= 480 );
291             silk_assert( silk_RSHIFT32( nSamplesOut, S->nPostUpsamplers  ) <= 480 );
292
293             if( S->nPreDownsamplers > 0 ) {
294                 S->down_pre_function( S->sDownPre, in_buf, in, nSamplesIn );
295                 if( S->nPostUpsamplers > 0 ) {
296                     S->resampler_function( S, out_buf, in_buf, silk_RSHIFT32( nSamplesIn, S->nPreDownsamplers ) );
297                     S->up_post_function( S->sUpPost, out, out_buf, silk_RSHIFT32( nSamplesOut, S->nPostUpsamplers ) );
298                 } else {
299                     S->resampler_function( S, out, in_buf, silk_RSHIFT32( nSamplesIn, S->nPreDownsamplers ) );
300                 }
301             } else {
302                 S->resampler_function( S, out_buf, in, silk_RSHIFT32( nSamplesIn, S->nPreDownsamplers ) );
303                 S->up_post_function( S->sUpPost, out, out_buf, silk_RSHIFT32( nSamplesOut, S->nPostUpsamplers ) );
304             }
305
306             in += nSamplesIn;
307             out += nSamplesOut;
308             inLen -= nSamplesIn;
309         }
310     } else
311 #endif
312     {
313         /* Input and output sampling rate are at most 48000 Hz */
314         S->resampler_function( S, out, in, inLen );
315     }
316
317     return 0;
318 }