Tristan Matthews | 0a329cc | 2013-07-17 13:20:14 -0400 | [diff] [blame] | 1 | /* Copyright (C) 2002-2006 Jean-Marc Valin |
| 2 | File: ltp.c |
| 3 | Long-Term Prediction functions |
| 4 | |
| 5 | Redistribution and use in source and binary forms, with or without |
| 6 | modification, are permitted provided that the following conditions |
| 7 | are met: |
| 8 | |
| 9 | - Redistributions of source code must retain the above copyright |
| 10 | notice, this list of conditions and the following disclaimer. |
| 11 | |
| 12 | - Redistributions in binary form must reproduce the above copyright |
| 13 | notice, this list of conditions and the following disclaimer in the |
| 14 | documentation and/or other materials provided with the distribution. |
| 15 | |
| 16 | - Neither the name of the Xiph.org Foundation nor the names of its |
| 17 | contributors may be used to endorse or promote products derived from |
| 18 | this software without specific prior written permission. |
| 19 | |
| 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
| 21 | ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
| 22 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR |
| 23 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR |
| 24 | CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, |
| 25 | EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, |
| 26 | PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR |
| 27 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF |
| 28 | LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING |
| 29 | NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS |
| 30 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
| 31 | */ |
| 32 | |
| 33 | #ifdef HAVE_CONFIG_H |
| 34 | #include "config.h" |
| 35 | #endif |
| 36 | |
| 37 | #include <math.h> |
| 38 | #include "ltp.h" |
| 39 | #include "stack_alloc.h" |
| 40 | #include "filters.h" |
| 41 | #include <speex/speex_bits.h> |
| 42 | #include "math_approx.h" |
| 43 | #include "os_support.h" |
| 44 | |
| 45 | #ifndef NULL |
| 46 | #define NULL 0 |
| 47 | #endif |
| 48 | |
| 49 | |
| 50 | #ifdef _USE_SSE |
| 51 | #include "ltp_sse.h" |
| 52 | #elif defined (ARM4_ASM) || defined(ARM5E_ASM) |
| 53 | #include "ltp_arm4.h" |
| 54 | #elif defined (BFIN_ASM) |
| 55 | #include "ltp_bfin.h" |
| 56 | #endif |
| 57 | |
| 58 | #ifndef OVERRIDE_INNER_PROD |
| 59 | spx_word32_t inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len) |
| 60 | { |
| 61 | spx_word32_t sum=0; |
| 62 | len >>= 2; |
| 63 | while(len--) |
| 64 | { |
| 65 | spx_word32_t part=0; |
| 66 | part = MAC16_16(part,*x++,*y++); |
| 67 | part = MAC16_16(part,*x++,*y++); |
| 68 | part = MAC16_16(part,*x++,*y++); |
| 69 | part = MAC16_16(part,*x++,*y++); |
| 70 | /* HINT: If you had a 40-bit accumulator, you could shift only at the end */ |
| 71 | sum = ADD32(sum,SHR32(part,6)); |
| 72 | } |
| 73 | return sum; |
| 74 | } |
| 75 | #endif |
| 76 | |
| 77 | #ifndef OVERRIDE_PITCH_XCORR |
| 78 | #if 0 /* HINT: Enable this for machines with enough registers (i.e. not x86) */ |
| 79 | void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack) |
| 80 | { |
| 81 | int i,j; |
| 82 | for (i=0;i<nb_pitch;i+=4) |
| 83 | { |
| 84 | /* Compute correlation*/ |
| 85 | /*corr[nb_pitch-1-i]=inner_prod(x, _y+i, len);*/ |
| 86 | spx_word32_t sum1=0; |
| 87 | spx_word32_t sum2=0; |
| 88 | spx_word32_t sum3=0; |
| 89 | spx_word32_t sum4=0; |
| 90 | const spx_word16_t *y = _y+i; |
| 91 | const spx_word16_t *x = _x; |
| 92 | spx_word16_t y0, y1, y2, y3; |
| 93 | /*y0=y[0];y1=y[1];y2=y[2];y3=y[3];*/ |
| 94 | y0=*y++; |
| 95 | y1=*y++; |
| 96 | y2=*y++; |
| 97 | y3=*y++; |
| 98 | for (j=0;j<len;j+=4) |
| 99 | { |
| 100 | spx_word32_t part1; |
| 101 | spx_word32_t part2; |
| 102 | spx_word32_t part3; |
| 103 | spx_word32_t part4; |
| 104 | part1 = MULT16_16(*x,y0); |
| 105 | part2 = MULT16_16(*x,y1); |
| 106 | part3 = MULT16_16(*x,y2); |
| 107 | part4 = MULT16_16(*x,y3); |
| 108 | x++; |
| 109 | y0=*y++; |
| 110 | part1 = MAC16_16(part1,*x,y1); |
| 111 | part2 = MAC16_16(part2,*x,y2); |
| 112 | part3 = MAC16_16(part3,*x,y3); |
| 113 | part4 = MAC16_16(part4,*x,y0); |
| 114 | x++; |
| 115 | y1=*y++; |
| 116 | part1 = MAC16_16(part1,*x,y2); |
| 117 | part2 = MAC16_16(part2,*x,y3); |
| 118 | part3 = MAC16_16(part3,*x,y0); |
| 119 | part4 = MAC16_16(part4,*x,y1); |
| 120 | x++; |
| 121 | y2=*y++; |
| 122 | part1 = MAC16_16(part1,*x,y3); |
| 123 | part2 = MAC16_16(part2,*x,y0); |
| 124 | part3 = MAC16_16(part3,*x,y1); |
| 125 | part4 = MAC16_16(part4,*x,y2); |
| 126 | x++; |
| 127 | y3=*y++; |
| 128 | |
| 129 | sum1 = ADD32(sum1,SHR32(part1,6)); |
| 130 | sum2 = ADD32(sum2,SHR32(part2,6)); |
| 131 | sum3 = ADD32(sum3,SHR32(part3,6)); |
| 132 | sum4 = ADD32(sum4,SHR32(part4,6)); |
| 133 | } |
| 134 | corr[nb_pitch-1-i]=sum1; |
| 135 | corr[nb_pitch-2-i]=sum2; |
| 136 | corr[nb_pitch-3-i]=sum3; |
| 137 | corr[nb_pitch-4-i]=sum4; |
| 138 | } |
| 139 | |
| 140 | } |
| 141 | #else |
| 142 | void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack) |
| 143 | { |
| 144 | int i; |
| 145 | for (i=0;i<nb_pitch;i++) |
| 146 | { |
| 147 | /* Compute correlation*/ |
| 148 | corr[nb_pitch-1-i]=inner_prod(_x, _y+i, len); |
| 149 | } |
| 150 | |
| 151 | } |
| 152 | #endif |
| 153 | #endif |
| 154 | |
| 155 | #ifndef OVERRIDE_COMPUTE_PITCH_ERROR |
| 156 | static inline spx_word32_t compute_pitch_error(spx_word16_t *C, spx_word16_t *g, spx_word16_t pitch_control) |
| 157 | { |
| 158 | spx_word32_t sum = 0; |
| 159 | sum = ADD32(sum,MULT16_16(MULT16_16_16(g[0],pitch_control),C[0])); |
| 160 | sum = ADD32(sum,MULT16_16(MULT16_16_16(g[1],pitch_control),C[1])); |
| 161 | sum = ADD32(sum,MULT16_16(MULT16_16_16(g[2],pitch_control),C[2])); |
| 162 | sum = SUB32(sum,MULT16_16(MULT16_16_16(g[0],g[1]),C[3])); |
| 163 | sum = SUB32(sum,MULT16_16(MULT16_16_16(g[2],g[1]),C[4])); |
| 164 | sum = SUB32(sum,MULT16_16(MULT16_16_16(g[2],g[0]),C[5])); |
| 165 | sum = SUB32(sum,MULT16_16(MULT16_16_16(g[0],g[0]),C[6])); |
| 166 | sum = SUB32(sum,MULT16_16(MULT16_16_16(g[1],g[1]),C[7])); |
| 167 | sum = SUB32(sum,MULT16_16(MULT16_16_16(g[2],g[2]),C[8])); |
| 168 | return sum; |
| 169 | } |
| 170 | #endif |
| 171 | |
| 172 | #ifndef OVERRIDE_OPEN_LOOP_NBEST_PITCH |
| 173 | void open_loop_nbest_pitch(spx_word16_t *sw, int start, int end, int len, int *pitch, spx_word16_t *gain, int N, char *stack) |
| 174 | { |
| 175 | int i,j,k; |
| 176 | VARDECL(spx_word32_t *best_score); |
| 177 | VARDECL(spx_word32_t *best_ener); |
| 178 | spx_word32_t e0; |
| 179 | VARDECL(spx_word32_t *corr); |
| 180 | #ifdef FIXED_POINT |
| 181 | /* In fixed-point, we need only one (temporary) array of 32-bit values and two (corr16, ener16) |
| 182 | arrays for (normalized) 16-bit values */ |
| 183 | VARDECL(spx_word16_t *corr16); |
| 184 | VARDECL(spx_word16_t *ener16); |
| 185 | spx_word32_t *energy; |
| 186 | int cshift=0, eshift=0; |
| 187 | int scaledown = 0; |
| 188 | ALLOC(corr16, end-start+1, spx_word16_t); |
| 189 | ALLOC(ener16, end-start+1, spx_word16_t); |
| 190 | ALLOC(corr, end-start+1, spx_word32_t); |
| 191 | energy = corr; |
| 192 | #else |
| 193 | /* In floating-point, we need to float arrays and no normalized copies */ |
| 194 | VARDECL(spx_word32_t *energy); |
| 195 | spx_word16_t *corr16; |
| 196 | spx_word16_t *ener16; |
| 197 | ALLOC(energy, end-start+2, spx_word32_t); |
| 198 | ALLOC(corr, end-start+1, spx_word32_t); |
| 199 | corr16 = corr; |
| 200 | ener16 = energy; |
| 201 | #endif |
| 202 | |
| 203 | ALLOC(best_score, N, spx_word32_t); |
| 204 | ALLOC(best_ener, N, spx_word32_t); |
| 205 | for (i=0;i<N;i++) |
| 206 | { |
| 207 | best_score[i]=-1; |
| 208 | best_ener[i]=0; |
| 209 | pitch[i]=start; |
| 210 | } |
| 211 | |
| 212 | #ifdef FIXED_POINT |
| 213 | for (i=-end;i<len;i++) |
| 214 | { |
| 215 | if (ABS16(sw[i])>16383) |
| 216 | { |
| 217 | scaledown=1; |
| 218 | break; |
| 219 | } |
| 220 | } |
| 221 | /* If the weighted input is close to saturation, then we scale it down */ |
| 222 | if (scaledown) |
| 223 | { |
| 224 | for (i=-end;i<len;i++) |
| 225 | { |
| 226 | sw[i]=SHR16(sw[i],1); |
| 227 | } |
| 228 | } |
| 229 | #endif |
| 230 | energy[0]=inner_prod(sw-start, sw-start, len); |
| 231 | e0=inner_prod(sw, sw, len); |
| 232 | for (i=start;i<end;i++) |
| 233 | { |
| 234 | /* Update energy for next pitch*/ |
| 235 | energy[i-start+1] = SUB32(ADD32(energy[i-start],SHR32(MULT16_16(sw[-i-1],sw[-i-1]),6)), SHR32(MULT16_16(sw[-i+len-1],sw[-i+len-1]),6)); |
| 236 | if (energy[i-start+1] < 0) |
| 237 | energy[i-start+1] = 0; |
| 238 | } |
| 239 | |
| 240 | #ifdef FIXED_POINT |
| 241 | eshift = normalize16(energy, ener16, 32766, end-start+1); |
| 242 | #endif |
| 243 | |
| 244 | /* In fixed-point, this actually overrites the energy array (aliased to corr) */ |
| 245 | pitch_xcorr(sw, sw-end, corr, len, end-start+1, stack); |
| 246 | |
| 247 | #ifdef FIXED_POINT |
| 248 | /* Normalize to 180 so we can square it and it still fits in 16 bits */ |
| 249 | cshift = normalize16(corr, corr16, 180, end-start+1); |
| 250 | /* If we scaled weighted input down, we need to scale it up again (OK, so we've just lost the LSB, who cares?) */ |
| 251 | if (scaledown) |
| 252 | { |
| 253 | for (i=-end;i<len;i++) |
| 254 | { |
| 255 | sw[i]=SHL16(sw[i],1); |
| 256 | } |
| 257 | } |
| 258 | #endif |
| 259 | |
| 260 | /* Search for the best pitch prediction gain */ |
| 261 | for (i=start;i<=end;i++) |
| 262 | { |
| 263 | spx_word16_t tmp = MULT16_16_16(corr16[i-start],corr16[i-start]); |
| 264 | /* Instead of dividing the tmp by the energy, we multiply on the other side */ |
| 265 | if (MULT16_16(tmp,best_ener[N-1])>MULT16_16(best_score[N-1],ADD16(1,ener16[i-start]))) |
| 266 | { |
| 267 | /* We can safely put it last and then check */ |
| 268 | best_score[N-1]=tmp; |
| 269 | best_ener[N-1]=ener16[i-start]+1; |
| 270 | pitch[N-1]=i; |
| 271 | /* Check if it comes in front of others */ |
| 272 | for (j=0;j<N-1;j++) |
| 273 | { |
| 274 | if (MULT16_16(tmp,best_ener[j])>MULT16_16(best_score[j],ADD16(1,ener16[i-start]))) |
| 275 | { |
| 276 | for (k=N-1;k>j;k--) |
| 277 | { |
| 278 | best_score[k]=best_score[k-1]; |
| 279 | best_ener[k]=best_ener[k-1]; |
| 280 | pitch[k]=pitch[k-1]; |
| 281 | } |
| 282 | best_score[j]=tmp; |
| 283 | best_ener[j]=ener16[i-start]+1; |
| 284 | pitch[j]=i; |
| 285 | break; |
| 286 | } |
| 287 | } |
| 288 | } |
| 289 | } |
| 290 | |
| 291 | /* Compute open-loop gain if necessary */ |
| 292 | if (gain) |
| 293 | { |
| 294 | for (j=0;j<N;j++) |
| 295 | { |
| 296 | spx_word16_t g; |
| 297 | i=pitch[j]; |
| 298 | g = DIV32(SHL32(EXTEND32(corr16[i-start]),cshift), 10+SHR32(MULT16_16(spx_sqrt(e0),spx_sqrt(SHL32(EXTEND32(ener16[i-start]),eshift))),6)); |
| 299 | /* FIXME: g = max(g,corr/energy) */ |
| 300 | if (g<0) |
| 301 | g = 0; |
| 302 | gain[j]=g; |
| 303 | } |
| 304 | } |
| 305 | |
| 306 | |
| 307 | } |
| 308 | #endif |
| 309 | |
| 310 | #ifndef OVERRIDE_PITCH_GAIN_SEARCH_3TAP_VQ |
| 311 | static int pitch_gain_search_3tap_vq( |
| 312 | const signed char *gain_cdbk, |
| 313 | int gain_cdbk_size, |
| 314 | spx_word16_t *C16, |
| 315 | spx_word16_t max_gain |
| 316 | ) |
| 317 | { |
| 318 | const signed char *ptr=gain_cdbk; |
| 319 | int best_cdbk=0; |
| 320 | spx_word32_t best_sum=-VERY_LARGE32; |
| 321 | spx_word32_t sum=0; |
| 322 | spx_word16_t g[3]; |
| 323 | spx_word16_t pitch_control=64; |
| 324 | spx_word16_t gain_sum; |
| 325 | int i; |
| 326 | |
| 327 | for (i=0;i<gain_cdbk_size;i++) { |
| 328 | |
| 329 | ptr = gain_cdbk+4*i; |
| 330 | g[0]=ADD16((spx_word16_t)ptr[0],32); |
| 331 | g[1]=ADD16((spx_word16_t)ptr[1],32); |
| 332 | g[2]=ADD16((spx_word16_t)ptr[2],32); |
| 333 | gain_sum = (spx_word16_t)ptr[3]; |
| 334 | |
| 335 | sum = compute_pitch_error(C16, g, pitch_control); |
| 336 | |
| 337 | if (sum>best_sum && gain_sum<=max_gain) { |
| 338 | best_sum=sum; |
| 339 | best_cdbk=i; |
| 340 | } |
| 341 | } |
| 342 | |
| 343 | return best_cdbk; |
| 344 | } |
| 345 | #endif |
| 346 | |
| 347 | /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */ |
| 348 | static spx_word32_t pitch_gain_search_3tap( |
| 349 | const spx_word16_t target[], /* Target vector */ |
| 350 | const spx_coef_t ak[], /* LPCs for this subframe */ |
| 351 | const spx_coef_t awk1[], /* Weighted LPCs #1 for this subframe */ |
| 352 | const spx_coef_t awk2[], /* Weighted LPCs #2 for this subframe */ |
| 353 | spx_sig_t exc[], /* Excitation */ |
| 354 | const signed char *gain_cdbk, |
| 355 | int gain_cdbk_size, |
| 356 | int pitch, /* Pitch value */ |
| 357 | int p, /* Number of LPC coeffs */ |
| 358 | int nsf, /* Number of samples in subframe */ |
| 359 | SpeexBits *bits, |
| 360 | char *stack, |
| 361 | const spx_word16_t *exc2, |
| 362 | const spx_word16_t *r, |
| 363 | spx_word16_t *new_target, |
| 364 | int *cdbk_index, |
| 365 | int plc_tuning, |
| 366 | spx_word32_t cumul_gain, |
| 367 | int scaledown |
| 368 | ) |
| 369 | { |
| 370 | int i,j; |
| 371 | VARDECL(spx_word16_t *tmp1); |
| 372 | VARDECL(spx_word16_t *e); |
| 373 | spx_word16_t *x[3]; |
| 374 | spx_word32_t corr[3]; |
| 375 | spx_word32_t A[3][3]; |
| 376 | spx_word16_t gain[3]; |
| 377 | spx_word32_t err; |
| 378 | spx_word16_t max_gain=128; |
| 379 | int best_cdbk=0; |
| 380 | |
| 381 | ALLOC(tmp1, 3*nsf, spx_word16_t); |
| 382 | ALLOC(e, nsf, spx_word16_t); |
| 383 | |
| 384 | if (cumul_gain > 262144) |
| 385 | max_gain = 31; |
| 386 | |
| 387 | x[0]=tmp1; |
| 388 | x[1]=tmp1+nsf; |
| 389 | x[2]=tmp1+2*nsf; |
| 390 | |
| 391 | for (j=0;j<nsf;j++) |
| 392 | new_target[j] = target[j]; |
| 393 | |
| 394 | { |
| 395 | VARDECL(spx_mem_t *mm); |
| 396 | int pp=pitch-1; |
| 397 | ALLOC(mm, p, spx_mem_t); |
| 398 | for (j=0;j<nsf;j++) |
| 399 | { |
| 400 | if (j-pp<0) |
| 401 | e[j]=exc2[j-pp]; |
| 402 | else if (j-pp-pitch<0) |
| 403 | e[j]=exc2[j-pp-pitch]; |
| 404 | else |
| 405 | e[j]=0; |
| 406 | } |
| 407 | #ifdef FIXED_POINT |
| 408 | /* Scale target and excitation down if needed (avoiding overflow) */ |
| 409 | if (scaledown) |
| 410 | { |
| 411 | for (j=0;j<nsf;j++) |
| 412 | e[j] = SHR16(e[j],1); |
| 413 | for (j=0;j<nsf;j++) |
| 414 | new_target[j] = SHR16(new_target[j],1); |
| 415 | } |
| 416 | #endif |
| 417 | for (j=0;j<p;j++) |
| 418 | mm[j] = 0; |
| 419 | iir_mem16(e, ak, e, nsf, p, mm, stack); |
| 420 | for (j=0;j<p;j++) |
| 421 | mm[j] = 0; |
| 422 | filter_mem16(e, awk1, awk2, e, nsf, p, mm, stack); |
| 423 | for (j=0;j<nsf;j++) |
| 424 | x[2][j] = e[j]; |
| 425 | } |
| 426 | for (i=1;i>=0;i--) |
| 427 | { |
| 428 | spx_word16_t e0=exc2[-pitch-1+i]; |
| 429 | #ifdef FIXED_POINT |
| 430 | /* Scale excitation down if needed (avoiding overflow) */ |
| 431 | if (scaledown) |
| 432 | e0 = SHR16(e0,1); |
| 433 | #endif |
| 434 | x[i][0]=MULT16_16_Q14(r[0], e0); |
| 435 | for (j=0;j<nsf-1;j++) |
| 436 | x[i][j+1]=ADD32(x[i+1][j],MULT16_16_P14(r[j+1], e0)); |
| 437 | } |
| 438 | |
| 439 | for (i=0;i<3;i++) |
| 440 | corr[i]=inner_prod(x[i],new_target,nsf); |
| 441 | for (i=0;i<3;i++) |
| 442 | for (j=0;j<=i;j++) |
| 443 | A[i][j]=A[j][i]=inner_prod(x[i],x[j],nsf); |
| 444 | |
| 445 | { |
| 446 | spx_word32_t C[9]; |
| 447 | #ifdef FIXED_POINT |
| 448 | spx_word16_t C16[9]; |
| 449 | #else |
| 450 | spx_word16_t *C16=C; |
| 451 | #endif |
| 452 | C[0]=corr[2]; |
| 453 | C[1]=corr[1]; |
| 454 | C[2]=corr[0]; |
| 455 | C[3]=A[1][2]; |
| 456 | C[4]=A[0][1]; |
| 457 | C[5]=A[0][2]; |
| 458 | C[6]=A[2][2]; |
| 459 | C[7]=A[1][1]; |
| 460 | C[8]=A[0][0]; |
| 461 | |
| 462 | /*plc_tuning *= 2;*/ |
| 463 | if (plc_tuning<2) |
| 464 | plc_tuning=2; |
| 465 | if (plc_tuning>30) |
| 466 | plc_tuning=30; |
| 467 | #ifdef FIXED_POINT |
| 468 | C[0] = SHL32(C[0],1); |
| 469 | C[1] = SHL32(C[1],1); |
| 470 | C[2] = SHL32(C[2],1); |
| 471 | C[3] = SHL32(C[3],1); |
| 472 | C[4] = SHL32(C[4],1); |
| 473 | C[5] = SHL32(C[5],1); |
| 474 | C[6] = MAC16_32_Q15(C[6],MULT16_16_16(plc_tuning,655),C[6]); |
| 475 | C[7] = MAC16_32_Q15(C[7],MULT16_16_16(plc_tuning,655),C[7]); |
| 476 | C[8] = MAC16_32_Q15(C[8],MULT16_16_16(plc_tuning,655),C[8]); |
| 477 | normalize16(C, C16, 32767, 9); |
| 478 | #else |
| 479 | C[6]*=.5*(1+.02*plc_tuning); |
| 480 | C[7]*=.5*(1+.02*plc_tuning); |
| 481 | C[8]*=.5*(1+.02*plc_tuning); |
| 482 | #endif |
| 483 | |
| 484 | best_cdbk = pitch_gain_search_3tap_vq(gain_cdbk, gain_cdbk_size, C16, max_gain); |
| 485 | |
| 486 | #ifdef FIXED_POINT |
| 487 | gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4]); |
| 488 | gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4+1]); |
| 489 | gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4+2]); |
| 490 | /*printf ("%d %d %d %d\n",gain[0],gain[1],gain[2], best_cdbk);*/ |
| 491 | #else |
| 492 | gain[0] = 0.015625*gain_cdbk[best_cdbk*4] + .5; |
| 493 | gain[1] = 0.015625*gain_cdbk[best_cdbk*4+1]+ .5; |
| 494 | gain[2] = 0.015625*gain_cdbk[best_cdbk*4+2]+ .5; |
| 495 | #endif |
| 496 | *cdbk_index=best_cdbk; |
| 497 | } |
| 498 | |
| 499 | SPEEX_MEMSET(exc, 0, nsf); |
| 500 | for (i=0;i<3;i++) |
| 501 | { |
| 502 | int j; |
| 503 | int tmp1, tmp3; |
| 504 | int pp=pitch+1-i; |
| 505 | tmp1=nsf; |
| 506 | if (tmp1>pp) |
| 507 | tmp1=pp; |
| 508 | for (j=0;j<tmp1;j++) |
| 509 | exc[j]=MAC16_16(exc[j],SHL16(gain[2-i],7),exc2[j-pp]); |
| 510 | tmp3=nsf; |
| 511 | if (tmp3>pp+pitch) |
| 512 | tmp3=pp+pitch; |
| 513 | for (j=tmp1;j<tmp3;j++) |
| 514 | exc[j]=MAC16_16(exc[j],SHL16(gain[2-i],7),exc2[j-pp-pitch]); |
| 515 | } |
| 516 | for (i=0;i<nsf;i++) |
| 517 | { |
| 518 | spx_word32_t tmp = ADD32(ADD32(MULT16_16(gain[0],x[2][i]),MULT16_16(gain[1],x[1][i])), |
| 519 | MULT16_16(gain[2],x[0][i])); |
| 520 | new_target[i] = SUB16(new_target[i], EXTRACT16(PSHR32(tmp,6))); |
| 521 | } |
| 522 | err = inner_prod(new_target, new_target, nsf); |
| 523 | |
| 524 | return err; |
| 525 | } |
| 526 | |
| 527 | /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */ |
| 528 | int pitch_search_3tap( |
| 529 | spx_word16_t target[], /* Target vector */ |
| 530 | spx_word16_t *sw, |
| 531 | spx_coef_t ak[], /* LPCs for this subframe */ |
| 532 | spx_coef_t awk1[], /* Weighted LPCs #1 for this subframe */ |
| 533 | spx_coef_t awk2[], /* Weighted LPCs #2 for this subframe */ |
| 534 | spx_sig_t exc[], /* Excitation */ |
| 535 | const void *par, |
| 536 | int start, /* Smallest pitch value allowed */ |
| 537 | int end, /* Largest pitch value allowed */ |
| 538 | spx_word16_t pitch_coef, /* Voicing (pitch) coefficient */ |
| 539 | int p, /* Number of LPC coeffs */ |
| 540 | int nsf, /* Number of samples in subframe */ |
| 541 | SpeexBits *bits, |
| 542 | char *stack, |
| 543 | spx_word16_t *exc2, |
| 544 | spx_word16_t *r, |
| 545 | int complexity, |
| 546 | int cdbk_offset, |
| 547 | int plc_tuning, |
| 548 | spx_word32_t *cumul_gain |
| 549 | ) |
| 550 | { |
| 551 | int i; |
| 552 | int cdbk_index, pitch=0, best_gain_index=0; |
| 553 | VARDECL(spx_sig_t *best_exc); |
| 554 | VARDECL(spx_word16_t *new_target); |
| 555 | VARDECL(spx_word16_t *best_target); |
| 556 | int best_pitch=0; |
| 557 | spx_word32_t err, best_err=-1; |
| 558 | int N; |
| 559 | const ltp_params *params; |
| 560 | const signed char *gain_cdbk; |
| 561 | int gain_cdbk_size; |
| 562 | int scaledown=0; |
| 563 | |
| 564 | VARDECL(int *nbest); |
| 565 | |
| 566 | params = (const ltp_params*) par; |
| 567 | gain_cdbk_size = 1<<params->gain_bits; |
| 568 | gain_cdbk = params->gain_cdbk + 4*gain_cdbk_size*cdbk_offset; |
| 569 | |
| 570 | N=complexity; |
| 571 | if (N>10) |
| 572 | N=10; |
| 573 | if (N<1) |
| 574 | N=1; |
| 575 | |
| 576 | ALLOC(nbest, N, int); |
| 577 | params = (const ltp_params*) par; |
| 578 | |
| 579 | if (end<start) |
| 580 | { |
| 581 | speex_bits_pack(bits, 0, params->pitch_bits); |
| 582 | speex_bits_pack(bits, 0, params->gain_bits); |
| 583 | SPEEX_MEMSET(exc, 0, nsf); |
| 584 | return start; |
| 585 | } |
| 586 | |
| 587 | #ifdef FIXED_POINT |
| 588 | /* Check if we need to scale everything down in the pitch search to avoid overflows */ |
| 589 | for (i=0;i<nsf;i++) |
| 590 | { |
| 591 | if (ABS16(target[i])>16383) |
| 592 | { |
| 593 | scaledown=1; |
| 594 | break; |
| 595 | } |
| 596 | } |
| 597 | for (i=-end;i<nsf;i++) |
| 598 | { |
| 599 | if (ABS16(exc2[i])>16383) |
| 600 | { |
| 601 | scaledown=1; |
| 602 | break; |
| 603 | } |
| 604 | } |
| 605 | #endif |
| 606 | if (N>end-start+1) |
| 607 | N=end-start+1; |
| 608 | if (end != start) |
| 609 | open_loop_nbest_pitch(sw, start, end, nsf, nbest, NULL, N, stack); |
| 610 | else |
| 611 | nbest[0] = start; |
| 612 | |
| 613 | ALLOC(best_exc, nsf, spx_sig_t); |
| 614 | ALLOC(new_target, nsf, spx_word16_t); |
| 615 | ALLOC(best_target, nsf, spx_word16_t); |
| 616 | |
| 617 | for (i=0;i<N;i++) |
| 618 | { |
| 619 | pitch=nbest[i]; |
| 620 | SPEEX_MEMSET(exc, 0, nsf); |
| 621 | err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, gain_cdbk, gain_cdbk_size, pitch, p, nsf, |
| 622 | bits, stack, exc2, r, new_target, &cdbk_index, plc_tuning, *cumul_gain, scaledown); |
| 623 | if (err<best_err || best_err<0) |
| 624 | { |
| 625 | SPEEX_COPY(best_exc, exc, nsf); |
| 626 | SPEEX_COPY(best_target, new_target, nsf); |
| 627 | best_err=err; |
| 628 | best_pitch=pitch; |
| 629 | best_gain_index=cdbk_index; |
| 630 | } |
| 631 | } |
| 632 | /*printf ("pitch: %d %d\n", best_pitch, best_gain_index);*/ |
| 633 | speex_bits_pack(bits, best_pitch-start, params->pitch_bits); |
| 634 | speex_bits_pack(bits, best_gain_index, params->gain_bits); |
| 635 | #ifdef FIXED_POINT |
| 636 | *cumul_gain = MULT16_32_Q13(SHL16(params->gain_cdbk[4*best_gain_index+3],8), MAX32(1024,*cumul_gain)); |
| 637 | #else |
| 638 | *cumul_gain = 0.03125*MAX32(1024,*cumul_gain)*params->gain_cdbk[4*best_gain_index+3]; |
| 639 | #endif |
| 640 | /*printf ("%f\n", cumul_gain);*/ |
| 641 | /*printf ("encode pitch: %d %d\n", best_pitch, best_gain_index);*/ |
| 642 | SPEEX_COPY(exc, best_exc, nsf); |
| 643 | SPEEX_COPY(target, best_target, nsf); |
| 644 | #ifdef FIXED_POINT |
| 645 | /* Scale target back up if needed */ |
| 646 | if (scaledown) |
| 647 | { |
| 648 | for (i=0;i<nsf;i++) |
| 649 | target[i]=SHL16(target[i],1); |
| 650 | } |
| 651 | #endif |
| 652 | return pitch; |
| 653 | } |
| 654 | |
| 655 | void pitch_unquant_3tap( |
| 656 | spx_word16_t exc[], /* Input excitation */ |
| 657 | spx_word32_t exc_out[], /* Output excitation */ |
| 658 | int start, /* Smallest pitch value allowed */ |
| 659 | int end, /* Largest pitch value allowed */ |
| 660 | spx_word16_t pitch_coef, /* Voicing (pitch) coefficient */ |
| 661 | const void *par, |
| 662 | int nsf, /* Number of samples in subframe */ |
| 663 | int *pitch_val, |
| 664 | spx_word16_t *gain_val, |
| 665 | SpeexBits *bits, |
| 666 | char *stack, |
| 667 | int count_lost, |
| 668 | int subframe_offset, |
| 669 | spx_word16_t last_pitch_gain, |
| 670 | int cdbk_offset |
| 671 | ) |
| 672 | { |
| 673 | int i; |
| 674 | int pitch; |
| 675 | int gain_index; |
| 676 | spx_word16_t gain[3]; |
| 677 | const signed char *gain_cdbk; |
| 678 | int gain_cdbk_size; |
| 679 | const ltp_params *params; |
| 680 | |
| 681 | params = (const ltp_params*) par; |
| 682 | gain_cdbk_size = 1<<params->gain_bits; |
| 683 | gain_cdbk = params->gain_cdbk + 4*gain_cdbk_size*cdbk_offset; |
| 684 | |
| 685 | pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits); |
| 686 | pitch += start; |
| 687 | gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits); |
| 688 | /*printf ("decode pitch: %d %d\n", pitch, gain_index);*/ |
| 689 | #ifdef FIXED_POINT |
| 690 | gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4]); |
| 691 | gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4+1]); |
| 692 | gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4+2]); |
| 693 | #else |
| 694 | gain[0] = 0.015625*gain_cdbk[gain_index*4]+.5; |
| 695 | gain[1] = 0.015625*gain_cdbk[gain_index*4+1]+.5; |
| 696 | gain[2] = 0.015625*gain_cdbk[gain_index*4+2]+.5; |
| 697 | #endif |
| 698 | |
| 699 | if (count_lost && pitch > subframe_offset) |
| 700 | { |
| 701 | spx_word16_t gain_sum; |
| 702 | if (1) { |
| 703 | #ifdef FIXED_POINT |
| 704 | spx_word16_t tmp = count_lost < 4 ? last_pitch_gain : SHR16(last_pitch_gain,1); |
| 705 | if (tmp>62) |
| 706 | tmp=62; |
| 707 | #else |
| 708 | spx_word16_t tmp = count_lost < 4 ? last_pitch_gain : 0.5 * last_pitch_gain; |
| 709 | if (tmp>.95) |
| 710 | tmp=.95; |
| 711 | #endif |
| 712 | gain_sum = gain_3tap_to_1tap(gain); |
| 713 | |
| 714 | if (gain_sum > tmp) |
| 715 | { |
| 716 | spx_word16_t fact = DIV32_16(SHL32(EXTEND32(tmp),14),gain_sum); |
| 717 | for (i=0;i<3;i++) |
| 718 | gain[i]=MULT16_16_Q14(fact,gain[i]); |
| 719 | } |
| 720 | |
| 721 | } |
| 722 | |
| 723 | } |
| 724 | |
| 725 | *pitch_val = pitch; |
| 726 | gain_val[0]=gain[0]; |
| 727 | gain_val[1]=gain[1]; |
| 728 | gain_val[2]=gain[2]; |
| 729 | gain[0] = SHL16(gain[0],7); |
| 730 | gain[1] = SHL16(gain[1],7); |
| 731 | gain[2] = SHL16(gain[2],7); |
| 732 | SPEEX_MEMSET(exc_out, 0, nsf); |
| 733 | for (i=0;i<3;i++) |
| 734 | { |
| 735 | int j; |
| 736 | int tmp1, tmp3; |
| 737 | int pp=pitch+1-i; |
| 738 | tmp1=nsf; |
| 739 | if (tmp1>pp) |
| 740 | tmp1=pp; |
| 741 | for (j=0;j<tmp1;j++) |
| 742 | exc_out[j]=MAC16_16(exc_out[j],gain[2-i],exc[j-pp]); |
| 743 | tmp3=nsf; |
| 744 | if (tmp3>pp+pitch) |
| 745 | tmp3=pp+pitch; |
| 746 | for (j=tmp1;j<tmp3;j++) |
| 747 | exc_out[j]=MAC16_16(exc_out[j],gain[2-i],exc[j-pp-pitch]); |
| 748 | } |
| 749 | /*for (i=0;i<nsf;i++) |
| 750 | exc[i]=PSHR32(exc32[i],13);*/ |
| 751 | } |
| 752 | |
| 753 | |
| 754 | /** Forced pitch delay and gain */ |
| 755 | int forced_pitch_quant( |
| 756 | spx_word16_t target[], /* Target vector */ |
| 757 | spx_word16_t *sw, |
| 758 | spx_coef_t ak[], /* LPCs for this subframe */ |
| 759 | spx_coef_t awk1[], /* Weighted LPCs #1 for this subframe */ |
| 760 | spx_coef_t awk2[], /* Weighted LPCs #2 for this subframe */ |
| 761 | spx_sig_t exc[], /* Excitation */ |
| 762 | const void *par, |
| 763 | int start, /* Smallest pitch value allowed */ |
| 764 | int end, /* Largest pitch value allowed */ |
| 765 | spx_word16_t pitch_coef, /* Voicing (pitch) coefficient */ |
| 766 | int p, /* Number of LPC coeffs */ |
| 767 | int nsf, /* Number of samples in subframe */ |
| 768 | SpeexBits *bits, |
| 769 | char *stack, |
| 770 | spx_word16_t *exc2, |
| 771 | spx_word16_t *r, |
| 772 | int complexity, |
| 773 | int cdbk_offset, |
| 774 | int plc_tuning, |
| 775 | spx_word32_t *cumul_gain |
| 776 | ) |
| 777 | { |
| 778 | int i; |
| 779 | VARDECL(spx_word16_t *res); |
| 780 | ALLOC(res, nsf, spx_word16_t); |
| 781 | #ifdef FIXED_POINT |
| 782 | if (pitch_coef>63) |
| 783 | pitch_coef=63; |
| 784 | #else |
| 785 | if (pitch_coef>.99) |
| 786 | pitch_coef=.99; |
| 787 | #endif |
| 788 | for (i=0;i<nsf&&i<start;i++) |
| 789 | { |
| 790 | exc[i]=MULT16_16(SHL16(pitch_coef, 7),exc2[i-start]); |
| 791 | } |
| 792 | for (;i<nsf;i++) |
| 793 | { |
| 794 | exc[i]=MULT16_32_Q15(SHL16(pitch_coef, 9),exc[i-start]); |
| 795 | } |
| 796 | for (i=0;i<nsf;i++) |
| 797 | res[i] = EXTRACT16(PSHR32(exc[i], SIG_SHIFT-1)); |
| 798 | syn_percep_zero16(res, ak, awk1, awk2, res, nsf, p, stack); |
| 799 | for (i=0;i<nsf;i++) |
| 800 | target[i]=EXTRACT16(SATURATE(SUB32(EXTEND32(target[i]),EXTEND32(res[i])),32700)); |
| 801 | return start; |
| 802 | } |
| 803 | |
| 804 | /** Unquantize forced pitch delay and gain */ |
| 805 | void forced_pitch_unquant( |
| 806 | spx_word16_t exc[], /* Input excitation */ |
| 807 | spx_word32_t exc_out[], /* Output excitation */ |
| 808 | int start, /* Smallest pitch value allowed */ |
| 809 | int end, /* Largest pitch value allowed */ |
| 810 | spx_word16_t pitch_coef, /* Voicing (pitch) coefficient */ |
| 811 | const void *par, |
| 812 | int nsf, /* Number of samples in subframe */ |
| 813 | int *pitch_val, |
| 814 | spx_word16_t *gain_val, |
| 815 | SpeexBits *bits, |
| 816 | char *stack, |
| 817 | int count_lost, |
| 818 | int subframe_offset, |
| 819 | spx_word16_t last_pitch_gain, |
| 820 | int cdbk_offset |
| 821 | ) |
| 822 | { |
| 823 | int i; |
| 824 | #ifdef FIXED_POINT |
| 825 | if (pitch_coef>63) |
| 826 | pitch_coef=63; |
| 827 | #else |
| 828 | if (pitch_coef>.99) |
| 829 | pitch_coef=.99; |
| 830 | #endif |
| 831 | for (i=0;i<nsf;i++) |
| 832 | { |
| 833 | exc_out[i]=MULT16_16(exc[i-start],SHL16(pitch_coef,7)); |
| 834 | exc[i] = EXTRACT16(PSHR32(exc_out[i],13)); |
| 835 | } |
| 836 | *pitch_val = start; |
| 837 | gain_val[0]=gain_val[2]=0; |
| 838 | gain_val[1] = pitch_coef; |
| 839 | } |