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