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