Line data Source code
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 :
45 0 : 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 0 : 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 0 : best_num[0] = -1;
63 0 : best_num[1] = -1;
64 0 : best_den[0] = 0;
65 0 : best_den[1] = 0;
66 0 : best_pitch[0] = 0;
67 0 : best_pitch[1] = 1;
68 0 : for (j=0;j<len;j++)
69 0 : Syy = ADD32(Syy, SHR32(MULT16_16(y[j],y[j]), yshift));
70 0 : for (i=0;i<max_pitch;i++)
71 : {
72 0 : if (xcorr[i]>0)
73 : {
74 : opus_val16 num;
75 : opus_val32 xcorr16;
76 0 : 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 0 : xcorr16 *= 1e-12f;
81 : #endif
82 0 : num = MULT16_16_Q15(xcorr16,xcorr16);
83 0 : if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy))
84 : {
85 0 : if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy))
86 : {
87 0 : best_num[1] = best_num[0];
88 0 : best_den[1] = best_den[0];
89 0 : best_pitch[1] = best_pitch[0];
90 0 : best_num[0] = num;
91 0 : best_den[0] = Syy;
92 0 : best_pitch[0] = i;
93 : } else {
94 0 : best_num[1] = num;
95 0 : best_den[1] = Syy;
96 0 : best_pitch[1] = i;
97 : }
98 : }
99 : }
100 0 : Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift);
101 0 : Syy = MAX32(1, Syy);
102 : }
103 0 : }
104 :
105 0 : 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 0 : num0=num[0];
115 0 : num1=num[1];
116 0 : num2=num[2];
117 0 : num3=num[3];
118 0 : num4=num[4];
119 0 : mem0=mem[0];
120 0 : mem1=mem[1];
121 0 : mem2=mem[2];
122 0 : mem3=mem[3];
123 0 : mem4=mem[4];
124 0 : for (i=0;i<N;i++)
125 : {
126 0 : opus_val32 sum = SHL32(EXTEND32(x[i]), SIG_SHIFT);
127 0 : sum = MAC16_16(sum,num0,mem0);
128 0 : sum = MAC16_16(sum,num1,mem1);
129 0 : sum = MAC16_16(sum,num2,mem2);
130 0 : sum = MAC16_16(sum,num3,mem3);
131 0 : sum = MAC16_16(sum,num4,mem4);
132 0 : mem4 = mem3;
133 0 : mem3 = mem2;
134 0 : mem2 = mem1;
135 0 : mem1 = mem0;
136 0 : mem0 = x[i];
137 0 : y[i] = ROUND16(sum, SIG_SHIFT);
138 : }
139 0 : mem[0]=mem0;
140 0 : mem[1]=mem1;
141 0 : mem[2]=mem2;
142 0 : mem[3]=mem3;
143 0 : mem[4]=mem4;
144 0 : }
145 :
146 :
147 0 : 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 0 : opus_val16 tmp=Q15ONE;
153 0 : opus_val16 lpc[4], mem[5]={0,0,0,0,0};
154 : opus_val16 lpc2[5];
155 0 : 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 0 : for (i=1;i<len>>1;i++)
173 0 : x_lp[i] = SHR32(HALF32(HALF32(x[0][(2*i-1)]+x[0][(2*i+1)])+x[0][2*i]), shift);
174 0 : x_lp[0] = SHR32(HALF32(HALF32(x[0][1])+x[0][0]), shift);
175 0 : if (C==2)
176 : {
177 0 : for (i=1;i<len>>1;i++)
178 0 : x_lp[i] += SHR32(HALF32(HALF32(x[1][(2*i-1)]+x[1][(2*i+1)])+x[1][2*i]), shift);
179 0 : x_lp[0] += SHR32(HALF32(HALF32(x[1][1])+x[1][0]), shift);
180 : }
181 :
182 0 : _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 0 : ac[0] *= 1.0001f;
190 : #endif
191 : /* Lag windowing */
192 0 : 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 0 : ac[i] -= ac[i]*(.008f*i)*(.008f*i);
199 : #endif
200 : }
201 :
202 0 : _celt_lpc(lpc, ac, 4);
203 0 : for (i=0;i<4;i++)
204 : {
205 0 : tmp = MULT16_16_Q15(QCONST16(.9f,15), tmp);
206 0 : lpc[i] = MULT16_16_Q15(lpc[i], tmp);
207 : }
208 : /* Add a zero */
209 0 : lpc2[0] = lpc[0] + QCONST16(.8f,SIG_SHIFT);
210 0 : lpc2[1] = lpc[1] + MULT16_16_Q15(c1,lpc[0]);
211 0 : lpc2[2] = lpc[2] + MULT16_16_Q15(c1,lpc[1]);
212 0 : lpc2[3] = lpc[3] + MULT16_16_Q15(c1,lpc[2]);
213 0 : lpc2[4] = MULT16_16_Q15(c1,lpc[3]);
214 0 : celt_fir5(x_lp, lpc2, x_lp, len>>1, mem);
215 0 : }
216 :
217 : /* Pure C implementation. */
218 : #ifdef FIXED_POINT
219 : opus_val32
220 : #else
221 : void
222 : #endif
223 0 : 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 0 : celt_assert(max_pitch>0);
259 0 : celt_assert((((unsigned char *)_x-(unsigned char *)NULL)&3)==0);
260 0 : for (i=0;i<max_pitch-3;i+=4)
261 : {
262 0 : opus_val32 sum[4]={0,0,0,0};
263 0 : xcorr_kernel(_x, _y+i, sum, len, arch);
264 0 : xcorr[i]=sum[0];
265 0 : xcorr[i+1]=sum[1];
266 0 : xcorr[i+2]=sum[2];
267 0 : 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 0 : for (;i<max_pitch;i++)
277 : {
278 : opus_val32 sum;
279 0 : sum = celt_inner_prod(_x, _y+i, len, arch);
280 0 : 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 0 : }
290 :
291 0 : 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 0 : 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 0 : celt_assert(len>0);
310 0 : celt_assert(max_pitch>0);
311 0 : lag = len+max_pitch;
312 :
313 0 : ALLOC(x_lp4, len>>2, opus_val16);
314 0 : ALLOC(y_lp4, lag>>2, opus_val16);
315 0 : ALLOC(xcorr, max_pitch>>1, opus_val32);
316 :
317 : /* Downsample by 2 again */
318 0 : for (j=0;j<len>>2;j++)
319 0 : x_lp4[j] = x_lp[2*j];
320 0 : for (j=0;j<lag>>2;j++)
321 0 : 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 0 : celt_pitch_xcorr(x_lp4, y_lp4, xcorr, len>>2, max_pitch>>2, arch);
346 :
347 0 : 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 0 : for (i=0;i<max_pitch>>1;i++)
358 : {
359 : opus_val32 sum;
360 0 : xcorr[i] = 0;
361 0 : if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
362 0 : 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 0 : sum = celt_inner_prod(x_lp, y+i, len>>1, arch);
369 : #endif
370 0 : xcorr[i] = MAX32(-1, sum);
371 : #ifdef FIXED_POINT
372 : maxcorr = MAX32(maxcorr, sum);
373 : #endif
374 : }
375 0 : 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 0 : if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
383 0 : {
384 : opus_val32 a, b, c;
385 0 : a = xcorr[best_pitch[0]-1];
386 0 : b = xcorr[best_pitch[0]];
387 0 : c = xcorr[best_pitch[0]+1];
388 0 : if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
389 0 : offset = 1;
390 0 : else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
391 0 : offset = -1;
392 : else
393 0 : offset = 0;
394 : } else {
395 0 : offset = 0;
396 : }
397 0 : *pitch = 2*best_pitch[0]-offset;
398 :
399 : RESTORE_STACK;
400 0 : }
401 :
402 : #ifdef FIXED_POINT
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
431 0 : static opus_val16 compute_pitch_gain(opus_val32 xy, opus_val32 xx, opus_val32 yy)
432 : {
433 0 : 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};
438 0 : 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 0 : minperiod0 = minperiod;
453 0 : maxperiod /= 2;
454 0 : minperiod /= 2;
455 0 : *T0_ /= 2;
456 0 : prev_period /= 2;
457 0 : N /= 2;
458 0 : x += maxperiod;
459 0 : if (*T0_>=maxperiod)
460 0 : *T0_=maxperiod-1;
461 :
462 0 : T = T0 = *T0_;
463 0 : ALLOC(yy_lookup, maxperiod+1, opus_val32);
464 0 : dual_inner_prod(x, x, x-T0, N, &xx, &xy, arch);
465 0 : yy_lookup[0] = xx;
466 0 : yy=xx;
467 0 : for (i=1;i<=maxperiod;i++)
468 : {
469 0 : yy = yy+MULT16_16(x[-i],x[-i])-MULT16_16(x[N-i],x[N-i]);
470 0 : yy_lookup[i] = MAX32(0, yy);
471 : }
472 0 : yy = yy_lookup[T0];
473 0 : best_xy = xy;
474 0 : best_yy = yy;
475 0 : g = g0 = compute_pitch_gain(xy, xx, yy);
476 : /* Look for any pitch at T/k */
477 0 : for (k=2;k<=15;k++)
478 : {
479 : int T1, T1b;
480 : opus_val16 g1;
481 0 : opus_val16 cont=0;
482 : opus_val16 thresh;
483 0 : T1 = celt_udiv(2*T0+k, 2*k);
484 0 : if (T1 < minperiod)
485 0 : break;
486 : /* Look for another strong correlation at T1b */
487 0 : if (k==2)
488 : {
489 0 : if (T1+T0>maxperiod)
490 0 : T1b = T0;
491 : else
492 0 : T1b = T0+T1;
493 : } else
494 : {
495 0 : T1b = celt_udiv(2*second_check[k]*T0+k, 2*k);
496 : }
497 0 : dual_inner_prod(x, &x[-T1], &x[-T1b], N, &xy, &xy2, arch);
498 0 : xy = HALF32(xy + xy2);
499 0 : yy = HALF32(yy_lookup[T1] + yy_lookup[T1b]);
500 0 : g1 = compute_pitch_gain(xy, xx, yy);
501 0 : if (abs(T1-prev_period)<=1)
502 0 : cont = prev_gain;
503 0 : else if (abs(T1-prev_period)<=2 && 5*k*k < T0)
504 0 : cont = HALF16(prev_gain);
505 : else
506 0 : cont = 0;
507 0 : 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 0 : if (T1<3*minperiod)
511 0 : thresh = MAX16(QCONST16(.4f,15), MULT16_16_Q15(QCONST16(.85f,15),g0)-cont);
512 0 : else if (T1<2*minperiod)
513 0 : thresh = MAX16(QCONST16(.5f,15), MULT16_16_Q15(QCONST16(.9f,15),g0)-cont);
514 0 : if (g1 > thresh)
515 : {
516 0 : best_xy = xy;
517 0 : best_yy = yy;
518 0 : T = T1;
519 0 : g = g1;
520 : }
521 : }
522 0 : best_xy = MAX32(0, best_xy);
523 0 : if (best_yy <= best_xy)
524 0 : pg = Q15ONE;
525 : else
526 0 : pg = SHR32(frac_div32(best_xy,best_yy+1),16);
527 :
528 0 : for (k=0;k<3;k++)
529 0 : xcorr[k] = celt_inner_prod(x, x-(T+k-1), N, arch);
530 0 : if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0]))
531 0 : offset = 1;
532 0 : else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2]))
533 0 : offset = -1;
534 : else
535 0 : offset = 0;
536 0 : if (pg > g)
537 0 : pg = g;
538 0 : *T0_ = 2*T+offset;
539 :
540 0 : if (*T0_<minperiod0)
541 0 : *T0_=minperiod0;
542 : RESTORE_STACK;
543 0 : return pg;
544 : }
|
