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