OLD | NEW |
(Empty) | |
| 1 /* Copyright (C) 2002-2006 Jean-Marc Valin |
| 2 File: filters.c |
| 3 Various analysis/synthesis filters |
| 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 "filters.h" |
| 38 #include "stack_alloc.h" |
| 39 #include "arch.h" |
| 40 #include "math_approx.h" |
| 41 #include "ltp.h" |
| 42 #include <math.h> |
| 43 |
| 44 #ifdef _USE_SSE |
| 45 #include "filters_sse.h" |
| 46 #elif defined (ARM4_ASM) || defined(ARM5E_ASM) |
| 47 #include "filters_arm4.h" |
| 48 #elif defined (BFIN_ASM) |
| 49 #include "filters_bfin.h" |
| 50 #endif |
| 51 |
| 52 |
| 53 |
| 54 void bw_lpc(spx_word16_t gamma, const spx_coef_t *lpc_in, spx_coef_t *lpc_out, i
nt order) |
| 55 { |
| 56 int i; |
| 57 spx_word16_t tmp=gamma; |
| 58 for (i=0;i<order;i++) |
| 59 { |
| 60 lpc_out[i] = MULT16_16_P15(tmp,lpc_in[i]); |
| 61 tmp = MULT16_16_P15(tmp, gamma); |
| 62 } |
| 63 } |
| 64 |
| 65 void sanitize_values32(spx_word32_t *vec, spx_word32_t min_val, spx_word32_t max
_val, int len) |
| 66 { |
| 67 int i; |
| 68 for (i=0;i<len;i++) |
| 69 { |
| 70 /* It's important we do the test that way so we can catch NaNs, which are
neither greater nor smaller */ |
| 71 if (!(vec[i]>=min_val && vec[i] <= max_val)) |
| 72 { |
| 73 if (vec[i] < min_val) |
| 74 vec[i] = min_val; |
| 75 else if (vec[i] > max_val) |
| 76 vec[i] = max_val; |
| 77 else /* Has to be NaN */ |
| 78 vec[i] = 0; |
| 79 } |
| 80 } |
| 81 } |
| 82 |
| 83 void highpass(const spx_word16_t *x, spx_word16_t *y, int len, int filtID, spx_m
em_t *mem) |
| 84 { |
| 85 int i; |
| 86 #ifdef FIXED_POINT |
| 87 const spx_word16_t Pcoef[5][3] = {{16384, -31313, 14991}, {16384, -31569, 152
49}, {16384, -31677, 15328}, {16384, -32313, 15947}, {16384, -22446, 6537}}; |
| 88 const spx_word16_t Zcoef[5][3] = {{15672, -31344, 15672}, {15802, -31601, 158
02}, {15847, -31694, 15847}, {16162, -32322, 16162}, {14418, -28836, 14418}}; |
| 89 #else |
| 90 const spx_word16_t Pcoef[5][3] = {{1.00000f, -1.91120f, 0.91498f}, {1.00000f,
-1.92683f, 0.93071f}, {1.00000f, -1.93338f, 0.93553f}, {1.00000f, -1.97226f, 0.
97332f}, {1.00000f, -1.37000f, 0.39900f}}; |
| 91 const spx_word16_t Zcoef[5][3] = {{0.95654f, -1.91309f, 0.95654f}, {0.96446f,
-1.92879f, 0.96446f}, {0.96723f, -1.93445f, 0.96723f}, {0.98645f, -1.97277f, 0.
98645f}, {0.88000f, -1.76000f, 0.88000f}}; |
| 92 #endif |
| 93 const spx_word16_t *den, *num; |
| 94 if (filtID>4) |
| 95 filtID=4; |
| 96 |
| 97 den = Pcoef[filtID]; num = Zcoef[filtID]; |
| 98 /*return;*/ |
| 99 for (i=0;i<len;i++) |
| 100 { |
| 101 spx_word16_t yi; |
| 102 spx_word32_t vout = ADD32(MULT16_16(num[0], x[i]),mem[0]); |
| 103 yi = EXTRACT16(SATURATE(PSHR32(vout,14),32767)); |
| 104 mem[0] = ADD32(MAC16_16(mem[1], num[1],x[i]), SHL32(MULT16_32_Q15(-den[1],
vout),1)); |
| 105 mem[1] = ADD32(MULT16_16(num[2],x[i]), SHL32(MULT16_32_Q15(-den[2],vout),1
)); |
| 106 y[i] = yi; |
| 107 } |
| 108 } |
| 109 |
| 110 #ifdef FIXED_POINT |
| 111 |
| 112 /* FIXME: These functions are ugly and probably introduce too much error */ |
| 113 void signal_mul(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len) |
| 114 { |
| 115 int i; |
| 116 for (i=0;i<len;i++) |
| 117 { |
| 118 y[i] = SHL32(MULT16_32_Q14(EXTRACT16(SHR32(x[i],7)),scale),7); |
| 119 } |
| 120 } |
| 121 |
| 122 void signal_div(const spx_word16_t *x, spx_word16_t *y, spx_word32_t scale, int
len) |
| 123 { |
| 124 int i; |
| 125 if (scale > SHL32(EXTEND32(SIG_SCALING), 8)) |
| 126 { |
| 127 spx_word16_t scale_1; |
| 128 scale = PSHR32(scale, SIG_SHIFT); |
| 129 scale_1 = EXTRACT16(PDIV32_16(SHL32(EXTEND32(SIG_SCALING),7),scale)); |
| 130 for (i=0;i<len;i++) |
| 131 { |
| 132 y[i] = MULT16_16_P15(scale_1, x[i]); |
| 133 } |
| 134 } else if (scale > SHR32(EXTEND32(SIG_SCALING), 2)) { |
| 135 spx_word16_t scale_1; |
| 136 scale = PSHR32(scale, SIG_SHIFT-5); |
| 137 scale_1 = DIV32_16(SHL32(EXTEND32(SIG_SCALING),3),scale); |
| 138 for (i=0;i<len;i++) |
| 139 { |
| 140 y[i] = PSHR32(MULT16_16(scale_1, SHL16(x[i],2)),8); |
| 141 } |
| 142 } else { |
| 143 spx_word16_t scale_1; |
| 144 scale = PSHR32(scale, SIG_SHIFT-7); |
| 145 if (scale < 5) |
| 146 scale = 5; |
| 147 scale_1 = DIV32_16(SHL32(EXTEND32(SIG_SCALING),3),scale); |
| 148 for (i=0;i<len;i++) |
| 149 { |
| 150 y[i] = PSHR32(MULT16_16(scale_1, SHL16(x[i],2)),6); |
| 151 } |
| 152 } |
| 153 } |
| 154 |
| 155 #else |
| 156 |
| 157 void signal_mul(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len) |
| 158 { |
| 159 int i; |
| 160 for (i=0;i<len;i++) |
| 161 y[i] = scale*x[i]; |
| 162 } |
| 163 |
| 164 void signal_div(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len) |
| 165 { |
| 166 int i; |
| 167 float scale_1 = 1/scale; |
| 168 for (i=0;i<len;i++) |
| 169 y[i] = scale_1*x[i]; |
| 170 } |
| 171 #endif |
| 172 |
| 173 |
| 174 |
| 175 #ifdef FIXED_POINT |
| 176 |
| 177 |
| 178 |
| 179 spx_word16_t compute_rms(const spx_sig_t *x, int len) |
| 180 { |
| 181 int i; |
| 182 spx_word32_t sum=0; |
| 183 spx_sig_t max_val=1; |
| 184 int sig_shift; |
| 185 |
| 186 for (i=0;i<len;i++) |
| 187 { |
| 188 spx_sig_t tmp = x[i]; |
| 189 if (tmp<0) |
| 190 tmp = -tmp; |
| 191 if (tmp > max_val) |
| 192 max_val = tmp; |
| 193 } |
| 194 |
| 195 sig_shift=0; |
| 196 while (max_val>16383) |
| 197 { |
| 198 sig_shift++; |
| 199 max_val >>= 1; |
| 200 } |
| 201 |
| 202 for (i=0;i<len;i+=4) |
| 203 { |
| 204 spx_word32_t sum2=0; |
| 205 spx_word16_t tmp; |
| 206 tmp = EXTRACT16(SHR32(x[i],sig_shift)); |
| 207 sum2 = MAC16_16(sum2,tmp,tmp); |
| 208 tmp = EXTRACT16(SHR32(x[i+1],sig_shift)); |
| 209 sum2 = MAC16_16(sum2,tmp,tmp); |
| 210 tmp = EXTRACT16(SHR32(x[i+2],sig_shift)); |
| 211 sum2 = MAC16_16(sum2,tmp,tmp); |
| 212 tmp = EXTRACT16(SHR32(x[i+3],sig_shift)); |
| 213 sum2 = MAC16_16(sum2,tmp,tmp); |
| 214 sum = ADD32(sum,SHR32(sum2,6)); |
| 215 } |
| 216 |
| 217 return EXTRACT16(PSHR32(SHL32(EXTEND32(spx_sqrt(DIV32(sum,len))),(sig_shift+3
)),SIG_SHIFT)); |
| 218 } |
| 219 |
| 220 spx_word16_t compute_rms16(const spx_word16_t *x, int len) |
| 221 { |
| 222 int i; |
| 223 spx_word16_t max_val=10; |
| 224 |
| 225 for (i=0;i<len;i++) |
| 226 { |
| 227 spx_sig_t tmp = x[i]; |
| 228 if (tmp<0) |
| 229 tmp = -tmp; |
| 230 if (tmp > max_val) |
| 231 max_val = tmp; |
| 232 } |
| 233 if (max_val>16383) |
| 234 { |
| 235 spx_word32_t sum=0; |
| 236 for (i=0;i<len;i+=4) |
| 237 { |
| 238 spx_word32_t sum2=0; |
| 239 sum2 = MAC16_16(sum2,SHR16(x[i],1),SHR16(x[i],1)); |
| 240 sum2 = MAC16_16(sum2,SHR16(x[i+1],1),SHR16(x[i+1],1)); |
| 241 sum2 = MAC16_16(sum2,SHR16(x[i+2],1),SHR16(x[i+2],1)); |
| 242 sum2 = MAC16_16(sum2,SHR16(x[i+3],1),SHR16(x[i+3],1)); |
| 243 sum = ADD32(sum,SHR32(sum2,6)); |
| 244 } |
| 245 return SHL16(spx_sqrt(DIV32(sum,len)),4); |
| 246 } else { |
| 247 spx_word32_t sum=0; |
| 248 int sig_shift=0; |
| 249 if (max_val < 8192) |
| 250 sig_shift=1; |
| 251 if (max_val < 4096) |
| 252 sig_shift=2; |
| 253 if (max_val < 2048) |
| 254 sig_shift=3; |
| 255 for (i=0;i<len;i+=4) |
| 256 { |
| 257 spx_word32_t sum2=0; |
| 258 sum2 = MAC16_16(sum2,SHL16(x[i],sig_shift),SHL16(x[i],sig_shift)); |
| 259 sum2 = MAC16_16(sum2,SHL16(x[i+1],sig_shift),SHL16(x[i+1],sig_shift)); |
| 260 sum2 = MAC16_16(sum2,SHL16(x[i+2],sig_shift),SHL16(x[i+2],sig_shift)); |
| 261 sum2 = MAC16_16(sum2,SHL16(x[i+3],sig_shift),SHL16(x[i+3],sig_shift)); |
| 262 sum = ADD32(sum,SHR32(sum2,6)); |
| 263 } |
| 264 return SHL16(spx_sqrt(DIV32(sum,len)),3-sig_shift); |
| 265 } |
| 266 } |
| 267 |
| 268 #ifndef OVERRIDE_NORMALIZE16 |
| 269 int normalize16(const spx_sig_t *x, spx_word16_t *y, spx_sig_t max_scale, int le
n) |
| 270 { |
| 271 int i; |
| 272 spx_sig_t max_val=1; |
| 273 int sig_shift; |
| 274 |
| 275 for (i=0;i<len;i++) |
| 276 { |
| 277 spx_sig_t tmp = x[i]; |
| 278 if (tmp<0) |
| 279 tmp = NEG32(tmp); |
| 280 if (tmp >= max_val) |
| 281 max_val = tmp; |
| 282 } |
| 283 |
| 284 sig_shift=0; |
| 285 while (max_val>max_scale) |
| 286 { |
| 287 sig_shift++; |
| 288 max_val >>= 1; |
| 289 } |
| 290 |
| 291 for (i=0;i<len;i++) |
| 292 y[i] = EXTRACT16(SHR32(x[i], sig_shift)); |
| 293 |
| 294 return sig_shift; |
| 295 } |
| 296 #endif |
| 297 |
| 298 #else |
| 299 |
| 300 spx_word16_t compute_rms(const spx_sig_t *x, int len) |
| 301 { |
| 302 int i; |
| 303 float sum=0; |
| 304 for (i=0;i<len;i++) |
| 305 { |
| 306 sum += x[i]*x[i]; |
| 307 } |
| 308 return sqrt(.1+sum/len); |
| 309 } |
| 310 spx_word16_t compute_rms16(const spx_word16_t *x, int len) |
| 311 { |
| 312 return compute_rms(x, len); |
| 313 } |
| 314 #endif |
| 315 |
| 316 |
| 317 |
| 318 #ifndef OVERRIDE_FILTER_MEM16 |
| 319 void filter_mem16(const spx_word16_t *x, const spx_coef_t *num, const spx_coef_t
*den, spx_word16_t *y, int N, int ord, spx_mem_t *mem, char *stack) |
| 320 { |
| 321 int i,j; |
| 322 spx_word16_t xi,yi,nyi; |
| 323 for (i=0;i<N;i++) |
| 324 { |
| 325 xi= x[i]; |
| 326 yi = EXTRACT16(SATURATE(ADD32(EXTEND32(x[i]),PSHR32(mem[0],LPC_SHIFT)),327
67)); |
| 327 nyi = NEG16(yi); |
| 328 for (j=0;j<ord-1;j++) |
| 329 { |
| 330 mem[j] = MAC16_16(MAC16_16(mem[j+1], num[j],xi), den[j],nyi); |
| 331 } |
| 332 mem[ord-1] = ADD32(MULT16_16(num[ord-1],xi), MULT16_16(den[ord-1],nyi)); |
| 333 y[i] = yi; |
| 334 } |
| 335 } |
| 336 #endif |
| 337 |
| 338 #ifndef OVERRIDE_IIR_MEM16 |
| 339 void iir_mem16(const spx_word16_t *x, const spx_coef_t *den, spx_word16_t *y, in
t N, int ord, spx_mem_t *mem, char *stack) |
| 340 { |
| 341 int i,j; |
| 342 spx_word16_t yi,nyi; |
| 343 |
| 344 for (i=0;i<N;i++) |
| 345 { |
| 346 yi = EXTRACT16(SATURATE(ADD32(EXTEND32(x[i]),PSHR32(mem[0],LPC_SHIFT)),327
67)); |
| 347 nyi = NEG16(yi); |
| 348 for (j=0;j<ord-1;j++) |
| 349 { |
| 350 mem[j] = MAC16_16(mem[j+1],den[j],nyi); |
| 351 } |
| 352 mem[ord-1] = MULT16_16(den[ord-1],nyi); |
| 353 y[i] = yi; |
| 354 } |
| 355 } |
| 356 #endif |
| 357 |
| 358 #ifndef OVERRIDE_FIR_MEM16 |
| 359 void fir_mem16(const spx_word16_t *x, const spx_coef_t *num, spx_word16_t *y, in
t N, int ord, spx_mem_t *mem, char *stack) |
| 360 { |
| 361 int i,j; |
| 362 spx_word16_t xi,yi; |
| 363 |
| 364 for (i=0;i<N;i++) |
| 365 { |
| 366 xi=x[i]; |
| 367 yi = EXTRACT16(SATURATE(ADD32(EXTEND32(x[i]),PSHR32(mem[0],LPC_SHIFT)),327
67)); |
| 368 for (j=0;j<ord-1;j++) |
| 369 { |
| 370 mem[j] = MAC16_16(mem[j+1], num[j],xi); |
| 371 } |
| 372 mem[ord-1] = MULT16_16(num[ord-1],xi); |
| 373 y[i] = yi; |
| 374 } |
| 375 } |
| 376 #endif |
| 377 |
| 378 |
| 379 void syn_percep_zero16(const spx_word16_t *xx, const spx_coef_t *ak, const spx_c
oef_t *awk1, const spx_coef_t *awk2, spx_word16_t *y, int N, int ord, char *stac
k) |
| 380 { |
| 381 int i; |
| 382 VARDECL(spx_mem_t *mem); |
| 383 ALLOC(mem, ord, spx_mem_t); |
| 384 for (i=0;i<ord;i++) |
| 385 mem[i]=0; |
| 386 iir_mem16(xx, ak, y, N, ord, mem, stack); |
| 387 for (i=0;i<ord;i++) |
| 388 mem[i]=0; |
| 389 filter_mem16(y, awk1, awk2, y, N, ord, mem, stack); |
| 390 } |
| 391 void residue_percep_zero16(const spx_word16_t *xx, const spx_coef_t *ak, const s
px_coef_t *awk1, const spx_coef_t *awk2, spx_word16_t *y, int N, int ord, char *
stack) |
| 392 { |
| 393 int i; |
| 394 VARDECL(spx_mem_t *mem); |
| 395 ALLOC(mem, ord, spx_mem_t); |
| 396 for (i=0;i<ord;i++) |
| 397 mem[i]=0; |
| 398 filter_mem16(xx, ak, awk1, y, N, ord, mem, stack); |
| 399 for (i=0;i<ord;i++) |
| 400 mem[i]=0; |
| 401 fir_mem16(y, awk2, y, N, ord, mem, stack); |
| 402 } |
| 403 |
| 404 |
| 405 #ifndef OVERRIDE_COMPUTE_IMPULSE_RESPONSE |
| 406 void compute_impulse_response(const spx_coef_t *ak, const spx_coef_t *awk1, cons
t spx_coef_t *awk2, spx_word16_t *y, int N, int ord, char *stack) |
| 407 { |
| 408 int i,j; |
| 409 spx_word16_t y1, ny1i, ny2i; |
| 410 VARDECL(spx_mem_t *mem1); |
| 411 VARDECL(spx_mem_t *mem2); |
| 412 ALLOC(mem1, ord, spx_mem_t); |
| 413 ALLOC(mem2, ord, spx_mem_t); |
| 414 |
| 415 y[0] = LPC_SCALING; |
| 416 for (i=0;i<ord;i++) |
| 417 y[i+1] = awk1[i]; |
| 418 i++; |
| 419 for (;i<N;i++) |
| 420 y[i] = VERY_SMALL; |
| 421 for (i=0;i<ord;i++) |
| 422 mem1[i] = mem2[i] = 0; |
| 423 for (i=0;i<N;i++) |
| 424 { |
| 425 y1 = ADD16(y[i], EXTRACT16(PSHR32(mem1[0],LPC_SHIFT))); |
| 426 ny1i = NEG16(y1); |
| 427 y[i] = PSHR32(ADD32(SHL32(EXTEND32(y1),LPC_SHIFT+1),mem2[0]),LPC_SHIFT); |
| 428 ny2i = NEG16(y[i]); |
| 429 for (j=0;j<ord-1;j++) |
| 430 { |
| 431 mem1[j] = MAC16_16(mem1[j+1], awk2[j],ny1i); |
| 432 mem2[j] = MAC16_16(mem2[j+1], ak[j],ny2i); |
| 433 } |
| 434 mem1[ord-1] = MULT16_16(awk2[ord-1],ny1i); |
| 435 mem2[ord-1] = MULT16_16(ak[ord-1],ny2i); |
| 436 } |
| 437 } |
| 438 #endif |
| 439 |
| 440 /* Decomposes a signal into low-band and high-band using a QMF */ |
| 441 void qmf_decomp(const spx_word16_t *xx, const spx_word16_t *aa, spx_word16_t *y1
, spx_word16_t *y2, int N, int M, spx_word16_t *mem, char *stack) |
| 442 { |
| 443 int i,j,k,M2; |
| 444 VARDECL(spx_word16_t *a); |
| 445 VARDECL(spx_word16_t *x); |
| 446 spx_word16_t *x2; |
| 447 |
| 448 ALLOC(a, M, spx_word16_t); |
| 449 ALLOC(x, N+M-1, spx_word16_t); |
| 450 x2=x+M-1; |
| 451 M2=M>>1; |
| 452 for (i=0;i<M;i++) |
| 453 a[M-i-1]= aa[i]; |
| 454 for (i=0;i<M-1;i++) |
| 455 x[i]=mem[M-i-2]; |
| 456 for (i=0;i<N;i++) |
| 457 x[i+M-1]=SHR16(xx[i],1); |
| 458 for (i=0;i<M-1;i++) |
| 459 mem[i]=SHR16(xx[N-i-1],1); |
| 460 for (i=0,k=0;i<N;i+=2,k++) |
| 461 { |
| 462 spx_word32_t y1k=0, y2k=0; |
| 463 for (j=0;j<M2;j++) |
| 464 { |
| 465 y1k=ADD32(y1k,MULT16_16(a[j],ADD16(x[i+j],x2[i-j]))); |
| 466 y2k=SUB32(y2k,MULT16_16(a[j],SUB16(x[i+j],x2[i-j]))); |
| 467 j++; |
| 468 y1k=ADD32(y1k,MULT16_16(a[j],ADD16(x[i+j],x2[i-j]))); |
| 469 y2k=ADD32(y2k,MULT16_16(a[j],SUB16(x[i+j],x2[i-j]))); |
| 470 } |
| 471 y1[k] = EXTRACT16(SATURATE(PSHR32(y1k,15),32767)); |
| 472 y2[k] = EXTRACT16(SATURATE(PSHR32(y2k,15),32767)); |
| 473 } |
| 474 } |
| 475 |
| 476 /* Re-synthesised a signal from the QMF low-band and high-band signals */ |
| 477 void qmf_synth(const spx_word16_t *x1, const spx_word16_t *x2, const spx_word16_
t *a, spx_word16_t *y, int N, int M, spx_word16_t *mem1, spx_word16_t *mem2, cha
r *stack) |
| 478 /* assumptions: |
| 479 all odd x[i] are zero -- well, actually they are left out of the array now |
| 480 N and M are multiples of 4 */ |
| 481 { |
| 482 int i, j; |
| 483 int M2, N2; |
| 484 VARDECL(spx_word16_t *xx1); |
| 485 VARDECL(spx_word16_t *xx2); |
| 486 |
| 487 M2 = M>>1; |
| 488 N2 = N>>1; |
| 489 ALLOC(xx1, M2+N2, spx_word16_t); |
| 490 ALLOC(xx2, M2+N2, spx_word16_t); |
| 491 |
| 492 for (i = 0; i < N2; i++) |
| 493 xx1[i] = x1[N2-1-i]; |
| 494 for (i = 0; i < M2; i++) |
| 495 xx1[N2+i] = mem1[2*i+1]; |
| 496 for (i = 0; i < N2; i++) |
| 497 xx2[i] = x2[N2-1-i]; |
| 498 for (i = 0; i < M2; i++) |
| 499 xx2[N2+i] = mem2[2*i+1]; |
| 500 |
| 501 for (i = 0; i < N2; i += 2) { |
| 502 spx_sig_t y0, y1, y2, y3; |
| 503 spx_word16_t x10, x20; |
| 504 |
| 505 y0 = y1 = y2 = y3 = 0; |
| 506 x10 = xx1[N2-2-i]; |
| 507 x20 = xx2[N2-2-i]; |
| 508 |
| 509 for (j = 0; j < M2; j += 2) { |
| 510 spx_word16_t x11, x21; |
| 511 spx_word16_t a0, a1; |
| 512 |
| 513 a0 = a[2*j]; |
| 514 a1 = a[2*j+1]; |
| 515 x11 = xx1[N2-1+j-i]; |
| 516 x21 = xx2[N2-1+j-i]; |
| 517 |
| 518 #ifdef FIXED_POINT |
| 519 /* We multiply twice by the same coef to avoid overflows */ |
| 520 y0 = MAC16_16(MAC16_16(y0, a0, x11), NEG16(a0), x21); |
| 521 y1 = MAC16_16(MAC16_16(y1, a1, x11), a1, x21); |
| 522 y2 = MAC16_16(MAC16_16(y2, a0, x10), NEG16(a0), x20); |
| 523 y3 = MAC16_16(MAC16_16(y3, a1, x10), a1, x20); |
| 524 #else |
| 525 y0 = ADD32(y0,MULT16_16(a0, x11-x21)); |
| 526 y1 = ADD32(y1,MULT16_16(a1, x11+x21)); |
| 527 y2 = ADD32(y2,MULT16_16(a0, x10-x20)); |
| 528 y3 = ADD32(y3,MULT16_16(a1, x10+x20)); |
| 529 #endif |
| 530 a0 = a[2*j+2]; |
| 531 a1 = a[2*j+3]; |
| 532 x10 = xx1[N2+j-i]; |
| 533 x20 = xx2[N2+j-i]; |
| 534 |
| 535 #ifdef FIXED_POINT |
| 536 /* We multiply twice by the same coef to avoid overflows */ |
| 537 y0 = MAC16_16(MAC16_16(y0, a0, x10), NEG16(a0), x20); |
| 538 y1 = MAC16_16(MAC16_16(y1, a1, x10), a1, x20); |
| 539 y2 = MAC16_16(MAC16_16(y2, a0, x11), NEG16(a0), x21); |
| 540 y3 = MAC16_16(MAC16_16(y3, a1, x11), a1, x21); |
| 541 #else |
| 542 y0 = ADD32(y0,MULT16_16(a0, x10-x20)); |
| 543 y1 = ADD32(y1,MULT16_16(a1, x10+x20)); |
| 544 y2 = ADD32(y2,MULT16_16(a0, x11-x21)); |
| 545 y3 = ADD32(y3,MULT16_16(a1, x11+x21)); |
| 546 #endif |
| 547 } |
| 548 #ifdef FIXED_POINT |
| 549 y[2*i] = EXTRACT16(SATURATE32(PSHR32(y0,15),32767)); |
| 550 y[2*i+1] = EXTRACT16(SATURATE32(PSHR32(y1,15),32767)); |
| 551 y[2*i+2] = EXTRACT16(SATURATE32(PSHR32(y2,15),32767)); |
| 552 y[2*i+3] = EXTRACT16(SATURATE32(PSHR32(y3,15),32767)); |
| 553 #else |
| 554 /* Normalize up explicitly if we're in float */ |
| 555 y[2*i] = 2.f*y0; |
| 556 y[2*i+1] = 2.f*y1; |
| 557 y[2*i+2] = 2.f*y2; |
| 558 y[2*i+3] = 2.f*y3; |
| 559 #endif |
| 560 } |
| 561 |
| 562 for (i = 0; i < M2; i++) |
| 563 mem1[2*i+1] = xx1[i]; |
| 564 for (i = 0; i < M2; i++) |
| 565 mem2[2*i+1] = xx2[i]; |
| 566 } |
| 567 |
| 568 #ifdef FIXED_POINT |
| 569 #if 0 |
| 570 const spx_word16_t shift_filt[3][7] = {{-33, 1043, -4551, 19959, 19959,
-4551, 1043}, |
| 571 {-98, 1133, -4425, 29179, 8895, -23
28, 444}, |
| 572 {444, -2328, 8895, 29179, -4425, 11
33, -98}}; |
| 573 #else |
| 574 const spx_word16_t shift_filt[3][7] = {{-390, 1540, -4993, 20123, 20123
, -4993, 1540}, |
| 575 {-1064, 2817, -6694, 31589, 6837, -
990, -209}, |
| 576 {-209, -990, 6837, 31589, -6694, 2
817, -1064}}; |
| 577 #endif |
| 578 #else |
| 579 #if 0 |
| 580 const float shift_filt[3][7] = {{-9.9369e-04, 3.1831e-02, -1.3889e-01, 6.0910e-0
1, 6.0910e-01, -1.3889e-01, 3.1831e-02}, |
| 581 {-0.0029937, 0.0345613, -0.1350474, 0.8904793, 0.27144
79, -0.0710304, 0.0135403}, |
| 582 {0.0135403, -0.0710304, 0.2714479, 0.8904793, -0.13504
74, 0.0345613, -0.0029937}}; |
| 583 #else |
| 584 const float shift_filt[3][7] = {{-0.011915f, 0.046995f, -0.152373f, 0.614108f, 0
.614108f, -0.152373f, 0.046995f}, |
| 585 {-0.0324855f, 0.0859768f, -0.2042986f, 0.9640297f, 0.2
086420f, -0.0302054f, -0.0063646f}, |
| 586 {-0.0063646f, -0.0302054f, 0.2086420f, 0.9640297f, -0.
2042986f, 0.0859768f, -0.0324855f}}; |
| 587 #endif |
| 588 #endif |
| 589 |
| 590 int interp_pitch( |
| 591 spx_word16_t *exc, /*decoded excitation*/ |
| 592 spx_word16_t *interp, /*decoded excitation*/ |
| 593 int pitch, /*pitch period*/ |
| 594 int len |
| 595 ) |
| 596 { |
| 597 int i,j,k; |
| 598 spx_word32_t corr[4][7]; |
| 599 spx_word32_t maxcorr; |
| 600 int maxi, maxj; |
| 601 for (i=0;i<7;i++) |
| 602 { |
| 603 corr[0][i] = inner_prod(exc, exc-pitch-3+i, len); |
| 604 } |
| 605 for (i=0;i<3;i++) |
| 606 { |
| 607 for (j=0;j<7;j++) |
| 608 { |
| 609 int i1, i2; |
| 610 spx_word32_t tmp=0; |
| 611 i1 = 3-j; |
| 612 if (i1<0) |
| 613 i1 = 0; |
| 614 i2 = 10-j; |
| 615 if (i2>7) |
| 616 i2 = 7; |
| 617 for (k=i1;k<i2;k++) |
| 618 tmp += MULT16_32_Q15(shift_filt[i][k],corr[0][j+k-3]); |
| 619 corr[i+1][j] = tmp; |
| 620 } |
| 621 } |
| 622 maxi=maxj=0; |
| 623 maxcorr = corr[0][0]; |
| 624 for (i=0;i<4;i++) |
| 625 { |
| 626 for (j=0;j<7;j++) |
| 627 { |
| 628 if (corr[i][j] > maxcorr) |
| 629 { |
| 630 maxcorr = corr[i][j]; |
| 631 maxi=i; |
| 632 maxj=j; |
| 633 } |
| 634 } |
| 635 } |
| 636 for (i=0;i<len;i++) |
| 637 { |
| 638 spx_word32_t tmp = 0; |
| 639 if (maxi>0) |
| 640 { |
| 641 for (k=0;k<7;k++) |
| 642 { |
| 643 tmp += MULT16_16(exc[i-(pitch-maxj+3)+k-3],shift_filt[maxi-1][k]); |
| 644 } |
| 645 } else { |
| 646 tmp = SHL32(exc[i-(pitch-maxj+3)],15); |
| 647 } |
| 648 interp[i] = PSHR32(tmp,15); |
| 649 } |
| 650 return pitch-maxj+3; |
| 651 } |
| 652 |
| 653 void multicomb( |
| 654 spx_word16_t *exc, /*decoded excitation*/ |
| 655 spx_word16_t *new_exc, /*enhanced excitation*/ |
| 656 spx_coef_t *ak, /*LPC filter coefs*/ |
| 657 int p, /*LPC order*/ |
| 658 int nsf, /*sub-frame size*/ |
| 659 int pitch, /*pitch period*/ |
| 660 int max_pitch, |
| 661 spx_word16_t comb_gain, /*gain of comb filter*/ |
| 662 char *stack |
| 663 ) |
| 664 { |
| 665 int i; |
| 666 VARDECL(spx_word16_t *iexc); |
| 667 spx_word16_t old_ener, new_ener; |
| 668 int corr_pitch; |
| 669 |
| 670 spx_word16_t iexc0_mag, iexc1_mag, exc_mag; |
| 671 spx_word32_t corr0, corr1; |
| 672 spx_word16_t gain0, gain1; |
| 673 spx_word16_t pgain1, pgain2; |
| 674 spx_word16_t c1, c2; |
| 675 spx_word16_t g1, g2; |
| 676 spx_word16_t ngain; |
| 677 spx_word16_t gg1, gg2; |
| 678 #ifdef FIXED_POINT |
| 679 int scaledown=0; |
| 680 #endif |
| 681 #if 0 /* Set to 1 to enable full pitch search */ |
| 682 int nol_pitch[6]; |
| 683 spx_word16_t nol_pitch_coef[6]; |
| 684 spx_word16_t ol_pitch_coef; |
| 685 open_loop_nbest_pitch(exc, 20, 120, nsf, |
| 686 nol_pitch, nol_pitch_coef, 6, stack); |
| 687 corr_pitch=nol_pitch[0]; |
| 688 ol_pitch_coef = nol_pitch_coef[0]; |
| 689 /*Try to remove pitch multiples*/ |
| 690 for (i=1;i<6;i++) |
| 691 { |
| 692 #ifdef FIXED_POINT |
| 693 if ((nol_pitch_coef[i]>MULT16_16_Q15(nol_pitch_coef[0],19661)) && |
| 694 #else |
| 695 if ((nol_pitch_coef[i]>.6*nol_pitch_coef[0]) && |
| 696 #endif |
| 697 (ABS(2*nol_pitch[i]-corr_pitch)<=2 || ABS(3*nol_pitch[i]-corr_pitch)<=3
|| |
| 698 ABS(4*nol_pitch[i]-corr_pitch)<=4 || ABS(5*nol_pitch[i]-corr_pitch)<=5)
) |
| 699 { |
| 700 corr_pitch = nol_pitch[i]; |
| 701 } |
| 702 } |
| 703 #else |
| 704 corr_pitch = pitch; |
| 705 #endif |
| 706 |
| 707 ALLOC(iexc, 2*nsf, spx_word16_t); |
| 708 |
| 709 interp_pitch(exc, iexc, corr_pitch, 80); |
| 710 if (corr_pitch>max_pitch) |
| 711 interp_pitch(exc, iexc+nsf, 2*corr_pitch, 80); |
| 712 else |
| 713 interp_pitch(exc, iexc+nsf, -corr_pitch, 80); |
| 714 |
| 715 #ifdef FIXED_POINT |
| 716 for (i=0;i<nsf;i++) |
| 717 { |
| 718 if (ABS16(exc[i])>16383) |
| 719 { |
| 720 scaledown = 1; |
| 721 break; |
| 722 } |
| 723 } |
| 724 if (scaledown) |
| 725 { |
| 726 for (i=0;i<nsf;i++) |
| 727 exc[i] = SHR16(exc[i],1); |
| 728 for (i=0;i<2*nsf;i++) |
| 729 iexc[i] = SHR16(iexc[i],1); |
| 730 } |
| 731 #endif |
| 732 /*interp_pitch(exc, iexc+2*nsf, 2*corr_pitch, 80);*/ |
| 733 |
| 734 /*printf ("%d %d %f\n", pitch, corr_pitch, max_corr*ener_1);*/ |
| 735 iexc0_mag = spx_sqrt(1000+inner_prod(iexc,iexc,nsf)); |
| 736 iexc1_mag = spx_sqrt(1000+inner_prod(iexc+nsf,iexc+nsf,nsf)); |
| 737 exc_mag = spx_sqrt(1+inner_prod(exc,exc,nsf)); |
| 738 corr0 = inner_prod(iexc,exc,nsf); |
| 739 if (corr0<0) |
| 740 corr0=0; |
| 741 corr1 = inner_prod(iexc+nsf,exc,nsf); |
| 742 if (corr1<0) |
| 743 corr1=0; |
| 744 #ifdef FIXED_POINT |
| 745 /* Doesn't cost much to limit the ratio and it makes the rest easier */ |
| 746 if (SHL32(EXTEND32(iexc0_mag),6) < EXTEND32(exc_mag)) |
| 747 iexc0_mag = ADD16(1,PSHR16(exc_mag,6)); |
| 748 if (SHL32(EXTEND32(iexc1_mag),6) < EXTEND32(exc_mag)) |
| 749 iexc1_mag = ADD16(1,PSHR16(exc_mag,6)); |
| 750 #endif |
| 751 if (corr0 > MULT16_16(iexc0_mag,exc_mag)) |
| 752 pgain1 = QCONST16(1., 14); |
| 753 else |
| 754 pgain1 = PDIV32_16(SHL32(PDIV32(corr0, exc_mag),14),iexc0_mag); |
| 755 if (corr1 > MULT16_16(iexc1_mag,exc_mag)) |
| 756 pgain2 = QCONST16(1., 14); |
| 757 else |
| 758 pgain2 = PDIV32_16(SHL32(PDIV32(corr1, exc_mag),14),iexc1_mag); |
| 759 gg1 = PDIV32_16(SHL32(EXTEND32(exc_mag),8), iexc0_mag); |
| 760 gg2 = PDIV32_16(SHL32(EXTEND32(exc_mag),8), iexc1_mag); |
| 761 if (comb_gain>0) |
| 762 { |
| 763 #ifdef FIXED_POINT |
| 764 c1 = (MULT16_16_Q15(QCONST16(.4,15),comb_gain)+QCONST16(.07,15)); |
| 765 c2 = QCONST16(.5,15)+MULT16_16_Q14(QCONST16(1.72,14),(c1-QCONST16(.07,15))
); |
| 766 #else |
| 767 c1 = .4*comb_gain+.07; |
| 768 c2 = .5+1.72*(c1-.07); |
| 769 #endif |
| 770 } else |
| 771 { |
| 772 c1=c2=0; |
| 773 } |
| 774 #ifdef FIXED_POINT |
| 775 g1 = 32767 - MULT16_16_Q13(MULT16_16_Q15(c2, pgain1),pgain1); |
| 776 g2 = 32767 - MULT16_16_Q13(MULT16_16_Q15(c2, pgain2),pgain2); |
| 777 #else |
| 778 g1 = 1-c2*pgain1*pgain1; |
| 779 g2 = 1-c2*pgain2*pgain2; |
| 780 #endif |
| 781 if (g1<c1) |
| 782 g1 = c1; |
| 783 if (g2<c1) |
| 784 g2 = c1; |
| 785 g1 = (spx_word16_t)PDIV32_16(SHL32(EXTEND32(c1),14),(spx_word16_t)g1); |
| 786 g2 = (spx_word16_t)PDIV32_16(SHL32(EXTEND32(c1),14),(spx_word16_t)g2); |
| 787 if (corr_pitch>max_pitch) |
| 788 { |
| 789 gain0 = MULT16_16_Q15(QCONST16(.7,15),MULT16_16_Q14(g1,gg1)); |
| 790 gain1 = MULT16_16_Q15(QCONST16(.3,15),MULT16_16_Q14(g2,gg2)); |
| 791 } else { |
| 792 gain0 = MULT16_16_Q15(QCONST16(.6,15),MULT16_16_Q14(g1,gg1)); |
| 793 gain1 = MULT16_16_Q15(QCONST16(.6,15),MULT16_16_Q14(g2,gg2)); |
| 794 } |
| 795 for (i=0;i<nsf;i++) |
| 796 new_exc[i] = ADD16(exc[i], EXTRACT16(PSHR32(ADD32(MULT16_16(gain0,iexc[i])
, MULT16_16(gain1,iexc[i+nsf])),8))); |
| 797 /* FIXME: compute_rms16 is currently not quite accurate enough (but close) */ |
| 798 new_ener = compute_rms16(new_exc, nsf); |
| 799 old_ener = compute_rms16(exc, nsf); |
| 800 |
| 801 if (old_ener < 1) |
| 802 old_ener = 1; |
| 803 if (new_ener < 1) |
| 804 new_ener = 1; |
| 805 if (old_ener > new_ener) |
| 806 old_ener = new_ener; |
| 807 ngain = PDIV32_16(SHL32(EXTEND32(old_ener),14),new_ener); |
| 808 |
| 809 for (i=0;i<nsf;i++) |
| 810 new_exc[i] = MULT16_16_Q14(ngain, new_exc[i]); |
| 811 #ifdef FIXED_POINT |
| 812 if (scaledown) |
| 813 { |
| 814 for (i=0;i<nsf;i++) |
| 815 exc[i] = SHL16(exc[i],1); |
| 816 for (i=0;i<nsf;i++) |
| 817 new_exc[i] = SHL16(SATURATE16(new_exc[i],16383),1); |
| 818 } |
| 819 #endif |
| 820 } |
| 821 |
OLD | NEW |