ref: 39fa45a7edd0a05898bec9ac9eae80e5e18013d7
dir: /src/polyphas.c/
/*
* July 14, 1998
* Copyright 1998 K. Bradley, Carnegie Mellon University
*
* This source code is freely redistributable and may be used for
* any purpose. This copyright notice must be maintained.
* Lance Norskog And Sundry Contributors are not responsible for
* the consequences of using this software.
*/
/*
* Sound Tools rate change effect file.
*/
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#ifdef HAVE_MALLOC_H
#include <malloc.h>
#endif
#include "st.h"
typedef struct _list {
int number;
float *data_buffer;
struct _list *next;
} List;
typedef struct polyphase {
unsigned long lcmrate; /* least common multiple of rates */
unsigned long inskip, outskip; /* LCM increments for I & O rates */
unsigned long total;
unsigned long intot, outtot; /* total samples in terms of LCM rate */
long lastsamp;
float **filt_array;
float **past_hist;
float *input_buffer;
int *filt_len;
List *l1, *l2;
} *poly_t;
/*
* Process options
*/
/* Options:
-w <nut / ham> : window type
-width <short / long> : window width
short = 128 samples
long = 1024 samples
<num> num: explicit number
-cutoff <float> : frequency cutoff for base bandwidth.
Default = 0.95 = 95%
*/
static int win_type = 0;
static int win_width = 1024;
static float cutoff = 0.95;
void poly_getopts(effp, n, argv)
eff_t effp;
int n;
char **argv;
{
/* 0: nuttall
1: hamming */
win_type = 0;
/* width: short = 128
long = 1024 (default) */
win_width = 1024;
/* cutoff: frequency cutoff of base bandwidth in percentage. */
cutoff = 0.95;
while(n >= 2) {
/* Window type check */
if(!strcmp(argv[0], "-w")) {
if(!strcmp(argv[1], "ham"))
win_type = 1;
if(!strcmp(argv[1], "nut"))
win_type = 0;
argv += 2;
n -= 2;
continue;
}
/* Window width check */
if(!strcmp(argv[0], "-width")) {
if(!strcmp(argv[1], "short"))
win_width = 128;
else if(!strcmp(argv[1], "long"))
win_width = 1024;
else
win_width = atoi(argv[1]);
argv += 2;
n -= 2;
continue;
}
/* Cutoff frequency check */
if(!strcmp(argv[0], "-cutoff")) {
cutoff = atof(argv[1]);
argv += 2;
n -= 2;
continue;
}
fail("Polyphase: unknown argument (%s %s)!", argv[0], argv[1]);
}
}
/*
* Prepare processing.
*/
static int primes[] = {
2, 3, 5, 7, 11, 13, 17, 19, 23, 29, 31, 37,
41, 43, 47, 53, 59, 61, 67, 71, 73, 79, 83, 89,
97, 101, 103, 107, 109, 113, 127, 131, 137, 139, 149, 151,
157, 163, 167, 173, 179, 181, 191, 193, 197, 199, 211, 223,
227, 229, 233, 239, 241, 251, 257, 263, 269, 271, 277, 281,
283, 293, 307, 311, 313, 317, 331, 337, 347, 349, 353, 359,
367, 373, 379, 383, 389, 397, 401, 409, 419, 421, 431, 433,
439, 443, 449, 457, 461, 463, 467, 479, 487, 491, 499, 503,
509, 521, 523, 541, 547, 557, 563, 569, 571, 577, 587, 593,
599, 601, 607, 613, 617, 619, 631, 641, 643, 647, 653, 659,
661, 673, 677, 683, 691, 701, 709, 719, 727, 733, 739, 743,
751, 757, 761, 769, 773, 787, 797, 809, 811, 821, 823, 827,
829, 839, 853, 857, 859, 863, 877, 881, 883, 887, 907, 911,
919, 929, 937, 941, 947, 953, 967, 971, 977, 983, 991, 997
};
#ifndef max
#define max(x,y) ((x > y) ? x : y)
#endif
List *prime(number)
int number;
{
int j;
List *element = NULL;
if(number == 1)
return NULL;
for(j=167;j>= 0;j--) {
if(number % primes[j] == 0) {
element = (List *) malloc(sizeof(List));
element->number = primes[j];
element->data_buffer = NULL;
element->next = prime(number / primes[j]);
break;
}
}
if(element == NULL) {
fail("Number %d too large of a prime.\n",number);
}
return element;
}
List *prime_inv(number)
int number;
{
int j;
List *element = NULL;
if(number == 1)
return NULL;
for(j=0;j<168;j++) {
if(number % primes[j] == 0) {
element = (List *) malloc(sizeof(List));
element->number = primes[j];
element->data_buffer = NULL;
element->next = prime_inv(number / primes[j]);
break;
}
}
if(element == NULL) {
fail("Number %d too large of a prime.\n",number);
}
return element;
}
#ifndef PI
#define PI 3.14159265358979
#endif
/* Calculate a Nuttall window of a given length.
Buffer must already be allocated to appropriate size.
*/
void nuttall(buffer, length)
float *buffer;
int length;
{
int j;
double N;
double N1;
if(buffer == NULL || length < 0)
fail("Illegal buffer %p or length %d to nuttall.\n", buffer, length);
/* Initial variable setups. */
N = (double) length - 1.0;
N1 = N / 2.0;
for(j = 0; j < length; j++) {
buffer[j] = 0.36335819 +
0.4891775 * cos(2*PI*1*(j - N1) / N) +
0.1365995 * cos(2*PI*2*(j - N1) / N) +
0.0106411 * cos(2*PI*3*(j - N1) / N);
}
}
/* Calculate a Hamming window of given length.
Buffer must already be allocated to appropriate size.
*/
void hamming(buffer, length)
float *buffer;
int length;
{
int j;
if(buffer == NULL || length < 0)
fail("Illegal buffer %p or length %d to hamming.\n",buffer,length);
for(j=0;j<length;j++)
buffer[j] = 0.5 - 0.46 * cos(2*PI*j/(length-1));
}
/* Calculate the sinc function properly */
float sinc(value)
float value;
{
return(fabs(value) < 1E-50 ? 1.0 : sin(value) / value);
}
/* Design a low-pass FIR filter using window technique.
Length of filter is in length, cutoff frequency in cutoff.
0 < cutoff <= 1.0 (normalized frequency)
buffer must already be allocated.
*/
void fir_design(buffer, length, cutoff)
float *buffer;
int length;
float cutoff;
{
int j;
float sum;
float *ham_win;
if(buffer == NULL || length < 0 || cutoff < 0 || cutoff > PI)
fail("Illegal buffer %p, length %d, or cutoff %f.\n",buffer,length,cutoff);
/* Design Hamming window: 43 dB cutoff */
ham_win = (float *)malloc(sizeof(float) * length);
/* Use the user-option of window type */
if(win_type == 0)
nuttall(ham_win, length);
else
hamming(ham_win,length);
/* Design filter: windowed sinc function */
sum = 0.0;
for(j=0;j<length;j++) {
buffer[j] = sinc(PI*cutoff*(j-length/2)) * ham_win[j] / (2*cutoff);
sum += buffer[j];
}
/* Normalize buffer to have gain of 1.0: prevent roundoff error */
for(j=0;j<length;j++)
buffer[j] /= sum;
free((void *) ham_win);
}
void poly_start(effp)
eff_t effp;
{
poly_t rate = (poly_t) effp->priv;
List *t, *t2;
int num_l1, num_l2;
int j,k;
float f_cutoff;
extern long lcm();
rate->lcmrate = lcm((long)effp->ininfo.rate, (long)effp->outinfo.rate);
/* Cursory check for LCM overflow.
* If both rate are below 65k, there should be no problem.
* 16 bits x 16 bits = 32 bits, which we can handle.
*/
rate->inskip = rate->lcmrate / effp->ininfo.rate;
rate->outskip = rate->lcmrate / effp->outinfo.rate;
/* Find the prime factors of inskip and outskip */
rate->l1 = prime(rate->inskip);
/* If we're going up, order things backwards. */
if(effp->ininfo.rate < effp->outinfo.rate)
rate->l2 = prime_inv(rate->outskip);
else
rate->l2 = prime(rate->outskip);
/* Find how many factors there were */
if(rate->l1 == NULL)
num_l1 = 0;
else
for(num_l1=0, t = rate->l1; t != NULL; num_l1++, t=t->next);
if(rate->l2 == NULL)
num_l2 = 0;
else
for(num_l2=0, t = rate->l2; t != NULL; num_l2++, t=t->next);
k = 0;
t = rate->l1;
/* Compact the lists to be less than 10 */
while(k < num_l1 - 1) {
if(t->number * t->next->number < 10) {
t->number = t->number * t->next->number;
t2 = t->next;
t->next = t->next->next;
t2->next = NULL;
free((void *) t2);
num_l1--;
} else {
k++;
t = t->next;
}
}
k = 0;
t = rate->l2;
while(k < num_l2 - 1) {
if(t->number * t->next->number < 10) {
t->number = t->number * t->next->number;
t2 = t->next;
t->next = t->next->next;
t2->next = NULL;
free((void *) t2);
num_l2--;
} else {
k++;
t = t->next;
}
}
/* l1 and l2 are now lists of the prime factors compacted,
meaning that they're the lists of up/down sampling we need
*/
/* Stretch them to be the same length by padding with 1 (no-op) */
if(num_l1 < num_l2) {
t = rate->l1;
if(t == NULL) {
rate->l1 = (List *)malloc(sizeof(List));
rate->l1->next = NULL;
rate->l1->number = 1;
rate->l1->data_buffer = NULL;
t = rate->l1;
num_l1++;
}
while(t->next != NULL)
t = t->next;
for(k=0;k<num_l2-num_l1;k++) {
t->next = (List *) malloc(sizeof(List));
t->next->number = 1;
t->next->data_buffer = NULL;
t = t->next;
}
t->next = NULL;
num_l1 = num_l2;
} else {
t = rate->l2;
if(t == NULL) {
rate->l2 = (List *)malloc(sizeof(List));
rate->l2->next = NULL;
rate->l2->number = 1;
rate->l2->data_buffer = NULL;
t = rate->l2;
num_l2++;
}
/*
while(t->next != NULL)
t = t->next;
*/
for(k=0;k<num_l1-num_l2;k++) {
t = rate->l2;
rate->l2 = (List *) malloc(sizeof(List));
rate->l2->number = 1;
rate->l2->data_buffer = NULL;
rate->l2->next = t;
}
/* t->next = NULL; */
num_l2 = num_l1;
}
/* l1 and l2 are now the same size. */
rate->total = num_l1;
report("Poly: input rate %d, output rate %d. %d stages.",effp->ininfo.rate, effp->outinfo.rate,num_l1);
report("Poly: window: %s size: %d cutoff: %f.", (win_type == 0) ? ("nut") : ("ham"), win_width, cutoff);
for(k=0, t=rate->l1, t2=rate->l2;k<num_l1;k++,t=t->next,t2=t2->next)
report("Poly: stage %d: Up by %d, down by %d.",k+1,t->number,t2->number);
/* We'll have an array of filters and past history */
rate->filt_array = (float **) malloc(sizeof(float *) * num_l1);
rate->past_hist = (float **) malloc(sizeof(float *) * num_l1);
rate->filt_len = (int *) malloc(sizeof(int) * num_l1);
for(k = 0, t = rate->l1, t2 = rate->l2; k < num_l1; k++) {
rate->filt_len[k] = max(2 * 10 * max(t->number,t2->number), win_width);
rate->filt_array[k] = (float *) malloc(sizeof(float) * rate->filt_len[k]);
rate->past_hist[k] = (float *) malloc(sizeof(float) * rate->filt_len[k]);
t->data_buffer = (float *) malloc(sizeof(float) * 1024 * rate->inskip);
for(j = 0; j < rate->filt_len[k]; j++)
rate->past_hist[k][j] = 0.0;
f_cutoff = (t->number > t2->number) ?
(float) t->number : (float) t2->number;
fir_design(rate->filt_array[k], rate->filt_len[k]-1, cutoff / f_cutoff);
t = t->next;
t2 = t2->next;
}
rate->input_buffer = (float *) malloc(sizeof(float) * 2048);
}
/*
* Processed signed long samples from ibuf to obuf.
* Return number of samples processed.
*/
static float *h;
static int M, L, N;
void polyphase_init(coef, num_coef, up_rate, down_rate)
float *coef;
int num_coef;
int up_rate;
int down_rate;
{
h = coef;
M = down_rate;
L = up_rate;
N = num_coef;
}
void polyphase(input, output, past, num_samples_input)
float *input;
float *output;
float *past;
int num_samples_input;
{
int num_output;
int m,n;
float sum;
float inp;
int base;
int h_base;
num_output = num_samples_input * L / M;
for(m=0;m<num_output;m++) {
sum = 0.0;
base = (int) (m*M/L);
h_base = (m*M) % L;
for(n=0;n<N / L;n++) {
if(base - n < 0)
inp = past[base - n + N];
else
inp = input[base - n];
sum += h[n*L + h_base] * inp;
}
output[m] = sum * L * 0.95;
}
}
void poly_flow(effp, ibuf, obuf, isamp, osamp)
eff_t effp;
long *ibuf, *obuf;
int *isamp, *osamp;
{
poly_t rate = (poly_t) effp->priv;
float *temp_buf, *temp_buf2;
int j,k;
List *t1, *t2;
int in_size, out_size;
/* Sanity check: how much can we tolerate? */
in_size = *isamp;
out_size = in_size * rate->inskip / rate->outskip;
if(out_size > *osamp) {
in_size = *osamp * rate->outskip / rate->inskip;
*isamp = in_size;
}
/* Check to see if we're really draining */
if(ibuf != NULL) {
for(k=0;k<*isamp;k++)
rate->input_buffer[k] = (float) (ibuf[k] >> 16);
} else {
for(k=0;k<*isamp;k++)
rate->input_buffer[k] = 0.0;
}
temp_buf = rate->input_buffer;
t1 = rate->l1;
t2 = rate->l2;
for(k=0;k<rate->total;k++,t1=t1->next,t2=t2->next) {
polyphase_init(rate->filt_array[k], rate->filt_len[k],
t1->number,t2->number);
out_size = (in_size) * t1->number / t2->number;
temp_buf2 = t1->data_buffer;
polyphase(temp_buf, temp_buf2, rate->past_hist[k], in_size);
for(j = 0; j < rate->filt_len[k]; j++)
rate->past_hist[k][j] = temp_buf[j+in_size - rate->filt_len[k]];
in_size = out_size;
temp_buf = temp_buf2;
}
if(out_size > *osamp)
out_size = *osamp;
*osamp = out_size;
if(ibuf != NULL) {
for(k=0;k < out_size;k++)
obuf[k] = ((int) temp_buf[k]) << 16;
} else {
/* Wait for all-zero samples to come through.
Should happen eventually with all-zero
input */
int found = 0;
for(k=0; k < out_size; k++) {
obuf[k] = ((int) temp_buf[k] << 16);
if(obuf[k] != 0)
found = 1;
}
if(!found)
*osamp = 0;
}
}
/*
* Process tail of input samples.
*/
void poly_drain(effp, obuf, osamp)
eff_t effp;
long *obuf;
long *osamp;
{
long in_size = 1024;
/* Call "flow" with NULL input. */
poly_flow(effp, NULL, obuf, &in_size, osamp);
}
/*
* Do anything required when you stop reading samples.
* Don't close input file!
*/
void poly_stop(effp)
eff_t effp;
{
List *t, *t2;
poly_t rate = (poly_t) effp->priv;
int k;
/* Free lists */
for(t = rate->l1; t != NULL; ) {
t2 = t->next;
t->next = NULL;
if(t->data_buffer != NULL)
free((void *) t->data_buffer);
free((void *) t);
t = t2;
}
for(t = rate->l2; t != NULL; ) {
t2 = t->next;
t->next = NULL;
if(t->data_buffer != NULL)
free((void *) t->data_buffer);
free((void *) t);
t = t2;
}
for(k = 0; k < rate->total;k++) {
free((void *) rate->past_hist[k]);
free((void *) rate->filt_array[k]);
}
free((void *) rate->past_hist);
free((void *) rate->filt_array);
free((void *) rate->filt_len);
}