home *** CD-ROM | disk | FTP | other *** search
/ AmigActive 3 / AACD03.BIN / AACD / Sound / SoX / Source / polyphas.c < prev    next >
C/C++ Source or Header  |  1999-07-18  |  15KB  |  652 lines
  1.  
  2. /*
  3.  * July 14, 1998
  4.  * Copyright 1998  K. Bradley, Carnegie Mellon University
  5.  *
  6.  * This source code is freely redistributable and may be used for
  7.  * any purpose.  This copyright notice must be maintained. 
  8.  * Lance Norskog And Sundry Contributors are not responsible for 
  9.  * the consequences of using this software.
  10.  */
  11.  
  12. /*
  13.  * Sound Tools rate change effect file.
  14.  */
  15.  
  16. #include <math.h>
  17. #include <stdio.h>
  18. #include <stdlib.h>
  19. #ifdef HAVE_MALLOC_H
  20. #include <malloc.h>
  21. #endif
  22. #include "st.h"
  23.  
  24. typedef struct _list {
  25.     int number;
  26.     float *data_buffer;
  27.     struct _list *next;
  28. } List;
  29.  
  30. typedef struct polyphase {
  31.  
  32.   unsigned long    lcmrate;       /* least common multiple of rates */
  33.   unsigned long inskip, outskip;   /* LCM increments for I & O rates */
  34.   unsigned long total;
  35.   unsigned long intot, outtot;       /* total samples in terms of LCM rate */
  36.   long    lastsamp;
  37.  
  38.   float **filt_array;
  39.   float **past_hist;
  40.   float *input_buffer;
  41.   int *filt_len;
  42.  
  43.   List *l1, *l2;
  44.   
  45. } *poly_t;
  46.  
  47. /*
  48.  * Process options
  49.  */
  50.  
  51. /* Options:  
  52.  
  53.    -w <nut / ham>        :  window type
  54.    -width <short / long> :  window width
  55.                             short = 128 samples
  56.                 long  = 1024 samples
  57.       <num>                num:  explicit number
  58.  
  59.    -cutoff <float>       :  frequency cutoff for base bandwidth.
  60.                             Default = 0.95 = 95%
  61. */
  62.  
  63. static int win_type  = 0;
  64. static int win_width = 1024;
  65. static float cutoff = 0.95;
  66.    
  67. void poly_getopts(effp, n, argv) 
  68. eff_t effp;
  69. int n;
  70. char **argv;
  71. {
  72.   /* 0: nuttall
  73.      1: hamming */
  74.   win_type = 0;
  75.  
  76.   /* width:  short = 128
  77.              long = 1024 (default) */
  78.   win_width = 1024;
  79.  
  80.   /* cutoff:  frequency cutoff of base bandwidth in percentage. */
  81.   cutoff = 0.95;
  82.  
  83.   while(n >= 2) {
  84.  
  85.     /* Window type check */
  86.     if(!strcmp(argv[0], "-w")) {
  87.       if(!strcmp(argv[1], "ham"))
  88.     win_type = 1;
  89.       if(!strcmp(argv[1], "nut"))
  90.     win_type = 0;
  91.  
  92.       argv += 2;
  93.       n -= 2;
  94.       continue;
  95.     }
  96.  
  97.     /* Window width check */
  98.     if(!strcmp(argv[0], "-width")) {
  99.       if(!strcmp(argv[1], "short"))
  100.     win_width = 128;
  101.       else if(!strcmp(argv[1], "long"))
  102.     win_width = 1024;
  103.       else
  104.     win_width = atoi(argv[1]);
  105.  
  106.       argv += 2;
  107.       n -= 2;
  108.       continue;
  109.     }
  110.  
  111.     /* Cutoff frequency check */
  112.     if(!strcmp(argv[0], "-cutoff")) {
  113.       cutoff = atof(argv[1]);
  114.       argv += 2;
  115.       n -= 2;
  116.       continue;
  117.     }
  118.  
  119.     fail("Polyphase: unknown argument (%s %s)!", argv[0], argv[1]);
  120.   }
  121. }
  122.  
  123. /*
  124.  * Prepare processing.
  125.  */
  126.  
  127. static int primes[] = {
  128.   2, 3, 5, 7, 11, 13, 17, 19, 23, 29, 31, 37,
  129.   41, 43, 47, 53, 59, 61, 67, 71, 73, 79, 83, 89,
  130.   97, 101, 103, 107, 109, 113, 127, 131, 137, 139, 149, 151,
  131.   157, 163, 167, 173, 179, 181, 191, 193, 197, 199, 211, 223,
  132.   227, 229, 233, 239, 241, 251, 257, 263, 269, 271, 277, 281,
  133.   283, 293, 307, 311, 313, 317, 331, 337, 347, 349, 353, 359,
  134.   367, 373, 379, 383, 389, 397, 401, 409, 419, 421, 431, 433,
  135.   439, 443, 449, 457, 461, 463, 467, 479, 487, 491, 499, 503,
  136.   509, 521, 523, 541, 547, 557, 563, 569, 571, 577, 587, 593,
  137.   599, 601, 607, 613, 617, 619, 631, 641, 643, 647, 653, 659,
  138.   661, 673, 677, 683, 691, 701, 709, 719, 727, 733, 739, 743,
  139.   751, 757, 761, 769, 773, 787, 797, 809, 811, 821, 823, 827,
  140.   829, 839, 853, 857, 859, 863, 877, 881, 883, 887, 907, 911,
  141.   919, 929, 937, 941, 947, 953, 967, 971, 977, 983, 991, 997
  142. };
  143.  
  144. #ifndef max
  145. #define max(x,y) ((x > y) ? x : y)
  146. #endif
  147.  
  148. List *prime(number)
  149. int number;
  150. {
  151.     int j;
  152.     List *element = NULL;
  153.  
  154.     if(number == 1)
  155.       return NULL;
  156.  
  157.     for(j=167;j>= 0;j--) {
  158.     if(number % primes[j] == 0) {
  159.         element = (List *) malloc(sizeof(List));
  160.         element->number = primes[j];
  161.         element->data_buffer = NULL;
  162.         element->next = prime(number / primes[j]);
  163.         break;
  164.     }
  165.     }
  166.  
  167.     if(element == NULL) {
  168.     fail("Number %d too large of a prime.\n",number);
  169.     }
  170.  
  171.     return element;
  172. }
  173.  
  174. List *prime_inv(number)
  175. int number;
  176. {
  177.     int j;
  178.     List *element = NULL;
  179.  
  180.     if(number == 1)
  181.       return NULL;
  182.  
  183.     for(j=0;j<168;j++) {
  184.     if(number % primes[j] == 0) {
  185.         element = (List *) malloc(sizeof(List));
  186.         element->number = primes[j];
  187.         element->data_buffer = NULL;
  188.         element->next = prime_inv(number / primes[j]);
  189.         break;
  190.     }
  191.     }
  192.  
  193.     if(element == NULL) {
  194.     fail("Number %d too large of a prime.\n",number);
  195.     }
  196.  
  197.     return element;
  198. }
  199.  
  200. #ifndef PI
  201. #define PI 3.14159265358979
  202. #endif
  203.  
  204. /* Calculate a Nuttall window of a given length.
  205.    Buffer must already be allocated to appropriate size.
  206.    */
  207.  
  208. void nuttall(buffer, length)
  209. float *buffer;
  210. int length;
  211. {
  212.   int j;
  213.   double N;
  214.   double N1;
  215.  
  216.   if(buffer == NULL || length < 0)
  217.     fail("Illegal buffer %p or length %d to nuttall.\n", buffer, length);
  218.  
  219.   /* Initial variable setups. */
  220.   N = (double) length - 1.0;
  221.   N1 = N / 2.0;
  222.  
  223.   for(j = 0; j < length; j++) {
  224.     buffer[j] = 0.36335819 + 
  225.       0.4891775 * cos(2*PI*1*(j - N1) / N) +
  226.       0.1365995 * cos(2*PI*2*(j - N1) / N) + 
  227.       0.0106411 * cos(2*PI*3*(j - N1) / N);
  228.   }
  229. }
  230. /* Calculate a Hamming window of given length.
  231.    Buffer must already be allocated to appropriate size.
  232. */
  233.  
  234. void hamming(buffer, length)
  235. float *buffer;
  236. int length;
  237. {
  238.     int j;
  239.  
  240.     if(buffer == NULL || length < 0)
  241.       fail("Illegal buffer %p or length %d to hamming.\n",buffer,length);
  242.  
  243.     for(j=0;j<length;j++) 
  244.       buffer[j] = 0.5 - 0.46 * cos(2*PI*j/(length-1));
  245. }
  246.  
  247. /* Calculate the sinc function properly */
  248.  
  249. float sinc(value)
  250. float value;
  251. {   
  252.     return(fabs(value) < 1E-50 ? 1.0 : sin(value) / value);
  253. }
  254.  
  255. /* Design a low-pass FIR filter using window technique.
  256.    Length of filter is in length, cutoff frequency in cutoff.
  257.    0 < cutoff <= 1.0 (normalized frequency)
  258.  
  259.    buffer must already be allocated.
  260. */
  261. void fir_design(buffer, length, cutoff)
  262. float *buffer;
  263. int length;
  264. float cutoff;
  265. {
  266.     int j;
  267.     float sum;
  268.     float *ham_win;
  269.  
  270.     if(buffer == NULL || length < 0 || cutoff < 0 || cutoff > PI)
  271.       fail("Illegal buffer %p, length %d, or cutoff %f.\n",buffer,length,cutoff);
  272.  
  273.     /* Design Hamming window:  43 dB cutoff */
  274.     ham_win = (float *)malloc(sizeof(float) * length);
  275.  
  276.     /* Use the user-option of window type */
  277.     if(win_type == 0) 
  278.       nuttall(ham_win, length);
  279.     else
  280.       hamming(ham_win,length);
  281.  
  282.     /* Design filter:  windowed sinc function */
  283.     sum = 0.0;
  284.     for(j=0;j<length;j++) {
  285.     buffer[j] = sinc(PI*cutoff*(j-length/2)) * ham_win[j] / (2*cutoff);
  286.     sum += buffer[j];
  287.     }
  288.  
  289.     /* Normalize buffer to have gain of 1.0: prevent roundoff error */
  290.     for(j=0;j<length;j++)
  291.       buffer[j] /= sum;
  292.  
  293.     free((void *) ham_win);
  294. }
  295.     
  296.  
  297. void poly_start(effp)
  298. eff_t effp;
  299. {
  300.     poly_t rate = (poly_t) effp->priv;
  301.     List *t, *t2;
  302.     int num_l1, num_l2;
  303.     int j,k;
  304.     float f_cutoff;
  305.  
  306.     extern long lcm();
  307.     
  308.     rate->lcmrate = lcm((long)effp->ininfo.rate, (long)effp->outinfo.rate);
  309.  
  310.     /* Cursory check for LCM overflow.  
  311.      * If both rate are below 65k, there should be no problem.
  312.      * 16 bits x 16 bits = 32 bits, which we can handle.
  313.      */
  314.  
  315.     rate->inskip = rate->lcmrate / effp->ininfo.rate;
  316.     rate->outskip = rate->lcmrate / effp->outinfo.rate; 
  317.  
  318.     /* Find the prime factors of inskip and outskip */
  319.     rate->l1 = prime(rate->inskip);
  320.  
  321.     /* If we're going up, order things backwards. */
  322.     if(effp->ininfo.rate < effp->outinfo.rate)
  323.       rate->l2 = prime_inv(rate->outskip);
  324.     else
  325.       rate->l2 = prime(rate->outskip);
  326.     
  327.     /* Find how many factors there were */
  328.     if(rate->l1 == NULL)
  329.       num_l1 = 0;
  330.     else
  331.       for(num_l1=0, t = rate->l1; t != NULL; num_l1++, t=t->next);
  332.  
  333.     if(rate->l2 == NULL) 
  334.       num_l2 = 0;
  335.     else
  336.       for(num_l2=0, t = rate->l2; t != NULL; num_l2++, t=t->next);
  337.  
  338.     k = 0;
  339.     t = rate->l1;
  340.  
  341.     /* Compact the lists to be less than 10 */
  342.     while(k < num_l1 - 1) {
  343.     if(t->number * t->next->number < 10) {
  344.         t->number = t->number * t->next->number;
  345.         t2 = t->next;
  346.         t->next = t->next->next;
  347.         t2->next = NULL;
  348.         free((void *) t2);
  349.         num_l1--;
  350.     } else {
  351.         k++;
  352.         t = t->next;
  353.     }
  354.     }
  355.  
  356.     k = 0;
  357.     t = rate->l2;
  358.  
  359.     while(k < num_l2 - 1) {
  360.     if(t->number * t->next->number < 10) {
  361.         t->number = t->number * t->next->number;
  362.         t2 = t->next;
  363.         t->next = t->next->next;
  364.         t2->next = NULL;
  365.         free((void *) t2);
  366.         num_l2--;
  367.     } else {
  368.         k++;
  369.         t = t->next;
  370.     }
  371.     }
  372.  
  373.     /* l1 and l2 are now lists of the prime factors compacted,
  374.        meaning that they're the lists of up/down sampling we need
  375.        */
  376.  
  377.     /* Stretch them to be the same length by padding with 1 (no-op) */
  378.     if(num_l1 < num_l2) {
  379.     t = rate->l1;
  380.  
  381.     if(t == NULL) {
  382.         rate->l1 = (List *)malloc(sizeof(List));
  383.         rate->l1->next = NULL;
  384.         rate->l1->number = 1;
  385.         rate->l1->data_buffer = NULL;
  386.         t = rate->l1;
  387.         num_l1++;
  388.     }
  389.  
  390.     while(t->next != NULL)
  391.       t = t->next;
  392.  
  393.     for(k=0;k<num_l2-num_l1;k++) {
  394.         t->next = (List *) malloc(sizeof(List));
  395.         t->next->number = 1;
  396.         t->next->data_buffer = NULL;
  397.         t = t->next;
  398.     }
  399.  
  400.     t->next = NULL;
  401.     num_l1 = num_l2;
  402.     } else {
  403.     t = rate->l2;
  404.  
  405.     if(t == NULL) {
  406.         rate->l2 = (List *)malloc(sizeof(List));
  407.         rate->l2->next = NULL;
  408.         rate->l2->number = 1;
  409.         rate->l2->data_buffer = NULL;
  410.         t = rate->l2;
  411.         num_l2++;
  412.     }
  413.       
  414.     /*
  415.       while(t->next != NULL)
  416.       t = t->next;
  417.       */
  418.  
  419.     for(k=0;k<num_l1-num_l2;k++) {
  420.       t = rate->l2;
  421.       rate->l2 = (List *) malloc(sizeof(List));
  422.       rate->l2->number = 1;
  423.       rate->l2->data_buffer = NULL;
  424.       rate->l2->next = t;
  425.     }
  426.  
  427.     /* t->next = NULL; */
  428.     num_l2 = num_l1;
  429.     }
  430.  
  431.     /* l1 and l2 are now the same size. */
  432.     rate->total = num_l1;
  433.  
  434.     report("Poly:  input rate %d, output rate %d.  %d stages.",effp->ininfo.rate, effp->outinfo.rate,num_l1);
  435.     report("Poly:  window: %s  size: %d  cutoff: %f.", (win_type == 0) ? ("nut") : ("ham"), win_width, cutoff);
  436.  
  437.     for(k=0, t=rate->l1, t2=rate->l2;k<num_l1;k++,t=t->next,t2=t2->next)
  438.       report("Poly:  stage %d:  Up by %d, down by %d.",k+1,t->number,t2->number);
  439.  
  440.     /* We'll have an array of filters and past history */
  441.     rate->filt_array = (float **) malloc(sizeof(float *) * num_l1);
  442.     rate->past_hist = (float **) malloc(sizeof(float *) * num_l1);
  443.     rate->filt_len = (int *) malloc(sizeof(int) * num_l1);
  444.  
  445.     for(k = 0, t = rate->l1, t2 = rate->l2; k < num_l1; k++) {
  446.  
  447.       rate->filt_len[k] = max(2 * 10 * max(t->number,t2->number), win_width);
  448.       rate->filt_array[k] = (float *) malloc(sizeof(float) * rate->filt_len[k]);
  449.       rate->past_hist[k] = (float *) malloc(sizeof(float) * rate->filt_len[k]);
  450.       
  451.       t->data_buffer = (float *) malloc(sizeof(float) * 1024 * rate->inskip);
  452.     
  453.       for(j = 0; j < rate->filt_len[k]; j++)
  454.     rate->past_hist[k][j] = 0.0;
  455.  
  456.       f_cutoff = (t->number > t2->number) ? 
  457.     (float) t->number : (float) t2->number;
  458.  
  459.       fir_design(rate->filt_array[k], rate->filt_len[k]-1, cutoff / f_cutoff);
  460.  
  461.       t = t->next;
  462.       t2 = t2->next;
  463.     }
  464.  
  465.     rate->input_buffer = (float *) malloc(sizeof(float) * 2048);
  466. }
  467.  
  468. /*
  469.  * Processed signed long samples from ibuf to obuf.
  470.  * Return number of samples processed.
  471.  */
  472.  
  473. static float *h;
  474. static int M, L, N;
  475.  
  476. void polyphase_init(coef, num_coef, up_rate, down_rate)
  477. float *coef;
  478. int num_coef;
  479. int up_rate;
  480. int down_rate;
  481. {
  482.     h = coef;
  483.     M = down_rate;
  484.     L = up_rate;
  485.     N = num_coef;
  486. }
  487.     
  488. void polyphase(input, output, past, num_samples_input)
  489. float *input;
  490. float *output;
  491. float *past;
  492. int num_samples_input;
  493. {
  494.     int num_output;
  495.     int m,n;
  496.     float sum;
  497.     float inp;
  498.     int base;
  499.     int h_base;
  500.  
  501.     num_output = num_samples_input * L / M;
  502.  
  503.     for(m=0;m<num_output;m++) {
  504.     sum = 0.0;
  505.     base = (int) (m*M/L);
  506.     h_base = (m*M) % L;
  507.  
  508.     for(n=0;n<N / L;n++) {
  509.         if(base - n < 0)
  510.           inp = past[base - n + N];
  511.         else
  512.           inp = input[base - n];
  513.  
  514.         sum += h[n*L + h_base] * inp;
  515.     }
  516.  
  517.     output[m] = sum * L * 0.95;
  518.     }
  519. }
  520.  
  521. void poly_flow(effp, ibuf, obuf, isamp, osamp)
  522. eff_t effp;
  523. long *ibuf, *obuf;
  524. int *isamp, *osamp;
  525. {
  526.   poly_t rate = (poly_t) effp->priv;
  527.   float *temp_buf, *temp_buf2;
  528.   int j,k;
  529.   List *t1, *t2;
  530.   int in_size, out_size;
  531.  
  532.   /* Sanity check:  how much can we tolerate? */
  533.   in_size = *isamp;
  534.   out_size = in_size * rate->inskip / rate->outskip;
  535.   if(out_size > *osamp) {
  536.     in_size = *osamp * rate->outskip / rate->inskip;
  537.     *isamp = in_size;
  538.   }
  539.   
  540.   /* Check to see if we're really draining */
  541.   if(ibuf != NULL) {
  542.     for(k=0;k<*isamp;k++)
  543.       rate->input_buffer[k] = (float) (ibuf[k] >> 16);
  544.   } else {
  545.     for(k=0;k<*isamp;k++)
  546.       rate->input_buffer[k] = 0.0;
  547.   }      
  548.   
  549.   temp_buf = rate->input_buffer;
  550.   
  551.   t1 = rate->l1;
  552.   t2 = rate->l2;
  553.   
  554.   for(k=0;k<rate->total;k++,t1=t1->next,t2=t2->next) {
  555.     
  556.     polyphase_init(rate->filt_array[k], rate->filt_len[k], 
  557.            t1->number,t2->number);
  558.     
  559.     out_size = (in_size) * t1->number / t2->number;
  560.     
  561.     temp_buf2 = t1->data_buffer;
  562.     
  563.     polyphase(temp_buf, temp_buf2, rate->past_hist[k], in_size);
  564.     
  565.     for(j = 0; j < rate->filt_len[k]; j++) 
  566.       rate->past_hist[k][j] = temp_buf[j+in_size - rate->filt_len[k]];
  567.     
  568.     in_size = out_size;
  569.     
  570.     temp_buf = temp_buf2;
  571.   }
  572.   
  573.   if(out_size > *osamp)
  574.     out_size = *osamp;
  575.   
  576.   *osamp = out_size;
  577.  
  578.   if(ibuf != NULL) {
  579.     for(k=0;k < out_size;k++)
  580.       obuf[k] = ((int) temp_buf[k]) << 16;
  581.   } else {
  582.  
  583.     /* Wait for all-zero samples to come through.
  584.        Should happen eventually with all-zero
  585.        input */
  586.     int found = 0;
  587.  
  588.     for(k=0; k < out_size; k++) {
  589.       obuf[k] = ((int) temp_buf[k] << 16);
  590.       if(obuf[k] != 0)
  591.     found = 1;
  592.     }
  593.     if(!found)
  594.       *osamp = 0;
  595.   }
  596. }
  597.  
  598. /*
  599.  * Process tail of input samples.
  600.  */
  601. void poly_drain(effp, obuf, osamp)
  602. eff_t effp;
  603. long *obuf;
  604. long *osamp;
  605. {
  606.   long in_size = 1024;
  607.  
  608.   /* Call "flow" with NULL input. */
  609.   poly_flow(effp, NULL, obuf, &in_size, osamp);
  610. }
  611.  
  612. /*
  613.  * Do anything required when you stop reading samples.  
  614.  * Don't close input file! 
  615.  */
  616. void poly_stop(effp)
  617. eff_t effp;
  618. {
  619.     List *t, *t2;
  620.     poly_t rate = (poly_t) effp->priv;
  621.     int k;
  622.  
  623.     /* Free lists */
  624.     for(t = rate->l1; t != NULL; ) {
  625.     t2 = t->next;
  626.     t->next = NULL;
  627.     if(t->data_buffer != NULL)
  628.       free((void *) t->data_buffer);
  629.     free((void *) t);
  630.     t = t2;
  631.     }
  632.  
  633.     for(t = rate->l2; t != NULL; ) {
  634.     t2 = t->next;
  635.     t->next = NULL;
  636.     if(t->data_buffer != NULL)
  637.       free((void *) t->data_buffer);
  638.     free((void *) t);
  639.     t = t2;
  640.     }
  641.  
  642.     for(k = 0; k < rate->total;k++) {
  643.     free((void *) rate->past_hist[k]);
  644.     free((void *) rate->filt_array[k]);
  645.     }
  646.  
  647.     free((void *) rate->past_hist);
  648.     free((void *) rate->filt_array);
  649.     free((void *) rate->filt_len);
  650. }
  651.  
  652.