Cisco optimization for x86 & fixed point
[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, int arch)
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, arch);
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 celt_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 celt_pitch_xcorr_c(const opus_val16 *_x, const opus_val16 *_y,
254       opus_val32 *xcorr, int len, int max_pitch, int arch)
255 {
256    int i;
257    /*The EDSP version requires that max_pitch is at least 1, and that _x is
258       32-bit aligned.
259      Since it's hard to put asserts in assembly, put them here.*/
260 #ifdef FIXED_POINT
261    opus_val32 maxcorr=1;
262 #endif
263    celt_assert(max_pitch>0);
264    celt_assert((((unsigned char *)_x-(unsigned char *)NULL)&3)==0);
265    for (i=0;i<max_pitch-3;i+=4)
266    {
267       opus_val32 sum[4]={0,0,0,0};
268       xcorr_kernel(_x, _y+i, sum, len, arch);
269       xcorr[i]=sum[0];
270       xcorr[i+1]=sum[1];
271       xcorr[i+2]=sum[2];
272       xcorr[i+3]=sum[3];
273 #ifdef FIXED_POINT
274       sum[0] = MAX32(sum[0], sum[1]);
275       sum[2] = MAX32(sum[2], sum[3]);
276       sum[0] = MAX32(sum[0], sum[2]);
277       maxcorr = MAX32(maxcorr, sum[0]);
278 #endif
279    }
280    /* In case max_pitch isn't a multiple of 4, do non-unrolled version. */
281    for (;i<max_pitch;i++)
282    {
283       opus_val32 sum;
284       sum = celt_inner_prod(_x, _y+i, len, arch);
285       xcorr[i] = sum;
286 #ifdef FIXED_POINT
287       maxcorr = MAX32(maxcorr, sum);
288 #endif
289    }
290 #ifdef FIXED_POINT
291    return maxcorr;
292 #endif
293 }
294
295 #endif
296 void pitch_search(const opus_val16 * OPUS_RESTRICT x_lp, opus_val16 * OPUS_RESTRICT y,
297                   int len, int max_pitch, int *pitch, int arch)
298 {
299    int i, j;
300    int lag;
301    int best_pitch[2]={0,0};
302    VARDECL(opus_val16, x_lp4);
303    VARDECL(opus_val16, y_lp4);
304    VARDECL(opus_val32, xcorr);
305 #ifdef FIXED_POINT
306    opus_val32 maxcorr;
307    opus_val32 xmax, ymax;
308    int shift=0;
309 #endif
310    int offset;
311
312    SAVE_STACK;
313
314    celt_assert(len>0);
315    celt_assert(max_pitch>0);
316    lag = len+max_pitch;
317
318    ALLOC(x_lp4, len>>2, opus_val16);
319    ALLOC(y_lp4, lag>>2, opus_val16);
320    ALLOC(xcorr, max_pitch>>1, opus_val32);
321
322    /* Downsample by 2 again */
323    for (j=0;j<len>>2;j++)
324       x_lp4[j] = x_lp[2*j];
325    for (j=0;j<lag>>2;j++)
326       y_lp4[j] = y[2*j];
327
328 #ifdef FIXED_POINT
329    xmax = celt_maxabs16(x_lp4, len>>2);
330    ymax = celt_maxabs16(y_lp4, lag>>2);
331    shift = celt_ilog2(MAX32(1, MAX32(xmax, ymax)))-11;
332    if (shift>0)
333    {
334       for (j=0;j<len>>2;j++)
335          x_lp4[j] = SHR16(x_lp4[j], shift);
336       for (j=0;j<lag>>2;j++)
337          y_lp4[j] = SHR16(y_lp4[j], shift);
338       /* Use double the shift for a MAC */
339       shift *= 2;
340    } else {
341       shift = 0;
342    }
343 #endif
344
345    /* Coarse search with 4x decimation */
346
347 #ifdef FIXED_POINT
348    maxcorr =
349 #endif
350    celt_pitch_xcorr(x_lp4, y_lp4, xcorr, len>>2, max_pitch>>2, arch);
351
352    find_best_pitch(xcorr, y_lp4, len>>2, max_pitch>>2, best_pitch
353 #ifdef FIXED_POINT
354                    , 0, maxcorr
355 #endif
356                    );
357
358    /* Finer search with 2x decimation */
359 #ifdef FIXED_POINT
360    maxcorr=1;
361 #endif
362    for (i=0;i<max_pitch>>1;i++)
363    {
364       opus_val32 sum;
365       xcorr[i] = 0;
366       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
367          continue;
368 #ifdef FIXED_POINT
369       sum = 0;
370       for (j=0;j<len>>1;j++)
371          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
372 #else
373       sum = celt_inner_prod_c(x_lp, y+i, len>>1);
374 #endif
375       xcorr[i] = MAX32(-1, sum);
376 #ifdef FIXED_POINT
377       maxcorr = MAX32(maxcorr, sum);
378 #endif
379    }
380    find_best_pitch(xcorr, y, len>>1, max_pitch>>1, best_pitch
381 #ifdef FIXED_POINT
382                    , shift+1, maxcorr
383 #endif
384                    );
385
386    /* Refine by pseudo-interpolation */
387    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
388    {
389       opus_val32 a, b, c;
390       a = xcorr[best_pitch[0]-1];
391       b = xcorr[best_pitch[0]];
392       c = xcorr[best_pitch[0]+1];
393       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
394          offset = 1;
395       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
396          offset = -1;
397       else
398          offset = 0;
399    } else {
400       offset = 0;
401    }
402    *pitch = 2*best_pitch[0]-offset;
403
404    RESTORE_STACK;
405 }
406
407 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2};
408 opus_val16 remove_doubling(opus_val16 *x, int maxperiod, int minperiod,
409       int N, int *T0_, int prev_period, opus_val16 prev_gain, int arch)
410 {
411    int k, i, T, T0;
412    opus_val16 g, g0;
413    opus_val16 pg;
414    opus_val32 xy,xx,yy,xy2;
415    opus_val32 xcorr[3];
416    opus_val32 best_xy, best_yy;
417    int offset;
418    int minperiod0;
419    VARDECL(opus_val32, yy_lookup);
420    SAVE_STACK;
421
422    minperiod0 = minperiod;
423    maxperiod /= 2;
424    minperiod /= 2;
425    *T0_ /= 2;
426    prev_period /= 2;
427    N /= 2;
428    x += maxperiod;
429    if (*T0_>=maxperiod)
430       *T0_=maxperiod-1;
431
432    T = T0 = *T0_;
433    ALLOC(yy_lookup, maxperiod+1, opus_val32);
434    dual_inner_prod(x, x, x-T0, N, &xx, &xy);
435    yy_lookup[0] = xx;
436    yy=xx;
437    for (i=1;i<=maxperiod;i++)
438    {
439       yy = yy+MULT16_16(x[-i],x[-i])-MULT16_16(x[N-i],x[N-i]);
440       yy_lookup[i] = MAX32(0, yy);
441    }
442    yy = yy_lookup[T0];
443    best_xy = xy;
444    best_yy = yy;
445 #ifdef FIXED_POINT
446       {
447          opus_val32 x2y2;
448          int sh, t;
449          x2y2 = 1+HALF32(MULT32_32_Q31(xx,yy));
450          sh = celt_ilog2(x2y2)>>1;
451          t = VSHR32(x2y2, 2*(sh-7));
452          g = g0 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
453       }
454 #else
455       g = g0 = xy/celt_sqrt(1+xx*yy);
456 #endif
457    /* Look for any pitch at T/k */
458    for (k=2;k<=15;k++)
459    {
460       int T1, T1b;
461       opus_val16 g1;
462       opus_val16 cont=0;
463       opus_val16 thresh;
464       T1 = celt_udiv(2*T0+k, 2*k);
465       if (T1 < minperiod)
466          break;
467       /* Look for another strong correlation at T1b */
468       if (k==2)
469       {
470          if (T1+T0>maxperiod)
471             T1b = T0;
472          else
473             T1b = T0+T1;
474       } else
475       {
476          T1b = celt_udiv(2*second_check[k]*T0+k, 2*k);
477       }
478       dual_inner_prod(x, &x[-T1], &x[-T1b], N, &xy, &xy2);
479       xy += xy2;
480       yy = yy_lookup[T1] + yy_lookup[T1b];
481 #ifdef FIXED_POINT
482       {
483          opus_val32 x2y2;
484          int sh, t;
485          x2y2 = 1+MULT32_32_Q31(xx,yy);
486          sh = celt_ilog2(x2y2)>>1;
487          t = VSHR32(x2y2, 2*(sh-7));
488          g1 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
489       }
490 #else
491       g1 = xy/celt_sqrt(1+2.f*xx*1.f*yy);
492 #endif
493       if (abs(T1-prev_period)<=1)
494          cont = prev_gain;
495       else if (abs(T1-prev_period)<=2 && 5*k*k < T0)
496          cont = HALF32(prev_gain);
497       else
498          cont = 0;
499       thresh = MAX16(QCONST16(.3f,15), MULT16_16_Q15(QCONST16(.7f,15),g0)-cont);
500       /* Bias against very high pitch (very short period) to avoid false-positives
501          due to short-term correlation */
502       if (T1<3*minperiod)
503          thresh = MAX16(QCONST16(.4f,15), MULT16_16_Q15(QCONST16(.85f,15),g0)-cont);
504       else if (T1<2*minperiod)
505          thresh = MAX16(QCONST16(.5f,15), MULT16_16_Q15(QCONST16(.9f,15),g0)-cont);
506       if (g1 > thresh)
507       {
508          best_xy = xy;
509          best_yy = yy;
510          T = T1;
511          g = g1;
512       }
513    }
514    best_xy = MAX32(0, best_xy);
515    if (best_yy <= best_xy)
516       pg = Q15ONE;
517    else
518       pg = SHR32(frac_div32(best_xy,best_yy+1),16);
519
520    for (k=0;k<3;k++)
521       xcorr[k] = celt_inner_prod(x, x-(T+k-1), N, arch);
522    if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0]))
523       offset = 1;
524    else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2]))
525       offset = -1;
526    else
527       offset = 0;
528    if (pg > g)
529       pg = g;
530    *T0_ = 2*T+offset;
531
532    if (*T0_<minperiod0)
533       *T0_=minperiod0;
534    RESTORE_STACK;
535    return pg;
536 }