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