Fixes a fixed-point overflow by scaling pitch search more conservatively
[opus.git] / celt / pitch.c
1 /* Copyright (c) 2007-2008 CSIRO
2    Copyright (c) 2007-2009 Xiph.Org Foundation
3    Written by Jean-Marc Valin */
4 /**
5    @file pitch.c
6    @brief Pitch analysis
7  */
8
9 /*
10    Redistribution and use in source and binary forms, with or without
11    modification, are permitted provided that the following conditions
12    are met:
13
14    - Redistributions of source code must retain the above copyright
15    notice, this list of conditions and the following disclaimer.
16
17    - Redistributions in binary form must reproduce the above copyright
18    notice, this list of conditions and the following disclaimer in the
19    documentation and/or other materials provided with the distribution.
20
21    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
24    A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
25    OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
26    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
27    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
28    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
29    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
30    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
31    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32 */
33
34 #ifdef HAVE_CONFIG_H
35 #include "config.h"
36 #endif
37
38 #include "pitch.h"
39 #include "os_support.h"
40 #include "modes.h"
41 #include "stack_alloc.h"
42 #include "mathops.h"
43 #include "celt_lpc.h"
44
45 static void find_best_pitch(opus_val32 *xcorr, opus_val16 *y, int len,
46                             int max_pitch, int *best_pitch
47 #ifdef FIXED_POINT
48                             , int yshift, opus_val32 maxcorr
49 #endif
50                             )
51 {
52    int i, j;
53    opus_val32 Syy=1;
54    opus_val16 best_num[2];
55    opus_val32 best_den[2];
56 #ifdef FIXED_POINT
57    int xshift;
58
59    xshift = celt_ilog2(maxcorr)-14;
60 #endif
61
62    best_num[0] = -1;
63    best_num[1] = -1;
64    best_den[0] = 0;
65    best_den[1] = 0;
66    best_pitch[0] = 0;
67    best_pitch[1] = 1;
68    for (j=0;j<len;j++)
69       Syy = ADD32(Syy, SHR32(MULT16_16(y[j],y[j]), yshift));
70    for (i=0;i<max_pitch;i++)
71    {
72       if (xcorr[i]>0)
73       {
74          opus_val16 num;
75          opus_val32 xcorr16;
76          xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift));
77 #ifndef FIXED_POINT
78          /* Considering the range of xcorr16, this should avoid both underflows
79             and overflows (inf) when squaring xcorr16 */
80          xcorr16 *= 1e-12;
81 #endif
82          num = MULT16_16_Q15(xcorr16,xcorr16);
83          if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy))
84          {
85             if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy))
86             {
87                best_num[1] = best_num[0];
88                best_den[1] = best_den[0];
89                best_pitch[1] = best_pitch[0];
90                best_num[0] = num;
91                best_den[0] = Syy;
92                best_pitch[0] = i;
93             } else {
94                best_num[1] = num;
95                best_den[1] = Syy;
96                best_pitch[1] = i;
97             }
98          }
99       }
100       Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift);
101       Syy = MAX32(1, Syy);
102    }
103 }
104
105 void pitch_downsample(celt_sig * restrict x[], opus_val16 * restrict x_lp,
106       int len, int C)
107 {
108    int i;
109    opus_val32 ac[5];
110    opus_val16 tmp=Q15ONE;
111    opus_val16 lpc[4], mem[4]={0,0,0,0};
112 #ifdef FIXED_POINT
113    int shift;
114    opus_val32 maxabs = MAX32(1, celt_maxabs32(x[0], len));
115    if (C==2)
116       maxabs = MAX32(maxabs, celt_maxabs32(x[1], len));
117    shift = IMAX(0,celt_ilog2(maxabs)-10);
118    if (C==2)
119       shift++;
120 #endif
121    for (i=1;i<len>>1;i++)
122       x_lp[i] = SHR32(HALF32(HALF32(x[0][(2*i-1)]+x[0][(2*i+1)])+x[0][2*i]), shift);
123    x_lp[0] = SHR32(HALF32(HALF32(x[0][1])+x[0][0]), shift);
124    if (C==2)
125    {
126       for (i=1;i<len>>1;i++)
127          x_lp[i] += SHR32(HALF32(HALF32(x[1][(2*i-1)]+x[1][(2*i+1)])+x[1][2*i]), shift);
128       x_lp[0] += SHR32(HALF32(HALF32(x[1][1])+x[1][0]), shift);
129    }
130
131    _celt_autocorr(x_lp, ac, NULL, 0,
132                   4, len>>1);
133
134    /* Noise floor -40 dB */
135 #ifdef FIXED_POINT
136    ac[0] += SHR32(ac[0],13);
137 #else
138    ac[0] *= 1.0001f;
139 #endif
140    /* Lag windowing */
141    for (i=1;i<=4;i++)
142    {
143       /*ac[i] *= exp(-.5*(2*M_PI*.002*i)*(2*M_PI*.002*i));*/
144 #ifdef FIXED_POINT
145       ac[i] -= MULT16_32_Q15(2*i*i, ac[i]);
146 #else
147       ac[i] -= ac[i]*(.008f*i)*(.008f*i);
148 #endif
149    }
150
151    _celt_lpc(lpc, ac, 4);
152    for (i=0;i<4;i++)
153    {
154       tmp = MULT16_16_Q15(QCONST16(.9f,15), tmp);
155       lpc[i] = MULT16_16_Q15(lpc[i], tmp);
156    }
157    celt_fir(x_lp, lpc, x_lp, len>>1, 4, mem);
158
159    mem[0]=0;
160    lpc[0]=QCONST16(.8f,12);
161    celt_fir(x_lp, lpc, x_lp, len>>1, 1, mem);
162
163 }
164
165 void pitch_search(const opus_val16 * restrict x_lp, opus_val16 * restrict y,
166                   int len, int max_pitch, int *pitch)
167 {
168    int i, j;
169    int lag;
170    int best_pitch[2]={0,0};
171    VARDECL(opus_val16, x_lp4);
172    VARDECL(opus_val16, y_lp4);
173    VARDECL(opus_val32, xcorr);
174 #ifdef FIXED_POINT
175    opus_val32 maxcorr=1;
176    int shift=0;
177 #endif
178    int offset;
179
180    SAVE_STACK;
181
182    celt_assert(len>0);
183    celt_assert(max_pitch>0);
184    lag = len+max_pitch;
185
186    ALLOC(x_lp4, len>>2, opus_val16);
187    ALLOC(y_lp4, lag>>2, opus_val16);
188    ALLOC(xcorr, max_pitch>>1, opus_val32);
189
190    /* Downsample by 2 again */
191    for (j=0;j<len>>2;j++)
192       x_lp4[j] = x_lp[2*j];
193    for (j=0;j<lag>>2;j++)
194       y_lp4[j] = y[2*j];
195
196 #ifdef FIXED_POINT
197    shift = celt_ilog2(MAX16(1, MAX16(celt_maxabs16(x_lp4, len>>2), celt_maxabs16(y_lp4, lag>>2))))-11;
198    if (shift>0)
199    {
200       for (j=0;j<len>>2;j++)
201          x_lp4[j] = SHR16(x_lp4[j], shift);
202       for (j=0;j<lag>>2;j++)
203          y_lp4[j] = SHR16(y_lp4[j], shift);
204       /* Use double the shift for a MAC */
205       shift *= 2;
206    } else {
207       shift = 0;
208    }
209 #endif
210
211    /* Coarse search with 4x decimation */
212
213    for (i=0;i<max_pitch>>2;i++)
214    {
215       opus_val32 sum = 0;
216       for (j=0;j<len>>2;j++)
217          sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]);
218       xcorr[i] = MAX32(-1, sum);
219 #ifdef FIXED_POINT
220       maxcorr = MAX32(maxcorr, sum);
221 #endif
222    }
223    find_best_pitch(xcorr, y_lp4, len>>2, max_pitch>>2, best_pitch
224 #ifdef FIXED_POINT
225                    , 0, maxcorr
226 #endif
227                    );
228
229    /* Finer search with 2x decimation */
230 #ifdef FIXED_POINT
231    maxcorr=1;
232 #endif
233    for (i=0;i<max_pitch>>1;i++)
234    {
235       opus_val32 sum=0;
236       xcorr[i] = 0;
237       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
238          continue;
239       for (j=0;j<len>>1;j++)
240          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
241       xcorr[i] = MAX32(-1, sum);
242 #ifdef FIXED_POINT
243       maxcorr = MAX32(maxcorr, sum);
244 #endif
245    }
246    find_best_pitch(xcorr, y, len>>1, max_pitch>>1, best_pitch
247 #ifdef FIXED_POINT
248                    , shift+1, maxcorr
249 #endif
250                    );
251
252    /* Refine by pseudo-interpolation */
253    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
254    {
255       opus_val32 a, b, c;
256       a = xcorr[best_pitch[0]-1];
257       b = xcorr[best_pitch[0]];
258       c = xcorr[best_pitch[0]+1];
259       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
260          offset = 1;
261       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
262          offset = -1;
263       else
264          offset = 0;
265    } else {
266       offset = 0;
267    }
268    *pitch = 2*best_pitch[0]-offset;
269
270    RESTORE_STACK;
271 }
272
273 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2};
274 opus_val16 remove_doubling(opus_val16 *x, int maxperiod, int minperiod,
275       int N, int *T0_, int prev_period, opus_val16 prev_gain)
276 {
277    int k, i, T, T0;
278    opus_val16 g, g0;
279    opus_val16 pg;
280    opus_val32 xy,xx,yy;
281    opus_val32 xcorr[3];
282    opus_val32 best_xy, best_yy;
283    int offset;
284    int minperiod0;
285
286    minperiod0 = minperiod;
287    maxperiod /= 2;
288    minperiod /= 2;
289    *T0_ /= 2;
290    prev_period /= 2;
291    N /= 2;
292    x += maxperiod;
293    if (*T0_>=maxperiod)
294       *T0_=maxperiod-1;
295
296    T = T0 = *T0_;
297    xx=xy=yy=0;
298    for (i=0;i<N;i++)
299    {
300       xy = MAC16_16(xy, x[i], x[i-T0]);
301       xx = MAC16_16(xx, x[i], x[i]);
302       yy = MAC16_16(yy, x[i-T0],x[i-T0]);
303    }
304    best_xy = xy;
305    best_yy = yy;
306 #ifdef FIXED_POINT
307       {
308          opus_val32 x2y2;
309          int sh, t;
310          x2y2 = 1+HALF32(MULT32_32_Q31(xx,yy));
311          sh = celt_ilog2(x2y2)>>1;
312          t = VSHR32(x2y2, 2*(sh-7));
313          g = g0 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
314       }
315 #else
316       g = g0 = xy/celt_sqrt(1+xx*yy);
317 #endif
318    /* Look for any pitch at T/k */
319    for (k=2;k<=15;k++)
320    {
321       int T1, T1b;
322       opus_val16 g1;
323       opus_val16 cont=0;
324       T1 = (2*T0+k)/(2*k);
325       if (T1 < minperiod)
326          break;
327       /* Look for another strong correlation at T1b */
328       if (k==2)
329       {
330          if (T1+T0>maxperiod)
331             T1b = T0;
332          else
333             T1b = T0+T1;
334       } else
335       {
336          T1b = (2*second_check[k]*T0+k)/(2*k);
337       }
338       xy=yy=0;
339       for (i=0;i<N;i++)
340       {
341          xy = MAC16_16(xy, x[i], x[i-T1]);
342          yy = MAC16_16(yy, x[i-T1], x[i-T1]);
343
344          xy = MAC16_16(xy, x[i], x[i-T1b]);
345          yy = MAC16_16(yy, x[i-T1b], x[i-T1b]);
346       }
347 #ifdef FIXED_POINT
348       {
349          opus_val32 x2y2;
350          int sh, t;
351          x2y2 = 1+MULT32_32_Q31(xx,yy);
352          sh = celt_ilog2(x2y2)>>1;
353          t = VSHR32(x2y2, 2*(sh-7));
354          g1 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
355       }
356 #else
357       g1 = xy/celt_sqrt(1+2.f*xx*1.f*yy);
358 #endif
359       if (abs(T1-prev_period)<=1)
360          cont = prev_gain;
361       else if (abs(T1-prev_period)<=2 && 5*k*k < T0)
362          cont = HALF32(prev_gain);
363       else
364          cont = 0;
365       if (g1 > QCONST16(.3f,15) + MULT16_16_Q15(QCONST16(.4f,15),g0)-cont)
366       {
367          best_xy = xy;
368          best_yy = yy;
369          T = T1;
370          g = g1;
371       }
372    }
373    best_xy = MAX32(0, best_xy);
374    if (best_yy <= best_xy)
375       pg = Q15ONE;
376    else
377       pg = SHR32(frac_div32(best_xy,best_yy+1),16);
378
379    for (k=0;k<3;k++)
380    {
381       int T1 = T+k-1;
382       xy = 0;
383       for (i=0;i<N;i++)
384          xy = MAC16_16(xy, x[i], x[i-T1]);
385       xcorr[k] = xy;
386    }
387    if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0]))
388       offset = 1;
389    else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2]))
390       offset = -1;
391    else
392       offset = 0;
393    if (pg > g)
394       pg = g;
395    *T0_ = 2*T+offset;
396
397    if (*T0_<minperiod0)
398       *T0_=minperiod0;
399    return pg;
400 }