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