Reorganised the pitch code to simplify the PLC case and fixed a buffer overflow
[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 pitch_downsample(const celt_sig * restrict x, celt_word16 * restrict x_lp, int len, int end, int _C, celt_sig * restrict xmem, celt_word16 * restrict filt_mem)
102 {
103    int i;
104    const int C = CHANNELS(_C);
105    for (i=1;i<len>>1;i++)
106       x_lp[i] = SHR32(HALF32(HALF32(x[(2*i-1)*C]+x[(2*i+1)*C])+x[2*i*C]), SIG_SHIFT);
107    x_lp[0] = SHR32(HALF32(HALF32(*xmem+x[C])+x[0]), SIG_SHIFT);
108    *xmem = x[end-C];
109    if (C==2)
110    {
111       for (i=1;i<len>>1;i++)
112       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);
113       x_lp[0] += SHR32(HALF32(HALF32(x[C+1])+x[1]), SIG_SHIFT);
114       *xmem += x[end-C+1];
115    }
116
117 #if 0
118    {
119       int j;
120       float ac[3]={0,0,0};
121       float ak[2];
122       float det;
123       for (i=0;i<3;i++)
124       {
125          for (j=0;j<(len>>1)-i;j++)
126          {
127             ac[i] += x_lp[j]*x_lp[j+i];
128          }
129       }
130       det = 1./(.1+ac[0]*ac[0]-ac[1]*ac[1]);
131       ak[0] = det*(ac[0]*ac[1] - ac[1]*ac[2]);
132       ak[1] = det*(-ac[1]*ac[1] + ac[0]*ac[2]);
133       /*printf ("%f %f %f\n", 1., -ak[0], -ak[1]);*/
134       float mem[2];
135       for (j=0;j<len>>1;j++)
136       {
137          float tmp = x_lp[j];
138          x_lp[j] = x_lp[j] - ak[0]*filt_mem[0] - ak[1]*filt_mem[1];
139          filt_mem[1]=mem[0];
140          filt_mem[0]=tmp;
141       }
142    }
143 #endif
144
145 }
146
147 void pitch_search(const CELTMode *m, const celt_word16 * restrict x_lp, celt_word16 * restrict y, int len, int max_pitch, int *pitch, celt_sig *xmem)
148 {
149    int i, j;
150    const int lag = MAX_PERIOD;
151    const int N = FRAMESIZE(m);
152    int best_pitch[2]={0};
153    VARDECL(celt_word16, x_lp4);
154    VARDECL(celt_word16, y_lp4);
155    VARDECL(celt_word32, xcorr);
156    celt_word32 maxcorr=1;
157    int offset;
158    int shift=0;
159
160    SAVE_STACK;
161
162    ALLOC(x_lp4, len>>2, celt_word16);
163    ALLOC(y_lp4, lag>>2, celt_word16);
164    ALLOC(xcorr, max_pitch>>1, celt_word32);
165
166    /* Downsample by 2 again */
167    for (j=0;j<len>>2;j++)
168       x_lp4[j] = x_lp[2*j];
169    for (j=0;j<lag>>2;j++)
170       y_lp4[j] = y[2*j];
171
172 #ifdef FIXED_POINT
173    shift = celt_ilog2(MAX16(celt_maxabs16(x_lp4, len>>2), celt_maxabs16(y_lp4, lag>>2)))-11;
174    if (shift>0)
175    {
176       for (j=0;j<len>>2;j++)
177          x_lp4[j] = SHR16(x_lp4[j], shift);
178       for (j=0;j<lag>>2;j++)
179          y_lp4[j] = SHR16(y_lp4[j], shift);
180       /* Use double the shift for a MAC */
181       shift *= 2;
182    } else {
183       shift = 0;
184    }
185 #endif
186
187    /* Coarse search with 4x decimation */
188
189    for (i=0;i<max_pitch>>2;i++)
190    {
191       celt_word32 sum = 0;
192       for (j=0;j<len>>2;j++)
193          sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]);
194       xcorr[i] = MAX32(-1, sum);
195       maxcorr = MAX32(maxcorr, sum);
196    }
197    find_best_pitch(xcorr, maxcorr, y_lp4, 0, len>>2, max_pitch>>2, best_pitch);
198
199    /* Finer search with 2x decimation */
200    maxcorr=1;
201    for (i=0;i<max_pitch>>1;i++)
202    {
203       celt_word32 sum=0;
204       xcorr[i] = 0;
205       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
206          continue;
207       for (j=0;j<len>>1;j++)
208          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
209       xcorr[i] = MAX32(-1, sum);
210       maxcorr = MAX32(maxcorr, sum);
211    }
212    find_best_pitch(xcorr, maxcorr, y, shift, len>>1, max_pitch>>1, best_pitch);
213
214    /* Refine by pseudo-interpolation */
215    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
216    {
217       celt_word32 a, b, c;
218       a = xcorr[best_pitch[0]-1];
219       b = xcorr[best_pitch[0]];
220       c = xcorr[best_pitch[0]+1];
221       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
222          offset = 1;
223       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
224          offset = -1;
225       else 
226          offset = 0;
227    } else {
228       offset = 0;
229    }
230    *pitch = 2*best_pitch[0]-offset;
231
232    CELT_MOVE(y, y+(N>>1), (lag-N)>>1);
233    CELT_MOVE(y+((lag-N)>>1), x_lp, N>>1);
234
235    RESTORE_STACK;
236
237    /*printf ("%d\n", *pitch);*/
238 }