286 | | int32 t; |
287 | | int32 *h, *hc, *he, half_width; |
288 | | |
289 | | work_len = src_len + fs->taps; |
290 | | work_buf = (sample*)block_alloc(sizeof(sample)*work_len); |
291 | | |
292 | | assert(fs->hold_bytes == fs->taps * sizeof (sample)); |
293 | | |
| 289 | int32 tmp, si_start, si_end, si, hold_bytes; |
| 290 | int32 *h, hi_start, hi_end, hi; |
| 291 | |
| 292 | hold_bytes = fs->taps / fs->scale * sizeof(sample); |
| 293 | work_len = src_len + hold_bytes / sizeof(sample); |
| 294 | work_buf = (sample*)block_alloc(sizeof(sample)*work_len); |
| 295 | |
299 | | memcpy(fs->hold_buf, src + src_len - fs->hold_bytes / sizeof(sample), fs->hold_bytes); |
300 | | |
301 | | d = dst; |
302 | | de = dst + dst_len; |
303 | | ss = work_buf; |
304 | | se = work_buf + work_len; |
305 | | he = fs->filter + fs->taps; |
306 | | h = fs->filter; |
307 | | half_width = fs->taps / 2; |
308 | | |
309 | | while (d < de) { |
310 | | hc = h; |
311 | | t = 0; |
312 | | sc = ss; |
313 | | /* Cast otherwise bounds checker complains about comparison */ |
314 | | /* when hc > he. */ |
315 | | while ((u_int32)hc < (u_int32)he) { |
316 | | t += (*hc) * (*sc); |
317 | | hc += fs->scale; |
318 | | sc++; |
319 | | } |
320 | | |
321 | | t = t >> SINC_ROLL; |
322 | | if (t > 32767) { |
323 | | *d = 32767; |
324 | | } else if (t < -32768) { |
325 | | *d = -32768; |
326 | | } else { |
327 | | *d = (sample)t; |
328 | | } |
329 | | |
330 | | d++; |
331 | | ss++; |
332 | | |
333 | | h = fs->filter + fs->scale - 1; |
334 | | while (h != fs->filter) { |
335 | | hc = h; |
336 | | t = 0; |
337 | | sc = ss; |
338 | | while ((u_int32)hc < (u_int32)he) { |
339 | | t += (*hc) * (*sc); |
340 | | hc += fs->scale; |
341 | | sc++; |
342 | | } |
343 | | t = t >> SINC_ROLL; |
344 | | if (t > 32767) { |
345 | | *d = 32767; |
346 | | } else if (t < -32768) { |
347 | | *d = -32768; |
348 | | } else { |
349 | | *d = (sample)t; |
350 | | } |
351 | | d++; |
352 | | h--; |
353 | | } |
354 | | } |
355 | | assert(d == de); |
356 | | /* assert(sc == se); */ |
| 303 | memcpy(fs->hold_buf, |
| 304 | src + src_len - hold_bytes / sizeof(sample), |
| 305 | hold_bytes); |
| 306 | |
| 307 | h = fs->filter; |
| 308 | hi_end = fs->taps; |
| 309 | |
| 310 | si_start = 0; |
| 311 | si_end = work_len - (fs->taps / fs->scale); |
| 312 | out = dst; |
| 313 | |
| 314 | switch (fs->scale) { |
| 315 | case 6: |
| 316 | while (si_start < si_end) { |
| 317 | si = si_start; |
| 318 | tmp = 0; |
| 319 | hi = 5; |
| 320 | while (hi < hi_end) { |
| 321 | tmp += work_buf[si] * h[hi]; |
| 322 | hi += fs->scale; |
| 323 | si += 1; |
| 324 | } |
| 325 | tmp /= SINC_SCALE; |
| 326 | if (tmp > 32767) { |
| 327 | cs_err("clipping\n"); |
| 328 | tmp = 32767; |
| 329 | } else if (tmp < -32767) { |
| 330 | cs_err("clipping\n"); |
| 331 | tmp = -32767; |
| 332 | } |
| 333 | *out++ = (short)tmp; |
| 334 | |
| 335 | si = si_start; |
| 336 | tmp = 0; |
| 337 | hi = 4; |
| 338 | while (hi < hi_end) { |
| 339 | tmp += work_buf[si] * h[hi]; |
| 340 | hi += fs->scale; |
| 341 | si += 1; |
| 342 | } |
| 343 | tmp /= SINC_SCALE; |
| 344 | if (tmp > 32767) { |
| 345 | cs_err("clipping\n"); |
| 346 | tmp = 32767; |
| 347 | } else if (tmp < -32767) { |
| 348 | cs_err("clipping\n"); |
| 349 | tmp = -32767; |
| 350 | } |
| 351 | *out++ = (short)tmp; |
| 352 | |
| 353 | si = si_start; |
| 354 | tmp = 0; |
| 355 | hi = 3; |
| 356 | while (hi < hi_end) { |
| 357 | tmp += work_buf[si] * h[hi]; |
| 358 | hi += fs->scale; |
| 359 | si += 1; |
| 360 | } |
| 361 | tmp /= SINC_SCALE; |
| 362 | if (tmp > 32767) { |
| 363 | cs_err("clipping\n"); |
| 364 | tmp = 32767; |
| 365 | } else if (tmp < -32767) { |
| 366 | cs_err("clipping\n"); |
| 367 | tmp = -32767; |
| 368 | } |
| 369 | *out++ = (short)tmp; |
| 370 | |
| 371 | si = si_start; |
| 372 | tmp = 0; |
| 373 | hi = 2; |
| 374 | while (hi < hi_end) { |
| 375 | tmp += work_buf[si] * h[hi]; |
| 376 | hi += fs->scale; |
| 377 | si += 1; |
| 378 | } |
| 379 | tmp /= SINC_SCALE; |
| 380 | if (tmp > 32767) { |
| 381 | cs_err("clipping\n"); |
| 382 | tmp = 32767; |
| 383 | } else if (tmp < -32767) { |
| 384 | cs_err("clipping\n"); |
| 385 | tmp = -32767; |
| 386 | } |
| 387 | *out++ = (short)tmp; |
| 388 | |
| 389 | si = si_start; |
| 390 | tmp = 0; |
| 391 | hi = 1; |
| 392 | while (hi < hi_end) { |
| 393 | tmp += work_buf[si] * h[hi]; |
| 394 | hi += fs->scale; |
| 395 | si += 1; |
| 396 | } |
| 397 | tmp /= SINC_SCALE; |
| 398 | if (tmp > 32767) { |
| 399 | cs_err("clipping\n"); |
| 400 | tmp = 32767; |
| 401 | } else if (tmp < -32767) { |
| 402 | cs_err("clipping\n"); |
| 403 | tmp = -32767; |
| 404 | } |
| 405 | *out++ = (short)tmp; |
| 406 | |
| 407 | si = si_start; |
| 408 | tmp = 0; |
| 409 | hi = 0; |
| 410 | while (hi < hi_end) { |
| 411 | tmp += work_buf[si] * h[hi]; |
| 412 | hi += fs->scale; |
| 413 | si += 1; |
| 414 | } |
| 415 | tmp /= SINC_SCALE; |
| 416 | if (tmp > 32767) { |
| 417 | cs_err("clipping\n"); |
| 418 | tmp = 32767; |
| 419 | } else if (tmp < -32767) { |
| 420 | cs_err("clipping\n"); |
| 421 | tmp = -32767; |
| 422 | } |
| 423 | *out++ = (short)tmp; |
| 424 | |
| 425 | si_start++; |
| 426 | } |
| 427 | break; |
| 428 | case 5: |
| 429 | while (si_start < si_end) { |
| 430 | si = si_start; |
| 431 | tmp = 0; |
| 432 | hi = 4; |
| 433 | while (hi < hi_end) { |
| 434 | tmp += work_buf[si] * h[hi]; |
| 435 | hi += fs->scale; |
| 436 | si += 1; |
| 437 | } |
| 438 | tmp /= SINC_SCALE; |
| 439 | if (tmp > 32767) { |
| 440 | tmp = 32767; |
| 441 | } else if (tmp < -32767) { |
| 442 | tmp = -32767; |
| 443 | } |
| 444 | *out++ = (short)tmp; |
| 445 | |
| 446 | si = si_start; |
| 447 | tmp = 0; |
| 448 | hi = 3; |
| 449 | while (hi < hi_end) { |
| 450 | tmp += work_buf[si] * h[hi]; |
| 451 | hi += fs->scale; |
| 452 | si += 1; |
| 453 | } |
| 454 | tmp /= SINC_SCALE; |
| 455 | if (tmp > 32767) { |
| 456 | tmp = 32767; |
| 457 | } else if (tmp < -32767) { |
| 458 | tmp = -32767; |
| 459 | } |
| 460 | *out++ = (short)tmp; |
| 461 | |
| 462 | si = si_start; |
| 463 | tmp = 0; |
| 464 | hi = 2; |
| 465 | while (hi < hi_end) { |
| 466 | tmp += work_buf[si] * h[hi]; |
| 467 | hi += fs->scale; |
| 468 | si += 1; |
| 469 | } |
| 470 | tmp /= SINC_SCALE; |
| 471 | if (tmp > 32767) { |
| 472 | tmp = 32767; |
| 473 | } else if (tmp < -32767) { |
| 474 | tmp = -32767; |
| 475 | } |
| 476 | *out++ = (short)tmp; |
| 477 | |
| 478 | si = si_start; |
| 479 | tmp = 0; |
| 480 | hi = 1; |
| 481 | while (hi < hi_end) { |
| 482 | tmp += work_buf[si] * h[hi]; |
| 483 | hi += fs->scale; |
| 484 | si += 1; |
| 485 | } |
| 486 | tmp /= SINC_SCALE; |
| 487 | if (tmp > 32767) { |
| 488 | tmp = 32767; |
| 489 | } else if (tmp < -32767) { |
| 490 | tmp = -32767; |
| 491 | } |
| 492 | *out++ = (short)tmp; |
| 493 | |
| 494 | si = si_start; |
| 495 | tmp = 0; |
| 496 | hi = 0; |
| 497 | while (hi < hi_end) { |
| 498 | tmp += work_buf[si] * h[hi]; |
| 499 | hi += fs->scale; |
| 500 | si += 1; |
| 501 | } |
| 502 | tmp /= SINC_SCALE; |
| 503 | if (tmp > 32767) { |
| 504 | tmp = 32767; |
| 505 | } else if (tmp < -32767) { |
| 506 | tmp = -32767; |
| 507 | } |
| 508 | *out++ = (short)tmp; |
| 509 | |
| 510 | si_start++; |
| 511 | } |
| 512 | break; |
| 513 | case 4: |
| 514 | while (si_start < si_end) { |
| 515 | si = si_start; |
| 516 | tmp = 0; |
| 517 | hi = 3; |
| 518 | while (hi < hi_end) { |
| 519 | tmp += work_buf[si] * h[hi]; |
| 520 | hi += fs->scale; |
| 521 | si += 1; |
| 522 | } |
| 523 | tmp /= SINC_SCALE; |
| 524 | if (tmp > 32767) { |
| 525 | tmp = 32767; |
| 526 | } else if (tmp < -32767) { |
| 527 | tmp = -32767; |
| 528 | } |
| 529 | *out++ = (short)tmp; |
| 530 | |
| 531 | si = si_start; |
| 532 | tmp = 0; |
| 533 | hi = 2; |
| 534 | while (hi < hi_end) { |
| 535 | tmp += work_buf[si] * h[hi]; |
| 536 | hi += fs->scale; |
| 537 | si += 1; |
| 538 | } |
| 539 | tmp /= SINC_SCALE; |
| 540 | if (tmp > 32767) { |
| 541 | tmp = 32767; |
| 542 | } else if (tmp < -32767) { |
| 543 | tmp = -32767; |
| 544 | } |
| 545 | *out++ = (short)tmp; |
| 546 | |
| 547 | si = si_start; |
| 548 | tmp = 0; |
| 549 | hi = 1; |
| 550 | while (hi < hi_end) { |
| 551 | tmp += work_buf[si] * h[hi]; |
| 552 | hi += fs->scale; |
| 553 | si += 1; |
| 554 | } |
| 555 | tmp /= SINC_SCALE; |
| 556 | if (tmp > 32767) { |
| 557 | tmp = 32767; |
| 558 | } else if (tmp < -32767) { |
| 559 | tmp = -32767; |
| 560 | } |
| 561 | *out++ = (short)tmp; |
| 562 | |
| 563 | si = si_start; |
| 564 | tmp = 0; |
| 565 | hi = 0; |
| 566 | while (hi < hi_end) { |
| 567 | tmp += work_buf[si] * h[hi]; |
| 568 | hi += fs->scale; |
| 569 | si += 1; |
| 570 | } |
| 571 | tmp /= SINC_SCALE; |
| 572 | if (tmp > 32767) { |
| 573 | tmp = 32767; |
| 574 | } else if (tmp < -32767) { |
| 575 | tmp = -32767; |
| 576 | } |
| 577 | *out++ = (short)tmp; |
| 578 | |
| 579 | si_start++; |
| 580 | } |
| 581 | break; |
| 582 | case 3: |
| 583 | while (si_start < si_end) { |
| 584 | si = si_start; |
| 585 | tmp = 0; |
| 586 | hi = 2; |
| 587 | while (hi < hi_end) { |
| 588 | tmp += work_buf[si] * h[hi]; |
| 589 | hi += fs->scale; |
| 590 | si += 1; |
| 591 | } |
| 592 | tmp /= SINC_SCALE; |
| 593 | if (tmp > 32767) { |
| 594 | tmp = 32767; |
| 595 | } else if (tmp < -32767) { |
| 596 | tmp = -32767; |
| 597 | } |
| 598 | *out++ = (short)tmp; |
| 599 | |
| 600 | si = si_start; |
| 601 | tmp = 0; |
| 602 | hi = 1; |
| 603 | while (hi < hi_end) { |
| 604 | tmp += work_buf[si] * h[hi]; |
| 605 | hi += fs->scale; |
| 606 | si += 1; |
| 607 | } |
| 608 | tmp /= SINC_SCALE; |
| 609 | if (tmp > 32767) { |
| 610 | tmp = 32767; |
| 611 | } else if (tmp < -32767) { |
| 612 | tmp = -32767; |
| 613 | } |
| 614 | *out++ = (short)tmp; |
| 615 | |
| 616 | si = si_start; |
| 617 | tmp = 0; |
| 618 | hi = 0; |
| 619 | while (hi < hi_end) { |
| 620 | tmp += work_buf[si] * h[hi]; |
| 621 | hi += fs->scale; |
| 622 | si += 1; |
| 623 | } |
| 624 | tmp /= SINC_SCALE; |
| 625 | if (tmp > 32767) { |
| 626 | tmp = 32767; |
| 627 | } else if (tmp < -32767) { |
| 628 | tmp = -32767; |
| 629 | } |
| 630 | *out++ = (short)tmp; |
| 631 | |
| 632 | si_start++; |
| 633 | } |
| 634 | break; |
| 635 | case 2: |
| 636 | while (si_start < si_end) { |
| 637 | si = si_start; |
| 638 | tmp = 0; |
| 639 | hi = 1; |
| 640 | while (hi < hi_end) { |
| 641 | tmp += work_buf[si] * h[hi]; |
| 642 | hi += fs->scale; |
| 643 | si += 1; |
| 644 | } |
| 645 | tmp /= SINC_SCALE; |
| 646 | if (tmp > 32767) { |
| 647 | tmp = 32767; |
| 648 | } else if (tmp < -32767) { |
| 649 | tmp = -32767; |
| 650 | } |
| 651 | *out++ = (short)tmp; |
| 652 | |
| 653 | si = si_start; |
| 654 | tmp = 0; |
| 655 | hi = 0; |
| 656 | while (hi < hi_end) { |
| 657 | tmp += work_buf[si] * h[hi]; |
| 658 | hi += fs->scale; |
| 659 | si += 1; |
| 660 | } |
| 661 | tmp /= SINC_SCALE; |
| 662 | if (tmp > 32767) { |
| 663 | tmp = 32767; |
| 664 | } else if (tmp < -32767) { |
| 665 | tmp = -32767; |
| 666 | } |
| 667 | *out++ = (short)tmp; |
| 668 | |
| 669 | si_start++; |
| 670 | } |
| 671 | break; |
| 672 | default: |
| 673 | while (si_start < si_end) { |
| 674 | hi_start = fs->scale - 1; |
| 675 | while (hi_start >= 0) { |
| 676 | tmp = 0; |
| 677 | si = si_start; |
| 678 | hi = hi_start; |
| 679 | while (hi < hi_end) { |
| 680 | tmp += work_buf[si] * h[hi]; |
| 681 | hi += fs->scale; |
| 682 | si += 1; |
| 683 | } |
| 684 | tmp /= SINC_SCALE; |
| 685 | if (tmp > 32767) { |
| 686 | tmp = 32767; |
| 687 | } else if (tmp < -32767) { |
| 688 | tmp = -32767; |
| 689 | } |
| 690 | *out++ = (short)tmp; |
| 691 | hi_start--; |
| 692 | } |
| 693 | si_start++; |
| 694 | } |
| 695 | } |
| 696 | assert(si_start == si_end); |
| 697 | assert(out == dst + dst_len); |
| 698 | |
361 | | #ifdef BRUTE_FORCE |
362 | | static void |
363 | | sinc_upsample_mono(struct s_filter_state *fs, |
364 | | sample *src, int src_len, |
365 | | sample *dst, int dst_len) |
366 | | { |
367 | | sample *work_buf; |
368 | | int32 i,j, t, work_len; |
369 | | |
370 | | work_len = (src_len + fs->taps - 1) * fs->scale; |
371 | | work_buf = (sample*)block_alloc(work_len * sizeof(sample)); |
372 | | memset(work_buf, 0, sizeof(sample) * work_len); |
373 | | |
374 | | /* Transfer samples into workspace */ |
375 | | for (i = 0; i < (int32)(fs->hold_bytes/sizeof(sample)); i++) { |
376 | | work_buf[i * fs->scale] = fs->hold_buf[i]; |
377 | | } |
378 | | j = i; |
379 | | for (i = 0; i < src_len; i++) { |
380 | | work_buf[(i+j) * fs->scale] = src[i]; |
381 | | } |
382 | | |
383 | | /* Copy hold over */ |
384 | | memcpy(fs->hold_buf, src + src_len - fs->hold_bytes/sizeof(sample), fs->hold_bytes); |
385 | | |
386 | | for (i = 0; i < dst_len; i++) { |
387 | | t = 0; |
388 | | for (j = 0; j < fs->taps; j++) { |
389 | | t += work_buf[i + j] * fs->filter[j]; |
390 | | } |
391 | | t >>= SINC_ROLL; |
392 | | dst[i] = t; |
393 | | } |
394 | | } |
395 | | #endif /* BRUTE_FORCE */ |
| 703 | #endif/* LIGHT */ |
| 704 | #ifdef HEAVY |
| 705 | |
| 706 | /* HEAVY and LIGHT should produce same result, at time of writing they do! */ |
| 707 | /* HEAVY is clumsy expand buffer method, LIGHT uses 1 less buffer and copies */ |
| 708 | |
| 709 | static void |
| 710 | sinc_upsample_mono (struct s_filter_state *fs, |
| 711 | sample *src, int src_len, |
| 712 | sample *dst, int dst_len) |
| 713 | { |
| 714 | sample *work_buf, *out; |
| 715 | sample *large_buf; |
| 716 | |
| 717 | int work_len, i, large_buf_len; |
| 718 | int32 tmp, si_start, si_end, hold_bytes; |
| 719 | int32 *h; |
| 720 | |
| 721 | hold_bytes = fs->taps / fs->scale * sizeof(sample); |
| 722 | work_len = src_len + hold_bytes / sizeof(sample); |
| 723 | work_buf = (sample*)block_alloc(sizeof(sample)*work_len); |
| 724 | |
| 725 | /* Get samples into work_buf */ |
| 726 | memcpy(work_buf, fs->hold_buf, hold_bytes); |
| 727 | memcpy(work_buf + hold_bytes / sizeof(sample), |
| 728 | src, |
| 729 | src_len * sizeof(sample)); |
| 730 | |
| 731 | /* Save last samples in src into hold_buf for next time */ |
| 732 | memcpy(fs->hold_buf, |
| 733 | src + src_len - hold_bytes / sizeof(sample), |
| 734 | hold_bytes); |
| 735 | |
| 736 | h = fs->filter; |
| 737 | |
| 738 | large_buf_len = fs->scale * src_len + fs->taps; |
| 739 | large_buf = (sample*)xmalloc(large_buf_len * sizeof(sample)); |
| 740 | memset(large_buf, 0, sizeof(sample) * large_buf_len); |
| 741 | |
| 742 | for (i = 0; i < work_len; i++) { |
| 743 | large_buf[fs->scale * i + fs->scale - 1] = work_buf[i]; |
| 744 | } |
| 745 | |
| 746 | out = dst; |
| 747 | si_start = 0; |
| 748 | si_end = large_buf_len - fs->taps; |
| 749 | |
| 750 | while (si_start < si_end) { |
| 751 | tmp = 0; |
| 752 | for(i = 0; i < fs->taps; i++) { |
| 753 | tmp += h[i] * large_buf[si_start + i]; |
| 754 | } |
| 755 | tmp /= SINC_SCALE; |
| 756 | if (tmp > 32767) { |
| 757 | tmp = 32767; |
| 758 | } else if (tmp < -32767) { |
| 759 | tmp = -32767; |
| 760 | } |
| 761 | *out++ = (short)tmp; |
| 762 | si_start++; |
| 763 | } |
| 764 | |
| 765 | assert(out == dst + dst_len); |
| 766 | |
| 767 | xfree(large_buf); |
| 768 | } |
| 769 | |
| 770 | #endif /* HEAVY */ |