Make pitch_xcorr() work when len and max_pitch aren't multiples of 4.
[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-12f;
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 * OPUS_RESTRICT x[], opus_val16 * OPUS_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 = celt_maxabs32(x[0], len);
115    if (C==2)
116    {
117       opus_val32 maxabs_1 = celt_maxabs32(x[1], len);
118       maxabs = MAX32(maxabs, maxabs_1);
119    }
120    if (maxabs<1)
121       maxabs=1;
122    shift = celt_ilog2(maxabs)-10;
123    if (shift<0)
124       shift=0;
125    if (C==2)
126       shift++;
127 #endif
128    for (i=1;i<len>>1;i++)
129       x_lp[i] = SHR32(HALF32(HALF32(x[0][(2*i-1)]+x[0][(2*i+1)])+x[0][2*i]), shift);
130    x_lp[0] = SHR32(HALF32(HALF32(x[0][1])+x[0][0]), shift);
131    if (C==2)
132    {
133       for (i=1;i<len>>1;i++)
134          x_lp[i] += SHR32(HALF32(HALF32(x[1][(2*i-1)]+x[1][(2*i+1)])+x[1][2*i]), shift);
135       x_lp[0] += SHR32(HALF32(HALF32(x[1][1])+x[1][0]), shift);
136    }
137
138    _celt_autocorr(x_lp, ac, NULL, 0,
139                   4, len>>1);
140
141    /* Noise floor -40 dB */
142 #ifdef FIXED_POINT
143    ac[0] += SHR32(ac[0],13);
144 #else
145    ac[0] *= 1.0001f;
146 #endif
147    /* Lag windowing */
148    for (i=1;i<=4;i++)
149    {
150       /*ac[i] *= exp(-.5*(2*M_PI*.002*i)*(2*M_PI*.002*i));*/
151 #ifdef FIXED_POINT
152       ac[i] -= MULT16_32_Q15(2*i*i, ac[i]);
153 #else
154       ac[i] -= ac[i]*(.008f*i)*(.008f*i);
155 #endif
156    }
157
158    _celt_lpc(lpc, ac, 4);
159    for (i=0;i<4;i++)
160    {
161       tmp = MULT16_16_Q15(QCONST16(.9f,15), tmp);
162       lpc[i] = MULT16_16_Q15(lpc[i], tmp);
163    }
164    celt_fir(x_lp, lpc, x_lp, len>>1, 4, mem);
165
166    mem[0]=0;
167    lpc[0]=QCONST16(.8f,12);
168    celt_fir(x_lp, lpc, x_lp, len>>1, 1, mem);
169
170 }
171
172 #if 0 /* This is a simple version of the pitch correlation that should work
173          well on DSPs like Blackfin and TI C5x/C6x */
174
175 static void pitch_xcorr(opus_val16 *x, opus_val16 *y, opus_val32 *xcorr, int len, int max_pitch
176 #ifdef FIXED_POINT
177       ,opus_val32 *maxval
178 #endif
179       )
180 {
181    int i, j;
182 #ifdef FIXED_POINT
183    opus_val32 maxcorr=1;
184 #endif
185    for (i=0;i<max_pitch;i++)
186    {
187       opus_val32 sum = 0;
188       for (j=0;j<len;j++)
189          sum = MAC16_16(sum, x[j],y[i+j]);
190       xcorr[i] = MAX32(-1, sum);
191 #ifdef FIXED_POINT
192       maxcorr = MAX32(maxcorr, sum);
193 #endif
194    }
195 #ifdef FIXED_POINT
196    *maxval = maxcorr;
197 #endif
198 }
199
200 #else /* Unrolled version of the pitch correlation -- runs faster on x86 and ARM */
201
202 static void pitch_xcorr(opus_val16 *_x, opus_val16 *_y, opus_val32 *xcorr, int len, int max_pitch
203 #ifdef FIXED_POINT
204       ,opus_val32 *maxval
205 #endif
206       )
207 {
208    int i,j;
209 #ifdef FIXED_POINT
210    opus_val32 maxcorr=1;
211 #endif
212    /* Truncate slightly if len is not a multiple of 4. */
213    len -= len&3;
214    for (i=0;i<max_pitch-3;i+=4)
215    {
216       /* Compute correlation*/
217       /*corr[nb_pitch-1-i]=inner_prod(x, _y+i, len);*/
218       opus_val32 sum1=0;
219       opus_val32 sum2=0;
220       opus_val32 sum3=0;
221       opus_val32 sum4=0;
222       const opus_val16 *y = _y+i;
223       const opus_val16 *x = _x;
224       opus_val16 y0, y1, y2, y3;
225       /*y0=y[0];y1=y[1];y2=y[2];y3=y[3];*/
226       y0=*y++;
227       y1=*y++;
228       y2=*y++;
229       for (j=0;j<len;j+=4)
230       {
231          opus_val16 tmp;
232          tmp = *x++;
233          y3=*y++;
234          sum1 = MAC16_16(sum1,tmp,y0);
235          sum2 = MAC16_16(sum2,tmp,y1);
236          sum3 = MAC16_16(sum3,tmp,y2);
237          sum4 = MAC16_16(sum4,tmp,y3);
238          tmp=*x++;
239          y0=*y++;
240          sum1 = MAC16_16(sum1,tmp,y1);
241          sum2 = MAC16_16(sum2,tmp,y2);
242          sum3 = MAC16_16(sum3,tmp,y3);
243          sum4 = MAC16_16(sum4,tmp,y0);
244          tmp=*x++;
245          y1=*y++;
246          sum1 = MAC16_16(sum1,tmp,y2);
247          sum2 = MAC16_16(sum2,tmp,y3);
248          sum3 = MAC16_16(sum3,tmp,y0);
249          sum4 = MAC16_16(sum4,tmp,y1);
250          tmp=*x++;
251          y2=*y++;
252          sum1 = MAC16_16(sum1,tmp,y3);
253          sum2 = MAC16_16(sum2,tmp,y0);
254          sum3 = MAC16_16(sum3,tmp,y1);
255          sum4 = MAC16_16(sum4,tmp,y2);
256       }
257       xcorr[i]=MAX32(-1, sum1);
258       xcorr[i+1]=MAX32(-1, sum2);
259       xcorr[i+2]=MAX32(-1, sum3);
260       xcorr[i+3]=MAX32(-1, sum4);
261 #ifdef FIXED_POINT
262       sum1 = MAX32(sum1, sum2);
263       sum3 = MAX32(sum3, sum4);
264       sum1 = MAX32(sum1, sum3);
265       maxcorr = MAX32(maxcorr, sum1);
266 #endif
267    }
268    /* In case max_pitch isn't a multiple of 4, do non-unrolled version. */
269    for (;i<max_pitch;i++)
270    {
271       opus_val32 sum = 0;
272       for (j=0;j<len;j++)
273          sum = MAC16_16(sum, _x[j],_y[i+j]);
274       xcorr[i] = MAX32(-1, sum);
275 #ifdef FIXED_POINT
276       maxcorr = MAX32(maxcorr, sum);
277 #endif
278    }
279 #ifdef FIXED_POINT
280    *maxval = maxcorr;
281 #endif
282 }
283
284 #endif
285 void pitch_search(const opus_val16 * OPUS_RESTRICT x_lp, opus_val16 * OPUS_RESTRICT y,
286                   int len, int max_pitch, int *pitch)
287 {
288    int i, j;
289    int lag;
290    int best_pitch[2]={0,0};
291    VARDECL(opus_val16, x_lp4);
292    VARDECL(opus_val16, y_lp4);
293    VARDECL(opus_val32, xcorr);
294 #ifdef FIXED_POINT
295    opus_val32 maxcorr;
296    opus_val32 xmax, ymax;
297    int shift=0;
298 #endif
299    int offset;
300
301    SAVE_STACK;
302
303    celt_assert(len>0);
304    celt_assert(max_pitch>0);
305    lag = len+max_pitch;
306
307    ALLOC(x_lp4, len>>2, opus_val16);
308    ALLOC(y_lp4, lag>>2, opus_val16);
309    ALLOC(xcorr, max_pitch>>1, opus_val32);
310
311    /* Downsample by 2 again */
312    for (j=0;j<len>>2;j++)
313       x_lp4[j] = x_lp[2*j];
314    for (j=0;j<lag>>2;j++)
315       y_lp4[j] = y[2*j];
316
317 #ifdef FIXED_POINT
318    xmax = celt_maxabs16(x_lp4, len>>2);
319    ymax = celt_maxabs16(y_lp4, lag>>2);
320    shift = celt_ilog2(MAX32(1, MAX32(xmax, ymax)))-11;
321    if (shift>0)
322    {
323       for (j=0;j<len>>2;j++)
324          x_lp4[j] = SHR16(x_lp4[j], shift);
325       for (j=0;j<lag>>2;j++)
326          y_lp4[j] = SHR16(y_lp4[j], shift);
327       /* Use double the shift for a MAC */
328       shift *= 2;
329    } else {
330       shift = 0;
331    }
332 #endif
333
334    /* Coarse search with 4x decimation */
335
336    pitch_xcorr(x_lp4, y_lp4, xcorr, len>>2, max_pitch>>2
337 #ifdef FIXED_POINT
338          ,&maxcorr
339 #endif
340          );
341
342    find_best_pitch(xcorr, y_lp4, len>>2, max_pitch>>2, best_pitch
343 #ifdef FIXED_POINT
344                    , 0, maxcorr
345 #endif
346                    );
347
348    /* Finer search with 2x decimation */
349 #ifdef FIXED_POINT
350    maxcorr=1;
351 #endif
352    for (i=0;i<max_pitch>>1;i++)
353    {
354       opus_val32 sum=0;
355       xcorr[i] = 0;
356       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
357          continue;
358       for (j=0;j<len>>1;j++)
359          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
360       xcorr[i] = MAX32(-1, sum);
361 #ifdef FIXED_POINT
362       maxcorr = MAX32(maxcorr, sum);
363 #endif
364    }
365    find_best_pitch(xcorr, y, len>>1, max_pitch>>1, best_pitch
366 #ifdef FIXED_POINT
367                    , shift+1, maxcorr
368 #endif
369                    );
370
371    /* Refine by pseudo-interpolation */
372    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
373    {
374       opus_val32 a, b, c;
375       a = xcorr[best_pitch[0]-1];
376       b = xcorr[best_pitch[0]];
377       c = xcorr[best_pitch[0]+1];
378       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
379          offset = 1;
380       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
381          offset = -1;
382       else
383          offset = 0;
384    } else {
385       offset = 0;
386    }
387    *pitch = 2*best_pitch[0]-offset;
388
389    RESTORE_STACK;
390 }
391
392 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2};
393 opus_val16 remove_doubling(opus_val16 *x, int maxperiod, int minperiod,
394       int N, int *T0_, int prev_period, opus_val16 prev_gain)
395 {
396    int k, i, T, T0;
397    opus_val16 g, g0;
398    opus_val16 pg;
399    opus_val32 xy,xx,yy;
400    opus_val32 xcorr[3];
401    opus_val32 best_xy, best_yy;
402    int offset;
403    int minperiod0;
404
405    minperiod0 = minperiod;
406    maxperiod /= 2;
407    minperiod /= 2;
408    *T0_ /= 2;
409    prev_period /= 2;
410    N /= 2;
411    x += maxperiod;
412    if (*T0_>=maxperiod)
413       *T0_=maxperiod-1;
414
415    T = T0 = *T0_;
416    xx=xy=yy=0;
417    for (i=0;i<N;i++)
418    {
419       xy = MAC16_16(xy, x[i], x[i-T0]);
420       xx = MAC16_16(xx, x[i], x[i]);
421       yy = MAC16_16(yy, x[i-T0],x[i-T0]);
422    }
423    best_xy = xy;
424    best_yy = yy;
425 #ifdef FIXED_POINT
426       {
427          opus_val32 x2y2;
428          int sh, t;
429          x2y2 = 1+HALF32(MULT32_32_Q31(xx,yy));
430          sh = celt_ilog2(x2y2)>>1;
431          t = VSHR32(x2y2, 2*(sh-7));
432          g = g0 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
433       }
434 #else
435       g = g0 = xy/celt_sqrt(1+xx*yy);
436 #endif
437    /* Look for any pitch at T/k */
438    for (k=2;k<=15;k++)
439    {
440       int T1, T1b;
441       opus_val16 g1;
442       opus_val16 cont=0;
443       opus_val16 thresh;
444       T1 = (2*T0+k)/(2*k);
445       if (T1 < minperiod)
446          break;
447       /* Look for another strong correlation at T1b */
448       if (k==2)
449       {
450          if (T1+T0>maxperiod)
451             T1b = T0;
452          else
453             T1b = T0+T1;
454       } else
455       {
456          T1b = (2*second_check[k]*T0+k)/(2*k);
457       }
458       xy=yy=0;
459       for (i=0;i<N;i++)
460       {
461          xy = MAC16_16(xy, x[i], x[i-T1]);
462          yy = MAC16_16(yy, x[i-T1], x[i-T1]);
463
464          xy = MAC16_16(xy, x[i], x[i-T1b]);
465          yy = MAC16_16(yy, x[i-T1b], x[i-T1b]);
466       }
467 #ifdef FIXED_POINT
468       {
469          opus_val32 x2y2;
470          int sh, t;
471          x2y2 = 1+MULT32_32_Q31(xx,yy);
472          sh = celt_ilog2(x2y2)>>1;
473          t = VSHR32(x2y2, 2*(sh-7));
474          g1 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
475       }
476 #else
477       g1 = xy/celt_sqrt(1+2.f*xx*1.f*yy);
478 #endif
479       if (abs(T1-prev_period)<=1)
480          cont = prev_gain;
481       else if (abs(T1-prev_period)<=2 && 5*k*k < T0)
482          cont = HALF32(prev_gain);
483       else
484          cont = 0;
485       thresh = MAX16(QCONST16(.3f,15), MULT16_16_Q15(QCONST16(.7f,15),g0)-cont);
486       /* Bias against very high pitch (very short period) to avoid false-positives
487          due to short-term correlation */
488       if (T1<3*minperiod)
489          thresh = MAX16(QCONST16(.4f,15), MULT16_16_Q15(QCONST16(.85f,15),g0)-cont);
490       else if (T1<2*minperiod)
491          thresh = MAX16(QCONST16(.5f,15), MULT16_16_Q15(QCONST16(.9f,15),g0)-cont);
492       if (g1 > thresh)
493       {
494          best_xy = xy;
495          best_yy = yy;
496          T = T1;
497          g = g1;
498       }
499    }
500    best_xy = MAX32(0, best_xy);
501    if (best_yy <= best_xy)
502       pg = Q15ONE;
503    else
504       pg = SHR32(frac_div32(best_xy,best_yy+1),16);
505
506    for (k=0;k<3;k++)
507    {
508       int T1 = T+k-1;
509       xy = 0;
510       for (i=0;i<N;i++)
511          xy = MAC16_16(xy, x[i], x[i-T1]);
512       xcorr[k] = xy;
513    }
514    if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0]))
515       offset = 1;
516    else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2]))
517       offset = -1;
518    else
519       offset = 0;
520    if (pg > g)
521       pg = g;
522    *T0_ = 2*T+offset;
523
524    if (*T0_<minperiod0)
525       *T0_=minperiod0;
526    return pg;
527 }