Use stack_alloc for pitch.c
[opus.git] / libcelt / 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    - Neither the name of the Xiph.org Foundation nor the names of its
22    contributors may be used to endorse or promote products derived from
23    this software without specific prior written permission.
24    
25    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
28    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
29    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
30    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
31    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
32    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
33    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
34    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
35    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
36 */
37
38
39 #ifdef HAVE_CONFIG_H
40 #include "config.h"
41 #endif
42
43 #include "pitch.h"
44 #include "os_support.h"
45 #include "modes.h"
46 #include "stack_alloc.h"
47 #include "mathops.h"
48
49 void find_best_pitch(celt_word32 *xcorr, celt_word32 maxcorr, celt_word16 *y, int yshift, int len, int max_pitch, int best_pitch[2])
50 {
51    int i, j;
52    celt_word32 Syy=1;
53    celt_word16 best_num[2];
54    celt_word32 best_den[2];
55 #ifdef FIXED_POINT
56    int xshift;
57
58    xshift = celt_ilog2(maxcorr)-14;
59 #endif
60
61    best_num[0] = -1;
62    best_num[1] = -1;
63    best_den[0] = 0;
64    best_den[1] = 0;
65    best_pitch[0] = 0;
66    best_pitch[1] = 1;
67    for (j=0;j<len;j++)
68       Syy = MAC16_16(Syy, y[j],y[j]);
69    for (i=0;i<max_pitch;i++)
70    {
71       float score;
72       if (xcorr[i]>0)
73       {
74          celt_word16 num;
75          celt_word32 xcorr16;
76          xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift));
77          num = MULT16_16_Q15(xcorr16,xcorr16);
78          score = num*1./Syy;
79          if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy))
80          {
81             if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy))
82             {
83                best_num[1] = best_num[0];
84                best_den[1] = best_den[0];
85                best_pitch[1] = best_pitch[0];
86                best_num[0] = num;
87                best_den[0] = Syy;
88                best_pitch[0] = i;
89             } else {
90                best_num[1] = num;
91                best_den[1] = Syy;
92                best_pitch[1] = i;
93             }
94          }
95       }
96       Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift);
97       Syy = MAX32(1, Syy);
98    }
99 }
100
101 void find_temporal_pitch(const CELTMode *m, const celt_sig * restrict x, celt_word16 * restrict y, int len, int max_pitch, int *pitch, int _C, celt_sig *xmem)
102 {
103    int i, j;
104    const int C = CHANNELS(_C);
105    const int lag = MAX_PERIOD;
106    const int N = FRAMESIZE(m);
107    int best_pitch[2]={0};
108    VARDECL(celt_word16, x_lp);
109    VARDECL(celt_word16, x_lp4);
110    VARDECL(celt_word16, y_lp4);
111    VARDECL(celt_word32, xcorr);
112    celt_word32 maxcorr=1;
113    int offset;
114    int shift=0;
115
116    SAVE_STACK;
117
118    ALLOC(x_lp, len>>1, celt_word16);
119    ALLOC(x_lp4, len>>2, celt_word16);
120    ALLOC(y_lp4, len>>2, celt_word16);
121    ALLOC(xcorr, max_pitch>>1, celt_word32);
122
123    /* Down-sample by two and downmix to mono */
124    for (i=1;i<len>>1;i++)
125       x_lp[i] = SHR32(HALF32(HALF32(x[(2*i-1)*C]+x[(2*i+1)*C])+x[2*i*C]), SIG_SHIFT);
126    x_lp[0] = SHR32(HALF32(HALF32(*xmem+x[C])+x[0]), SIG_SHIFT);
127    *xmem = x[N-C];
128    if (C==2)
129    {
130       for (i=1;i<len>>1;i++)
131       x_lp[i] = SHR32(HALF32(HALF32(x[(2*i-1)*C+1]+x[(2*i+1)*C+1])+x[2*i*C+1]), SIG_SHIFT);
132       x_lp[0] += SHR32(HALF32(HALF32(x[C+1])+x[1]), SIG_SHIFT);
133       *xmem += x[N-C+1];
134    }
135
136    /* Downsample by 2 again */
137    for (j=0;j<len>>2;j++)
138       x_lp4[j] = x_lp[2*j];
139    for (j=0;j<lag>>2;j++)
140       y_lp4[j] = y[2*j];
141
142 #ifdef FIXED_POINT
143    shift = celt_ilog2(MAX16(celt_maxabs16(x_lp4, len>>2), celt_maxabs16(y_lp4, lag>>2)))-11;
144    if (shift>0)
145    {
146       for (j=0;j<len>>2;j++)
147          x_lp4[j] = SHR16(x_lp4[j], shift);
148       for (j=0;j<lag>>2;j++)
149          y_lp4[j] = SHR16(y_lp4[j], shift);
150       /* Use double the shift for a MAC */
151       shift *= 2;
152    } else {
153       shift = 0;
154    }
155 #endif
156
157    /* Coarse search with 4x decimation */
158
159    for (i=0;i<max_pitch>>2;i++)
160    {
161       celt_word32 sum = 0;
162       for (j=0;j<len>>2;j++)
163          sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]);
164       xcorr[i] = MAX32(-1, sum);
165       maxcorr = MAX32(maxcorr, sum);
166    }
167    find_best_pitch(xcorr, maxcorr, y_lp4, 0, len>>2, max_pitch>>2, best_pitch);
168
169    /* Finer search with 2x decimation */
170    maxcorr=1;
171    for (i=0;i<max_pitch>>1;i++)
172    {
173       celt_word32 sum=0;
174       xcorr[i] = 0;
175       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
176          continue;
177       for (j=0;j<len>>1;j++)
178          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
179       xcorr[i] = MAX32(-1, sum);
180       maxcorr = MAX32(maxcorr, sum);
181    }
182    find_best_pitch(xcorr, maxcorr, y, shift, len>>1, max_pitch>>1, best_pitch);
183
184    /* Refine by pseudo-interpolation */
185    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
186    {
187       celt_word32 a, b, c;
188       a = xcorr[best_pitch[0]-1];
189       b = xcorr[best_pitch[0]];
190       c = xcorr[best_pitch[0]+1];
191       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
192          offset = 1;
193       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
194          offset = -1;
195       else 
196          offset = 0;
197    } else {
198       offset = 0;
199    }
200    *pitch = 2*best_pitch[0]-offset;
201
202    CELT_COPY(y, y+(N>>1), (lag-N)>>1);
203    CELT_COPY(y+((lag-N)>>1), x_lp, N>>1);
204
205    RESTORE_STACK;
206
207    /*printf ("%d\n", *pitch);*/
208 }