Jean-Marc Valin | 8b2ff0d | 2009-10-17 21:40:10 -0400 | [diff] [blame] | 1 | /* Copyright (c) 2007-2008 CSIRO |
| 2 | Copyright (c) 2007-2009 Xiph.Org Foundation |
| 3 | Written by Jean-Marc Valin */ |
Jean-Marc Valin | 14191b3 | 2007-11-30 12:15:49 +1100 | [diff] [blame] | 4 | /** |
| 5 | @file pitch.c |
| 6 | @brief Pitch analysis |
Jean-Marc Valin | 879fbfd | 2008-02-20 17:17:13 +1100 | [diff] [blame] | 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: |
Gregory Maxwell | 71d39ad | 2011-07-30 00:00:29 -0400 | [diff] [blame] | 13 | |
Jean-Marc Valin | 879fbfd | 2008-02-20 17:17:13 +1100 | [diff] [blame] | 14 | - Redistributions of source code must retain the above copyright |
| 15 | notice, this list of conditions and the following disclaimer. |
Gregory Maxwell | 71d39ad | 2011-07-30 00:00:29 -0400 | [diff] [blame] | 16 | |
Jean-Marc Valin | 879fbfd | 2008-02-20 17:17:13 +1100 | [diff] [blame] | 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. |
Gregory Maxwell | 71d39ad | 2011-07-30 00:00:29 -0400 | [diff] [blame] | 20 | |
Jean-Marc Valin | 879fbfd | 2008-02-20 17:17:13 +1100 | [diff] [blame] | 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 |
Jean-Marc Valin | cb05e7c | 2012-04-20 16:40:24 -0400 | [diff] [blame] | 24 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER |
| 25 | OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, |
Jean-Marc Valin | 879fbfd | 2008-02-20 17:17:13 +1100 | [diff] [blame] | 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. |
Jean-Marc Valin | 14191b3 | 2007-11-30 12:15:49 +1100 | [diff] [blame] | 32 | */ |
| 33 | |
Jean-Marc Valin | 02fa913 | 2008-02-20 12:09:29 +1100 | [diff] [blame] | 34 | #ifdef HAVE_CONFIG_H |
| 35 | #include "config.h" |
| 36 | #endif |
Jean-Marc Valin | 14191b3 | 2007-11-30 12:15:49 +1100 | [diff] [blame] | 37 | |
Jean-Marc Valin | f3efa3e | 2007-12-01 01:55:17 +1100 | [diff] [blame] | 38 | #include "pitch.h" |
Jean-Marc Valin | f93747c | 2008-03-05 17:20:30 +1100 | [diff] [blame] | 39 | #include "os_support.h" |
Jean-Marc Valin | f11d6f4 | 2008-04-18 23:13:14 +1000 | [diff] [blame] | 40 | #include "modes.h" |
Jean-Marc Valin | c7e0b76 | 2008-03-16 07:55:29 +1100 | [diff] [blame] | 41 | #include "stack_alloc.h" |
Jean-Marc Valin | 9319e3e | 2009-11-09 13:51:54 +0900 | [diff] [blame] | 42 | #include "mathops.h" |
Jean-Marc Valin | 2779df7 | 2011-10-04 13:26:53 -0400 | [diff] [blame] | 43 | #include "celt_lpc.h" |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 44 | |
Gregory Maxwell | 40f956e | 2011-09-01 19:42:37 -0400 | [diff] [blame] | 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 | ) |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 51 | { |
| 52 | int i, j; |
Jean-Marc Valin | ff5f722 | 2011-07-29 18:59:12 -0400 | [diff] [blame] | 53 | opus_val32 Syy=1; |
| 54 | opus_val16 best_num[2]; |
| 55 | opus_val32 best_den[2]; |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 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++) |
Jean-Marc Valin | 178758b | 2012-04-06 23:32:11 -0400 | [diff] [blame] | 69 | Syy = ADD32(Syy, SHR32(MULT16_16(y[j],y[j]), yshift)); |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 70 | for (i=0;i<max_pitch;i++) |
| 71 | { |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 72 | if (xcorr[i]>0) |
| 73 | { |
Jean-Marc Valin | ff5f722 | 2011-07-29 18:59:12 -0400 | [diff] [blame] | 74 | opus_val16 num; |
| 75 | opus_val32 xcorr16; |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 76 | xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift)); |
Jean-Marc Valin | 9faea25 | 2012-05-08 13:58:57 -0400 | [diff] [blame] | 77 | #ifndef FIXED_POINT |
| 78 | /* Considering the range of xcorr16, this should avoid both underflows |
| 79 | and overflows (inf) when squaring xcorr16 */ |
Ralph Giles | 027ec51 | 2012-10-23 10:49:18 -0700 | [diff] [blame] | 80 | xcorr16 *= 1e-12f; |
Jean-Marc Valin | 9faea25 | 2012-05-08 13:58:57 -0400 | [diff] [blame] | 81 | #endif |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 82 | num = MULT16_16_Q15(xcorr16,xcorr16); |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 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 | |
Jean-Marc Valin | fbf9998 | 2013-05-24 17:18:41 -0400 | [diff] [blame^] | 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 | |
Gregory Maxwell | de0b532 | 2012-07-18 12:12:35 -0400 | [diff] [blame] | 147 | void pitch_downsample(celt_sig * OPUS_RESTRICT x[], opus_val16 * OPUS_RESTRICT x_lp, |
Ralph Giles | 120800f | 2011-11-25 13:02:00 -0800 | [diff] [blame] | 148 | int len, int C) |
Jean-Marc Valin | e465c14 | 2009-11-26 00:39:36 -0500 | [diff] [blame] | 149 | { |
| 150 | int i; |
Jean-Marc Valin | ff5f722 | 2011-07-29 18:59:12 -0400 | [diff] [blame] | 151 | opus_val32 ac[5]; |
| 152 | opus_val16 tmp=Q15ONE; |
Jean-Marc Valin | fbf9998 | 2013-05-24 17:18:41 -0400 | [diff] [blame^] | 153 | opus_val16 lpc[4], mem[5]={0,0,0,0,0}; |
| 154 | opus_val16 lpc2[5]; |
| 155 | opus_val16 c1 = QCONST16(.8f,15); |
Jean-Marc Valin | 178758b | 2012-04-06 23:32:11 -0400 | [diff] [blame] | 156 | #ifdef FIXED_POINT |
| 157 | int shift; |
Jean-Marc Valin | 66ac102 | 2012-05-29 17:01:35 -0400 | [diff] [blame] | 158 | opus_val32 maxabs = celt_maxabs32(x[0], len); |
Jean-Marc Valin | 178758b | 2012-04-06 23:32:11 -0400 | [diff] [blame] | 159 | if (C==2) |
Jean-Marc Valin | 66ac102 | 2012-05-29 17:01:35 -0400 | [diff] [blame] | 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; |
Jean-Marc Valin | 178758b | 2012-04-06 23:32:11 -0400 | [diff] [blame] | 169 | if (C==2) |
| 170 | shift++; |
| 171 | #endif |
Jean-Marc Valin | e465c14 | 2009-11-26 00:39:36 -0500 | [diff] [blame] | 172 | for (i=1;i<len>>1;i++) |
Jean-Marc Valin | 178758b | 2012-04-06 23:32:11 -0400 | [diff] [blame] | 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); |
Jean-Marc Valin | e465c14 | 2009-11-26 00:39:36 -0500 | [diff] [blame] | 175 | if (C==2) |
| 176 | { |
| 177 | for (i=1;i<len>>1;i++) |
Jean-Marc Valin | 178758b | 2012-04-06 23:32:11 -0400 | [diff] [blame] | 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); |
Jean-Marc Valin | e465c14 | 2009-11-26 00:39:36 -0500 | [diff] [blame] | 180 | } |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 181 | |
| 182 | _celt_autocorr(x_lp, ac, NULL, 0, |
| 183 | 4, len>>1); |
| 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 | } |
Jean-Marc Valin | fbf9998 | 2013-05-24 17:18:41 -0400 | [diff] [blame^] | 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); |
Jean-Marc Valin | e465c14 | 2009-11-26 00:39:36 -0500 | [diff] [blame] | 215 | } |
| 216 | |
Jean-Marc Valin | 559fbe8 | 2013-05-24 01:09:31 -0400 | [diff] [blame] | 217 | #if 0 /* This is a simple version of the pitch correlation that should work |
| 218 | well on DSPs like Blackfin and TI C5x/C6x */ |
| 219 | |
| 220 | static void pitch_xcorr(opus_val16 *x, opus_val16 *y, opus_val32 *xcorr, int len, int max_pitch |
| 221 | #ifdef FIXED_POINT |
| 222 | ,opus_val32 *maxval |
| 223 | #endif |
| 224 | ) |
| 225 | { |
| 226 | int i, j; |
| 227 | #ifdef FIXED_POINT |
Jean-Marc Valin | 088929d | 2013-05-24 01:38:06 -0400 | [diff] [blame] | 228 | opus_val32 maxcorr=1; |
Jean-Marc Valin | 559fbe8 | 2013-05-24 01:09:31 -0400 | [diff] [blame] | 229 | #endif |
| 230 | for (i=0;i<max_pitch;i++) |
| 231 | { |
| 232 | opus_val32 sum = 0; |
| 233 | for (j=0;j<len;j++) |
| 234 | sum = MAC16_16(sum, x[j],y[i+j]); |
| 235 | xcorr[i] = MAX32(-1, sum); |
| 236 | #ifdef FIXED_POINT |
| 237 | maxcorr = MAX32(maxcorr, sum); |
| 238 | #endif |
| 239 | } |
| 240 | #ifdef FIXED_POINT |
| 241 | *maxval = maxcorr; |
| 242 | #endif |
| 243 | } |
| 244 | |
| 245 | #else /* Unrolled version of the pitch correlation -- runs faster on x86 and ARM */ |
| 246 | |
| 247 | static void pitch_xcorr(opus_val16 *_x, opus_val16 *_y, opus_val32 *xcorr, int len, int max_pitch |
| 248 | #ifdef FIXED_POINT |
| 249 | ,opus_val32 *maxval |
| 250 | #endif |
| 251 | ) |
| 252 | { |
| 253 | int i,j; |
| 254 | #ifdef FIXED_POINT |
Jean-Marc Valin | 088929d | 2013-05-24 01:38:06 -0400 | [diff] [blame] | 255 | opus_val32 maxcorr=1; |
Jean-Marc Valin | 559fbe8 | 2013-05-24 01:09:31 -0400 | [diff] [blame] | 256 | #endif |
Jean-Marc Valin | 85a6618 | 2013-05-24 03:41:04 -0400 | [diff] [blame] | 257 | /* Truncate slightly if len is not a multiple of 4. */ |
| 258 | len -= len&3; |
| 259 | for (i=0;i<max_pitch-3;i+=4) |
Jean-Marc Valin | 559fbe8 | 2013-05-24 01:09:31 -0400 | [diff] [blame] | 260 | { |
| 261 | /* Compute correlation*/ |
| 262 | /*corr[nb_pitch-1-i]=inner_prod(x, _y+i, len);*/ |
| 263 | opus_val32 sum1=0; |
| 264 | opus_val32 sum2=0; |
| 265 | opus_val32 sum3=0; |
| 266 | opus_val32 sum4=0; |
| 267 | const opus_val16 *y = _y+i; |
| 268 | const opus_val16 *x = _x; |
| 269 | opus_val16 y0, y1, y2, y3; |
| 270 | /*y0=y[0];y1=y[1];y2=y[2];y3=y[3];*/ |
| 271 | y0=*y++; |
| 272 | y1=*y++; |
| 273 | y2=*y++; |
| 274 | for (j=0;j<len;j+=4) |
| 275 | { |
| 276 | opus_val16 tmp; |
| 277 | tmp = *x++; |
| 278 | y3=*y++; |
| 279 | sum1 = MAC16_16(sum1,tmp,y0); |
| 280 | sum2 = MAC16_16(sum2,tmp,y1); |
| 281 | sum3 = MAC16_16(sum3,tmp,y2); |
| 282 | sum4 = MAC16_16(sum4,tmp,y3); |
| 283 | tmp=*x++; |
| 284 | y0=*y++; |
| 285 | sum1 = MAC16_16(sum1,tmp,y1); |
| 286 | sum2 = MAC16_16(sum2,tmp,y2); |
| 287 | sum3 = MAC16_16(sum3,tmp,y3); |
| 288 | sum4 = MAC16_16(sum4,tmp,y0); |
| 289 | tmp=*x++; |
| 290 | y1=*y++; |
| 291 | sum1 = MAC16_16(sum1,tmp,y2); |
| 292 | sum2 = MAC16_16(sum2,tmp,y3); |
| 293 | sum3 = MAC16_16(sum3,tmp,y0); |
| 294 | sum4 = MAC16_16(sum4,tmp,y1); |
| 295 | tmp=*x++; |
| 296 | y2=*y++; |
| 297 | sum1 = MAC16_16(sum1,tmp,y3); |
| 298 | sum2 = MAC16_16(sum2,tmp,y0); |
| 299 | sum3 = MAC16_16(sum3,tmp,y1); |
| 300 | sum4 = MAC16_16(sum4,tmp,y2); |
| 301 | } |
| 302 | xcorr[i]=MAX32(-1, sum1); |
| 303 | xcorr[i+1]=MAX32(-1, sum2); |
| 304 | xcorr[i+2]=MAX32(-1, sum3); |
| 305 | xcorr[i+3]=MAX32(-1, sum4); |
| 306 | #ifdef FIXED_POINT |
| 307 | sum1 = MAX32(sum1, sum2); |
| 308 | sum3 = MAX32(sum3, sum4); |
| 309 | sum1 = MAX32(sum1, sum3); |
| 310 | maxcorr = MAX32(maxcorr, sum1); |
| 311 | #endif |
| 312 | } |
Jean-Marc Valin | 85a6618 | 2013-05-24 03:41:04 -0400 | [diff] [blame] | 313 | /* In case max_pitch isn't a multiple of 4, do non-unrolled version. */ |
| 314 | for (;i<max_pitch;i++) |
| 315 | { |
| 316 | opus_val32 sum = 0; |
| 317 | for (j=0;j<len;j++) |
| 318 | sum = MAC16_16(sum, _x[j],_y[i+j]); |
| 319 | xcorr[i] = MAX32(-1, sum); |
| 320 | #ifdef FIXED_POINT |
| 321 | maxcorr = MAX32(maxcorr, sum); |
| 322 | #endif |
| 323 | } |
Jean-Marc Valin | 559fbe8 | 2013-05-24 01:09:31 -0400 | [diff] [blame] | 324 | #ifdef FIXED_POINT |
| 325 | *maxval = maxcorr; |
| 326 | #endif |
| 327 | } |
| 328 | |
| 329 | #endif |
Gregory Maxwell | de0b532 | 2012-07-18 12:12:35 -0400 | [diff] [blame] | 330 | void pitch_search(const opus_val16 * OPUS_RESTRICT x_lp, opus_val16 * OPUS_RESTRICT y, |
Jean-Marc Valin | e3e2c26 | 2011-01-26 13:09:53 -0500 | [diff] [blame] | 331 | int len, int max_pitch, int *pitch) |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 332 | { |
| 333 | int i, j; |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 334 | int lag; |
Gregory Maxwell | 06d57b2 | 2011-08-01 22:02:25 -0400 | [diff] [blame] | 335 | int best_pitch[2]={0,0}; |
Jean-Marc Valin | ff5f722 | 2011-07-29 18:59:12 -0400 | [diff] [blame] | 336 | VARDECL(opus_val16, x_lp4); |
| 337 | VARDECL(opus_val16, y_lp4); |
| 338 | VARDECL(opus_val32, xcorr); |
Gregory Maxwell | 40f956e | 2011-09-01 19:42:37 -0400 | [diff] [blame] | 339 | #ifdef FIXED_POINT |
Jean-Marc Valin | 559fbe8 | 2013-05-24 01:09:31 -0400 | [diff] [blame] | 340 | opus_val32 maxcorr; |
Jean-Marc Valin | b7bd4c2 | 2013-05-18 23:33:48 -0400 | [diff] [blame] | 341 | opus_val32 xmax, ymax; |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 342 | int shift=0; |
Gregory Maxwell | 40f956e | 2011-09-01 19:42:37 -0400 | [diff] [blame] | 343 | #endif |
| 344 | int offset; |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 345 | |
Thorvald Natvig | 065dafd | 2009-11-25 01:02:42 +0100 | [diff] [blame] | 346 | SAVE_STACK; |
| 347 | |
Gregory Maxwell | 5d5875a | 2011-10-03 21:07:39 -0400 | [diff] [blame] | 348 | celt_assert(len>0); |
| 349 | celt_assert(max_pitch>0); |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 350 | lag = len+max_pitch; |
| 351 | |
Jean-Marc Valin | ff5f722 | 2011-07-29 18:59:12 -0400 | [diff] [blame] | 352 | ALLOC(x_lp4, len>>2, opus_val16); |
| 353 | ALLOC(y_lp4, lag>>2, opus_val16); |
| 354 | ALLOC(xcorr, max_pitch>>1, opus_val32); |
Thorvald Natvig | 065dafd | 2009-11-25 01:02:42 +0100 | [diff] [blame] | 355 | |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 356 | /* Downsample by 2 again */ |
| 357 | for (j=0;j<len>>2;j++) |
| 358 | x_lp4[j] = x_lp[2*j]; |
| 359 | for (j=0;j<lag>>2;j++) |
| 360 | y_lp4[j] = y[2*j]; |
| 361 | |
| 362 | #ifdef FIXED_POINT |
Jean-Marc Valin | 66ac102 | 2012-05-29 17:01:35 -0400 | [diff] [blame] | 363 | xmax = celt_maxabs16(x_lp4, len>>2); |
| 364 | ymax = celt_maxabs16(y_lp4, lag>>2); |
Jean-Marc Valin | b7bd4c2 | 2013-05-18 23:33:48 -0400 | [diff] [blame] | 365 | shift = celt_ilog2(MAX32(1, MAX32(xmax, ymax)))-11; |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 366 | if (shift>0) |
| 367 | { |
| 368 | for (j=0;j<len>>2;j++) |
| 369 | x_lp4[j] = SHR16(x_lp4[j], shift); |
| 370 | for (j=0;j<lag>>2;j++) |
| 371 | y_lp4[j] = SHR16(y_lp4[j], shift); |
| 372 | /* Use double the shift for a MAC */ |
| 373 | shift *= 2; |
| 374 | } else { |
| 375 | shift = 0; |
| 376 | } |
| 377 | #endif |
| 378 | |
| 379 | /* Coarse search with 4x decimation */ |
| 380 | |
Jean-Marc Valin | 559fbe8 | 2013-05-24 01:09:31 -0400 | [diff] [blame] | 381 | pitch_xcorr(x_lp4, y_lp4, xcorr, len>>2, max_pitch>>2 |
Gregory Maxwell | 40f956e | 2011-09-01 19:42:37 -0400 | [diff] [blame] | 382 | #ifdef FIXED_POINT |
Jean-Marc Valin | 559fbe8 | 2013-05-24 01:09:31 -0400 | [diff] [blame] | 383 | ,&maxcorr |
Gregory Maxwell | 40f956e | 2011-09-01 19:42:37 -0400 | [diff] [blame] | 384 | #endif |
Jean-Marc Valin | 559fbe8 | 2013-05-24 01:09:31 -0400 | [diff] [blame] | 385 | ); |
| 386 | |
Gregory Maxwell | 40f956e | 2011-09-01 19:42:37 -0400 | [diff] [blame] | 387 | find_best_pitch(xcorr, y_lp4, len>>2, max_pitch>>2, best_pitch |
| 388 | #ifdef FIXED_POINT |
| 389 | , 0, maxcorr |
| 390 | #endif |
| 391 | ); |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 392 | |
| 393 | /* Finer search with 2x decimation */ |
Gregory Maxwell | 40f956e | 2011-09-01 19:42:37 -0400 | [diff] [blame] | 394 | #ifdef FIXED_POINT |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 395 | maxcorr=1; |
Gregory Maxwell | 40f956e | 2011-09-01 19:42:37 -0400 | [diff] [blame] | 396 | #endif |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 397 | for (i=0;i<max_pitch>>1;i++) |
| 398 | { |
Jean-Marc Valin | ff5f722 | 2011-07-29 18:59:12 -0400 | [diff] [blame] | 399 | opus_val32 sum=0; |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 400 | xcorr[i] = 0; |
| 401 | if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2) |
| 402 | continue; |
| 403 | for (j=0;j<len>>1;j++) |
| 404 | sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift); |
| 405 | xcorr[i] = MAX32(-1, sum); |
Gregory Maxwell | 40f956e | 2011-09-01 19:42:37 -0400 | [diff] [blame] | 406 | #ifdef FIXED_POINT |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 407 | maxcorr = MAX32(maxcorr, sum); |
Gregory Maxwell | 40f956e | 2011-09-01 19:42:37 -0400 | [diff] [blame] | 408 | #endif |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 409 | } |
Gregory Maxwell | 40f956e | 2011-09-01 19:42:37 -0400 | [diff] [blame] | 410 | find_best_pitch(xcorr, y, len>>1, max_pitch>>1, best_pitch |
| 411 | #ifdef FIXED_POINT |
Jean-Marc Valin | 178758b | 2012-04-06 23:32:11 -0400 | [diff] [blame] | 412 | , shift+1, maxcorr |
Gregory Maxwell | 40f956e | 2011-09-01 19:42:37 -0400 | [diff] [blame] | 413 | #endif |
| 414 | ); |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 415 | |
| 416 | /* Refine by pseudo-interpolation */ |
| 417 | if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1) |
| 418 | { |
Jean-Marc Valin | ff5f722 | 2011-07-29 18:59:12 -0400 | [diff] [blame] | 419 | opus_val32 a, b, c; |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 420 | a = xcorr[best_pitch[0]-1]; |
| 421 | b = xcorr[best_pitch[0]]; |
| 422 | c = xcorr[best_pitch[0]+1]; |
| 423 | if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a)) |
| 424 | offset = 1; |
| 425 | else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c)) |
| 426 | offset = -1; |
Gregory Maxwell | 71d39ad | 2011-07-30 00:00:29 -0400 | [diff] [blame] | 427 | else |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 428 | offset = 0; |
| 429 | } else { |
| 430 | offset = 0; |
| 431 | } |
| 432 | *pitch = 2*best_pitch[0]-offset; |
| 433 | |
Thorvald Natvig | 065dafd | 2009-11-25 01:02:42 +0100 | [diff] [blame] | 434 | RESTORE_STACK; |
Jean-Marc Valin | 294863b | 2009-11-08 22:29:54 +0900 | [diff] [blame] | 435 | } |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 436 | |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 437 | static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2}; |
Jean-Marc Valin | ff5f722 | 2011-07-29 18:59:12 -0400 | [diff] [blame] | 438 | opus_val16 remove_doubling(opus_val16 *x, int maxperiod, int minperiod, |
Ralph Giles | 120800f | 2011-11-25 13:02:00 -0800 | [diff] [blame] | 439 | int N, int *T0_, int prev_period, opus_val16 prev_gain) |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 440 | { |
Gregory Maxwell | b8a6b31 | 2011-02-03 22:56:01 -0500 | [diff] [blame] | 441 | int k, i, T, T0; |
Jean-Marc Valin | ff5f722 | 2011-07-29 18:59:12 -0400 | [diff] [blame] | 442 | opus_val16 g, g0; |
| 443 | opus_val16 pg; |
| 444 | opus_val32 xy,xx,yy; |
| 445 | opus_val32 xcorr[3]; |
| 446 | opus_val32 best_xy, best_yy; |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 447 | int offset; |
Jean-Marc Valin | d121260 | 2011-01-25 13:11:36 -0500 | [diff] [blame] | 448 | int minperiod0; |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 449 | |
Jean-Marc Valin | d121260 | 2011-01-25 13:11:36 -0500 | [diff] [blame] | 450 | minperiod0 = minperiod; |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 451 | maxperiod /= 2; |
| 452 | minperiod /= 2; |
Ralph Giles | 120800f | 2011-11-25 13:02:00 -0800 | [diff] [blame] | 453 | *T0_ /= 2; |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 454 | prev_period /= 2; |
| 455 | N /= 2; |
| 456 | x += maxperiod; |
Ralph Giles | 120800f | 2011-11-25 13:02:00 -0800 | [diff] [blame] | 457 | if (*T0_>=maxperiod) |
| 458 | *T0_=maxperiod-1; |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 459 | |
Ralph Giles | 120800f | 2011-11-25 13:02:00 -0800 | [diff] [blame] | 460 | T = T0 = *T0_; |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 461 | xx=xy=yy=0; |
| 462 | for (i=0;i<N;i++) |
| 463 | { |
| 464 | xy = MAC16_16(xy, x[i], x[i-T0]); |
| 465 | xx = MAC16_16(xx, x[i], x[i]); |
| 466 | yy = MAC16_16(yy, x[i-T0],x[i-T0]); |
| 467 | } |
| 468 | best_xy = xy; |
| 469 | best_yy = yy; |
| 470 | #ifdef FIXED_POINT |
| 471 | { |
Jean-Marc Valin | ff5f722 | 2011-07-29 18:59:12 -0400 | [diff] [blame] | 472 | opus_val32 x2y2; |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 473 | int sh, t; |
| 474 | x2y2 = 1+HALF32(MULT32_32_Q31(xx,yy)); |
| 475 | sh = celt_ilog2(x2y2)>>1; |
| 476 | t = VSHR32(x2y2, 2*(sh-7)); |
| 477 | g = g0 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1); |
| 478 | } |
| 479 | #else |
Gregory Maxwell | 662587d | 2011-08-01 20:41:54 -0400 | [diff] [blame] | 480 | g = g0 = xy/celt_sqrt(1+xx*yy); |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 481 | #endif |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 482 | /* Look for any pitch at T/k */ |
| 483 | for (k=2;k<=15;k++) |
| 484 | { |
| 485 | int T1, T1b; |
Jean-Marc Valin | ff5f722 | 2011-07-29 18:59:12 -0400 | [diff] [blame] | 486 | opus_val16 g1; |
| 487 | opus_val16 cont=0; |
Jean-Marc Valin | 0892c16 | 2012-01-12 03:44:49 -0500 | [diff] [blame] | 488 | opus_val16 thresh; |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 489 | T1 = (2*T0+k)/(2*k); |
| 490 | if (T1 < minperiod) |
| 491 | break; |
| 492 | /* Look for another strong correlation at T1b */ |
| 493 | if (k==2) |
| 494 | { |
| 495 | if (T1+T0>maxperiod) |
| 496 | T1b = T0; |
| 497 | else |
| 498 | T1b = T0+T1; |
| 499 | } else |
| 500 | { |
| 501 | T1b = (2*second_check[k]*T0+k)/(2*k); |
| 502 | } |
| 503 | xy=yy=0; |
| 504 | for (i=0;i<N;i++) |
| 505 | { |
| 506 | xy = MAC16_16(xy, x[i], x[i-T1]); |
| 507 | yy = MAC16_16(yy, x[i-T1], x[i-T1]); |
| 508 | |
| 509 | xy = MAC16_16(xy, x[i], x[i-T1b]); |
| 510 | yy = MAC16_16(yy, x[i-T1b], x[i-T1b]); |
| 511 | } |
| 512 | #ifdef FIXED_POINT |
| 513 | { |
Jean-Marc Valin | ff5f722 | 2011-07-29 18:59:12 -0400 | [diff] [blame] | 514 | opus_val32 x2y2; |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 515 | int sh, t; |
| 516 | x2y2 = 1+MULT32_32_Q31(xx,yy); |
| 517 | sh = celt_ilog2(x2y2)>>1; |
| 518 | t = VSHR32(x2y2, 2*(sh-7)); |
| 519 | g1 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1); |
| 520 | } |
| 521 | #else |
Gregory Maxwell | 662587d | 2011-08-01 20:41:54 -0400 | [diff] [blame] | 522 | g1 = xy/celt_sqrt(1+2.f*xx*1.f*yy); |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 523 | #endif |
| 524 | if (abs(T1-prev_period)<=1) |
| 525 | cont = prev_gain; |
| 526 | else if (abs(T1-prev_period)<=2 && 5*k*k < T0) |
| 527 | cont = HALF32(prev_gain); |
| 528 | else |
| 529 | cont = 0; |
Ralph Giles | 027ec51 | 2012-10-23 10:49:18 -0700 | [diff] [blame] | 530 | thresh = MAX16(QCONST16(.3f,15), MULT16_16_Q15(QCONST16(.7f,15),g0)-cont); |
Jean-Marc Valin | 0892c16 | 2012-01-12 03:44:49 -0500 | [diff] [blame] | 531 | /* Bias against very high pitch (very short period) to avoid false-positives |
| 532 | due to short-term correlation */ |
| 533 | if (T1<3*minperiod) |
Ralph Giles | 027ec51 | 2012-10-23 10:49:18 -0700 | [diff] [blame] | 534 | thresh = MAX16(QCONST16(.4f,15), MULT16_16_Q15(QCONST16(.85f,15),g0)-cont); |
Jean-Marc Valin | 0892c16 | 2012-01-12 03:44:49 -0500 | [diff] [blame] | 535 | else if (T1<2*minperiod) |
Ralph Giles | 027ec51 | 2012-10-23 10:49:18 -0700 | [diff] [blame] | 536 | thresh = MAX16(QCONST16(.5f,15), MULT16_16_Q15(QCONST16(.9f,15),g0)-cont); |
Jean-Marc Valin | 0892c16 | 2012-01-12 03:44:49 -0500 | [diff] [blame] | 537 | if (g1 > thresh) |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 538 | { |
| 539 | best_xy = xy; |
| 540 | best_yy = yy; |
| 541 | T = T1; |
| 542 | g = g1; |
| 543 | } |
| 544 | } |
Jean-Marc Valin | b3deb53 | 2012-04-24 17:00:54 -0400 | [diff] [blame] | 545 | best_xy = MAX32(0, best_xy); |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 546 | if (best_yy <= best_xy) |
| 547 | pg = Q15ONE; |
| 548 | else |
| 549 | pg = SHR32(frac_div32(best_xy,best_yy+1),16); |
| 550 | |
| 551 | for (k=0;k<3;k++) |
| 552 | { |
| 553 | int T1 = T+k-1; |
| 554 | xy = 0; |
| 555 | for (i=0;i<N;i++) |
| 556 | xy = MAC16_16(xy, x[i], x[i-T1]); |
| 557 | xcorr[k] = xy; |
| 558 | } |
| 559 | if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0])) |
| 560 | offset = 1; |
| 561 | else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2])) |
| 562 | offset = -1; |
| 563 | else |
| 564 | offset = 0; |
| 565 | if (pg > g) |
| 566 | pg = g; |
Ralph Giles | 120800f | 2011-11-25 13:02:00 -0800 | [diff] [blame] | 567 | *T0_ = 2*T+offset; |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 568 | |
Ralph Giles | 120800f | 2011-11-25 13:02:00 -0800 | [diff] [blame] | 569 | if (*T0_<minperiod0) |
| 570 | *T0_=minperiod0; |
Jean-Marc Valin | 35095c6 | 2010-11-04 13:24:44 -0400 | [diff] [blame] | 571 | return pg; |
| 572 | } |