Audio Resampler Implement

http://blog.csdn.net/sepnic/article/details/7387309

 

前些日子無聊實現的一個Audio PCM Resampler的代碼,僅僅支持採樣率爲44.1khz的源數據的向下轉換,可轉換成8khz/11.025khz/16khz/22.050khz。

接口設計參考android-4.0.3_r1\system\media\audio_utils\include\audio_utils\resampler.h,因此使用方法也和Android的那套接口保持一致。

  1. // File:   resampler.h   
  2. // Author: Loon <[email protected]>   
  3.   
  4. #ifndef __AUDIO_RESAMPLER_H   
  5. #define __AUDIO_RESAMPLER_H   
  6.   
  7.   
  8. #include <stdint.h>   
  9.   
  10. struct resampler_buffer {  
  11.     union {  
  12.         void*       raw;  
  13.         short*      i16;  
  14.         int8_t*     i8;  
  15.     };  
  16.     size_t frame_count;  
  17. };  
  18.   
  19. /* call back interface used by the resampler to get new data */  
  20. struct resampler_buffer_provider {  
  21.     /** 
  22.      *  get a new buffer of data: 
  23.      *   as input: buffer->frame_count is the number of frames requested 
  24.      *   as output: buffer->frame_count is the number of frames returned 
  25.      *              buffer->raw points to data returned 
  26.      */  
  27.     int (*get_next_buffer)(struct resampler_buffer_provider *provider,  
  28.             struct resampler_buffer *buffer);  
  29.     /** 
  30.      *  release a consumed buffer of data: 
  31.      *   as input: buffer->frame_count is the number of frames released 
  32.      *             buffer->raw points to data released 
  33.      */  
  34.     void (*release_buffer)(struct resampler_buffer_provider *provider,  
  35.             struct resampler_buffer *buffer);  
  36. };  
  37.   
  38. struct resampler_itfe {  
  39.     /** 
  40.      * reset resampler state 
  41.      */  
  42.     void (*reset)(struct resampler_itfe *resampler);  
  43.     /** 
  44.      * resample input from buffer provider and output at most *outFrameCount to out buffer. 
  45.      * *outFrameCount is updated with the actual number of frames produced. 
  46.      */  
  47.     int (*resample_from_provider)(struct resampler_itfe *resampler,  
  48.                     int16_t *out,  
  49.                     size_t *outFrameCount);  
  50.     /** 
  51.      * resample at most *inFrameCount frames from in buffer and output at most 
  52.      * *outFrameCount to out buffer. *inFrameCount and *outFrameCount are updated respectively 
  53.      * with the number of frames remaining in input and written to output. 
  54.      */  
  55.     int (*resample_from_input)(struct resampler_itfe *resampler,  
  56.                     int16_t *in,  
  57.                     size_t *inFrameCount,  
  58.                     int16_t *out,  
  59.                     size_t *outFrameCount);  
  60. };  
  61.   
  62. /** 
  63.  * create a resampler according to input parameters passed. 
  64.  * If resampler_buffer_provider is not NULL only resample_from_provider() can be called. 
  65.  * If resampler_buffer_provider is NULL only resample_from_input() can be called. 
  66.  */  
  67. int create_down_resampler(uint32_t outSampleRate,  
  68.           uint32_t channelCount,  
  69.           uint32_t frame_count,  
  70.           struct resampler_buffer_provider *provider,  
  71.           struct resampler_itfe **);  
  72.   
  73. /** 
  74.  * release resampler resources. 
  75.  */  
  76. void release_down_resampler(struct resampler_itfe *);  
  77.   
  78.   
  79. #endif /** __AUDIO_RESAMPLER_H */  
// File:   resampler.h
// Author: Loon <[email protected]>

#ifndef __AUDIO_RESAMPLER_H
#define __AUDIO_RESAMPLER_H


#include <stdint.h>

struct resampler_buffer {
    union {
        void*       raw;
        short*      i16;
        int8_t*     i8;
    };
    size_t frame_count;
};

/* call back interface used by the resampler to get new data */
struct resampler_buffer_provider {
    /**
     *  get a new buffer of data:
     *   as input: buffer->frame_count is the number of frames requested
     *   as output: buffer->frame_count is the number of frames returned
     *              buffer->raw points to data returned
     */
    int (*get_next_buffer)(struct resampler_buffer_provider *provider,
            struct resampler_buffer *buffer);
    /**
     *  release a consumed buffer of data:
     *   as input: buffer->frame_count is the number of frames released
     *             buffer->raw points to data released
     */
    void (*release_buffer)(struct resampler_buffer_provider *provider,
            struct resampler_buffer *buffer);
};

struct resampler_itfe {
    /**
     * reset resampler state
     */
    void (*reset)(struct resampler_itfe *resampler);
    /**
     * resample input from buffer provider and output at most *outFrameCount to out buffer.
     * *outFrameCount is updated with the actual number of frames produced.
     */
    int (*resample_from_provider)(struct resampler_itfe *resampler,
                    int16_t *out,
                    size_t *outFrameCount);
    /**
     * resample at most *inFrameCount frames from in buffer and output at most
     * *outFrameCount to out buffer. *inFrameCount and *outFrameCount are updated respectively
     * with the number of frames remaining in input and written to output.
     */
    int (*resample_from_input)(struct resampler_itfe *resampler,
                    int16_t *in,
                    size_t *inFrameCount,
                    int16_t *out,
                    size_t *outFrameCount);
};

/**
 * create a resampler according to input parameters passed.
 * If resampler_buffer_provider is not NULL only resample_from_provider() can be called.
 * If resampler_buffer_provider is NULL only resample_from_input() can be called.
 */
int create_down_resampler(uint32_t outSampleRate,
          uint32_t channelCount,
          uint32_t frame_count,
          struct resampler_buffer_provider *provider,
          struct resampler_itfe **);

/**
 * release resampler resources.
 */
void release_down_resampler(struct resampler_itfe *);


#endif /** __AUDIO_RESAMPLER_H */


  1. // File:   resampler.c   
  2. // Author: Loon <[email protected]>   
  3.   
  4. #include <stdio.h>   
  5. #include <unistd.h>   
  6. #include <sys/types.h>   
  7. #include <fcntl.h>   
  8. #include <errno.h>   
  9. #include "resampler.h"   
  10.   
  11. struct down_resampler {  
  12.     struct resampler_itfe itfe;  
  13.       
  14.     struct resampler_buffer_provider *provider; // buffer provider installed by client   
  15.     uint32_t out_sample_rate;                   // output sampling rate in Hz   
  16.     uint32_t channel_count;                     // number of channels (interleaved)   
  17.     uint32_t frame_count;                       // number of frames   
  18.       
  19.     int16_t *mInLeft;  
  20.     int16_t *mInRight;  
  21.     int16_t *mTmpLeft;  
  22.     int16_t *mTmpRight;  
  23.     int16_t *mTmp2Left;  
  24.     int16_t *mTmp2Right;  
  25.     int16_t *mOutLeft;  
  26.     int16_t *mOutRight;  
  27.     int mInInBuf;  
  28.     int mInTmpBuf;  
  29.     int mInTmp2Buf;  
  30.     int mOutBufPos;  
  31.     int mInOutBuf;  
  32. };  
  33.   
  34.   
  35. /*------------------------------------------------------------------------------*/  
  36.   
  37.   
  38. /*------------------------------------------------------------------------------*/  
  39.   
  40. /*  DownSampler Algorithm Implement */  
  41.   
  42. /* 
  43.  * 2.30 fixed point FIR filter coefficients for conversion 44100 -> 22050. 
  44.  * (Works equivalently for 22010 -> 11025 or any other halving, of course.) 
  45.  * 
  46.  * Transition band from about 18 kHz, passband ripple < 0.1 dB, 
  47.  * stopband ripple at about -55 dB, linear phase. 
  48.  * 
  49.  * Design and display in MATLAB or Octave using: 
  50.  * 
  51.  * filter = fir1(19, 0.5); filter = round(filter * 2**30); freqz(filter * 2**-30); 
  52.  */  
  53. static const int32_t filter_22khz_coeff[] = {  
  54.     2089257, 2898328, -5820678, -10484531,  
  55.     19038724, 30542725, -50469415, -81505260,  
  56.     152544464, 478517512, 478517512, 152544464,  
  57.     -81505260, -50469415, 30542725, 19038724,  
  58.     -10484531, -5820678, 2898328, 2089257,  
  59. };  
  60. #define NUM_COEFF_22KHZ (sizeof(filter_22khz_coeff) / sizeof(filter_22khz_coeff[0]))   
  61. #define OVERLAP_22KHZ (NUM_COEFF_22KHZ - 2)   
  62.   
  63. /* 
  64.  * Convolution of signals A and reverse(B). (In our case, the filter response 
  65.  * is symmetric, so the reversing doesn't matter.) 
  66.  * A is taken to be in 0.16 fixed-point, and B is taken to be in 2.30 fixed-point. 
  67.  * The answer will be in 16.16 fixed-point, unclipped. 
  68.  * 
  69.  * This function would probably be the prime candidate for SIMD conversion if 
  70.  * you want more speed. 
  71.  */  
  72. int32_t fir_convolve(const int16_t* a, const int32_t* b, int num_samples)  
  73. {  
  74.         int32_t sum = 1 << 13;  
  75.         for (int i = 0; i < num_samples; ++i) {  
  76.                 sum += a[i] * (b[i] >> 16);  
  77.         }  
  78.         return sum >> 14;  
  79. }  
  80.   
  81. /* Clip from 16.16 fixed-point to 0.16 fixed-point. */  
  82. int16_t clip(int32_t x)  
  83. {  
  84.     if (x < -32768) {  
  85.         return -32768;  
  86.     } else if (x > 32767) {  
  87.         return 32767;  
  88.     } else {  
  89.         return x;  
  90.     }  
  91. }  
  92.   
  93. /* 
  94.  * Convert a chunk from 44 kHz to 22 kHz. Will update num_samples_in and num_samples_out 
  95.  * accordingly, since it may leave input samples in the buffer due to overlap. 
  96.  * 
  97.  * Input and output are taken to be in 0.16 fixed-point. 
  98.  */  
  99. void resample_2_1(int16_t* input, int16_t* output, int* num_samples_in, int* num_samples_out)  
  100. {  
  101.     if (*num_samples_in < (int)NUM_COEFF_22KHZ) {  
  102.         *num_samples_out = 0;  
  103.         return;  
  104.     }  
  105.   
  106.     int odd_smp = *num_samples_in & 0x1;  
  107.     int num_samples = *num_samples_in - odd_smp - OVERLAP_22KHZ;  
  108.   
  109.     for (int i = 0; i < num_samples; i += 2) {  
  110.             output[i / 2] = clip(fir_convolve(input + i, filter_22khz_coeff, NUM_COEFF_22KHZ));  
  111.     }  
  112.   
  113.     memmove(input, input + num_samples, (OVERLAP_22KHZ + odd_smp) * sizeof(*input));  
  114.     *num_samples_out = num_samples / 2;  
  115.     *num_samples_in = OVERLAP_22KHZ + odd_smp;  
  116. }  
  117.   
  118. /* 
  119.  * 2.30 fixed point FIR filter coefficients for conversion 22050 -> 16000, 
  120.  * or 11025 -> 8000. 
  121.  * 
  122.  * Transition band from about 14 kHz, passband ripple < 0.1 dB, 
  123.  * stopband ripple at about -50 dB, linear phase. 
  124.  * 
  125.  * Design and display in MATLAB or Octave using: 
  126.  * 
  127.  * filter = fir1(23, 16000 / 22050); filter = round(filter * 2**30); freqz(filter * 2**-30); 
  128.  */  
  129. static const int32_t filter_16khz_coeff[] = {  
  130.     2057290, -2973608, 1880478, 4362037,  
  131.     -14639744, 18523609, -1609189, -38502470,  
  132.     78073125, -68353935, -59103896, 617555440,  
  133.     617555440, -59103896, -68353935, 78073125,  
  134.     -38502470, -1609189, 18523609, -14639744,  
  135.     4362037, 1880478, -2973608, 2057290,  
  136. };  
  137. #define NUM_COEFF_16KHZ (sizeof(filter_16khz_coeff) / sizeof(filter_16khz_coeff[0]))   
  138. #define OVERLAP_16KHZ (NUM_COEFF_16KHZ - 1)   
  139.   
  140. /* 
  141.  * Convert a chunk from 22 kHz to 16 kHz. Will update num_samples_in and 
  142.  * num_samples_out accordingly, since it may leave input samples in the buffer 
  143.  * due to overlap. 
  144.  * 
  145.  * This implementation is rather ad-hoc; it first low-pass filters the data 
  146.  * into a temporary buffer, and then converts chunks of 441 input samples at a 
  147.  * time into 320 output samples by simple linear interpolation. A better 
  148.  * implementation would use a polyphase filter bank to do these two operations 
  149.  * in one step. 
  150.  * 
  151.  * Input and output are taken to be in 0.16 fixed-point. 
  152.  */  
  153.   
  154. #define RESAMPLE_16KHZ_SAMPLES_IN 441   
  155. #define RESAMPLE_16KHZ_SAMPLES_OUT 320   
  156.   
  157. void resample_441_320(int16_t* input, int16_t* output, int* num_samples_in, int* num_samples_out)  
  158. {  
  159.     const int num_blocks = (*num_samples_in - OVERLAP_16KHZ) / RESAMPLE_16KHZ_SAMPLES_IN;  
  160.     if (num_blocks < 1) {  
  161.         *num_samples_out = 0;  
  162.         return;  
  163.     }  
  164.   
  165.     for (int i = 0; i < num_blocks; ++i) {  
  166.         uint32_t tmp[RESAMPLE_16KHZ_SAMPLES_IN];  
  167.         for (int j = 0; j < RESAMPLE_16KHZ_SAMPLES_IN; ++j) {  
  168.             tmp[j] = fir_convolve(input + i * RESAMPLE_16KHZ_SAMPLES_IN + j,  
  169.                           filter_16khz_coeff,  
  170.                           NUM_COEFF_16KHZ);  
  171.         }  
  172.   
  173.         const float step_float = (float)RESAMPLE_16KHZ_SAMPLES_IN / (float)RESAMPLE_16KHZ_SAMPLES_OUT;  
  174.   
  175.         uint32_t in_sample_num = 0;   // 16.16 fixed point   
  176.         const uint32_t step = (uint32_t)(step_float * 65536.0f + 0.5f);  // 16.16 fixed point   
  177.         for (int j = 0; j < RESAMPLE_16KHZ_SAMPLES_OUT; ++j, in_sample_num += step) {  
  178.             const uint32_t whole = in_sample_num >> 16;  
  179.             const uint32_t frac = (in_sample_num & 0xffff);  // 0.16 fixed point   
  180.             const int32_t s1 = tmp[whole];  
  181.             const int32_t s2 = tmp[whole + 1];  
  182.             *output++ = clip(s1 + (((s2 - s1) * (int32_t)frac) >> 16));  
  183.         }  
  184.     }  
  185.   
  186.     const int samples_consumed = num_blocks * RESAMPLE_16KHZ_SAMPLES_IN;  
  187.     memmove(input, input + samples_consumed, (*num_samples_in - samples_consumed) * sizeof(*input));  
  188.     *num_samples_in -= samples_consumed;  
  189.     *num_samples_out = RESAMPLE_16KHZ_SAMPLES_OUT * num_blocks;  
  190. }  
  191.   
  192.   
  193. /*------------------------------------------------------------------------------*/  
  194.   
  195.   
  196. /*------------------------------------------------------------------------------*/  
  197.   
  198. /* DownSampler Callback Functions */  
  199.   
  200. static void down_resampler_reset(struct resampler_itfe *resampler)  
  201. {  
  202.     struct down_resampler *rsmp =   
  203.         (struct down_resampler *)((char *)resampler - offsetof(struct down_resampler, itfe));  
  204.   
  205.     if (rsmp == NULL) {  
  206.         return;  
  207.     }  
  208.   
  209.     rsmp->mInInBuf = 0;  
  210.     rsmp->mInTmpBuf = 0;  
  211.     rsmp->mInTmp2Buf = 0;  
  212.     rsmp->mOutBufPos = 0;  
  213.     rsmp->mInOutBuf = 0;  
  214. }  
  215.   
  216. // outputs a number of frames less or equal to *outFrameCount and updates *outFrameCount   
  217. // with the actual number of frames produced.   
  218. static int down_resampler_resample_from_provider(struct resampler_itfe *resampler,  
  219.                        int16_t *out,  
  220.                        size_t *outFrameCount)  
  221. {  
  222.     struct down_resampler *rsmp =   
  223.         (struct down_resampler *)((char *)resampler - offsetof(struct down_resampler, itfe));  
  224.       
  225.     if (rsmp == NULL || out == NULL || outFrameCount == NULL) {  
  226.         return -EINVAL;  
  227.     }  
  228.     if (rsmp->provider == NULL) {  
  229.         *outFrameCount = 0;  
  230.         return -ENOSYS;  
  231.     }  
  232.       
  233.     int16_t *outLeft = rsmp->mTmp2Left;  
  234.     int16_t *outRight = rsmp->mTmp2Left;  
  235.     if (rsmp->out_sample_rate == 22050) {  
  236.         outLeft = rsmp->mTmpLeft;  
  237.         outRight = rsmp->mTmpRight;  
  238.     } else if (rsmp->out_sample_rate == 8000){  
  239.         outLeft = rsmp->mOutLeft;  
  240.         outRight = rsmp->mOutRight;  
  241.     }  
  242.   
  243.     int outFrames = 0;  
  244.     int remaingFrames = *outFrameCount;  
  245.   
  246.     if (rsmp->mInOutBuf) {  
  247.         int frames = (remaingFrames > rsmp->mInOutBuf) ? rsmp->mInOutBuf : remaingFrames;  
  248.   
  249.         for (int i = 0; i < frames; ++i) {  
  250.             out[i] = outLeft[rsmp->mOutBufPos + i];  
  251.         }  
  252.         if (rsmp->channel_count == 2) {  
  253.             for (int i = 0; i < frames; ++i) {  
  254.                 out[i * 2] = outLeft[rsmp->mOutBufPos + i];  
  255.                 out[i * 2 + 1] = outRight[rsmp->mOutBufPos + i];  
  256.             }  
  257.         }  
  258.         remaingFrames -= frames;  
  259.         rsmp->mInOutBuf -= frames;  
  260.         rsmp->mOutBufPos += frames;  
  261.         outFrames += frames;  
  262.     }  
  263.   
  264.     while (remaingFrames) {  
  265.         struct resampler_buffer buf;  
  266.         buf.frame_count =  rsmp->frame_count - rsmp->mInInBuf;  
  267.         int ret = rsmp->provider->get_next_buffer(rsmp->provider, &buf);  
  268.         if (buf.raw == NULL) {  
  269.             *outFrameCount = outFrames;  
  270.             return ret;  
  271.         }  
  272.   
  273.         for (size_t i = 0; i < buf.frame_count; ++i) {  
  274.             rsmp->mInLeft[i + rsmp->mInInBuf] = buf.i16[i];  
  275.         }  
  276.         if (rsmp->channel_count == 2) {  
  277.             for (size_t i = 0; i < buf.frame_count; ++i) {  
  278.                 rsmp->mInLeft[i + rsmp->mInInBuf] = buf.i16[i * 2];  
  279.                 rsmp->mInRight[i + rsmp->mInInBuf] = buf.i16[i * 2 + 1];  
  280.             }  
  281.         }  
  282.         rsmp->mInInBuf += buf.frame_count;  
  283.         rsmp->provider->release_buffer(rsmp->provider, &buf);  
  284.   
  285.         /* 44010 -> 22050 */  
  286.         {  
  287.             int samples_in_left = rsmp->mInInBuf;  
  288.             int samples_out_left;  
  289.             resample_2_1(rsmp->mInLeft, rsmp->mTmpLeft + rsmp->mInTmpBuf, &samples_in_left, &samples_out_left);  
  290.   
  291.             if (rsmp->channel_count == 2) {  
  292.                 int samples_in_right = rsmp->mInInBuf;  
  293.                 int samples_out_right;  
  294.                 resample_2_1(rsmp->mInRight, rsmp->mTmpRight + rsmp->mInTmpBuf, &samples_in_right, &samples_out_right);  
  295.             }  
  296.   
  297.             rsmp->mInInBuf = samples_in_left;  
  298.             rsmp->mInTmpBuf += samples_out_left;  
  299.             rsmp->mInOutBuf = samples_out_left;  
  300.         }  
  301.   
  302.         if (rsmp->out_sample_rate == 11025 || rsmp->out_sample_rate == 8000) {  
  303.             /* 22050 - > 11025 */  
  304.             int samples_in_left = rsmp->mInTmpBuf;  
  305.             int samples_out_left;  
  306.             resample_2_1(rsmp->mTmpLeft, rsmp->mTmp2Left + rsmp->mInTmp2Buf, &samples_in_left, &samples_out_left);  
  307.   
  308.             if (rsmp->channel_count == 2) {  
  309.                 int samples_in_right = rsmp->mInTmpBuf;  
  310.                 int samples_out_right;  
  311.                 resample_2_1(rsmp->mTmpRight, rsmp->mTmp2Right + rsmp->mInTmp2Buf, &samples_in_right, &samples_out_right);  
  312.             }  
  313.   
  314.   
  315.             rsmp->mInTmpBuf = samples_in_left;  
  316.             rsmp->mInTmp2Buf += samples_out_left;  
  317.             rsmp->mInOutBuf = samples_out_left;  
  318.   
  319.             if (rsmp->out_sample_rate == 8000) {  
  320.                 /* 11025 -> 8000*/  
  321.                 int samples_in_left = rsmp->mInTmp2Buf;  
  322.                 int samples_out_left;  
  323.                 resample_441_320(rsmp->mTmp2Left, rsmp->mOutLeft, &samples_in_left, &samples_out_left);  
  324.   
  325.                 if (rsmp->channel_count == 2) {  
  326.                     int samples_in_right = rsmp->mInTmp2Buf;  
  327.                     int samples_out_right;  
  328.                     resample_441_320(rsmp->mTmp2Right, rsmp->mOutRight, &samples_in_right, &samples_out_right);  
  329.                 }  
  330.   
  331.                 rsmp->mInTmp2Buf = samples_in_left;  
  332.                 rsmp->mInOutBuf = samples_out_left;  
  333.             } else {  
  334.                 rsmp->mInTmp2Buf = 0;  
  335.             }  
  336.   
  337.         } else if (rsmp->out_sample_rate == 16000) {   
  338.             /* 22050 -> 16000*/  
  339.             int samples_in_left = rsmp->mInTmpBuf;  
  340.             int samples_out_left;  
  341.             resample_441_320(rsmp->mTmpLeft, rsmp->mTmp2Left, &samples_in_left, &samples_out_left);  
  342.   
  343.             if (rsmp->channel_count == 2) {  
  344.                 int samples_in_right = rsmp->mInTmpBuf;  
  345.                 int samples_out_right;  
  346.                 resample_441_320(rsmp->mTmpRight, rsmp->mTmp2Right, &samples_in_right, &samples_out_right);  
  347.             }  
  348.   
  349.             rsmp->mInTmpBuf = samples_in_left;  
  350.             rsmp->mInOutBuf = samples_out_left;  
  351.         } else {  
  352.             rsmp->mInTmpBuf = 0;  
  353.         }  
  354.   
  355.         int frames = (remaingFrames > rsmp->mInOutBuf) ? rsmp->mInOutBuf : remaingFrames;  
  356.   
  357.         for (int i = 0; i < frames; ++i) {  
  358.             out[outFrames + i] = outLeft[i];  
  359.         }  
  360.         if (rsmp->channel_count == 2) {  
  361.             for (int i = 0; i < frames; ++i) {  
  362.                 out[(outFrames + i) * 2] = outLeft[i];  
  363.                 out[(outFrames + i) * 2 + 1] = outRight[i];  
  364.             }  
  365.         }  
  366.         remaingFrames -= frames;  
  367.         outFrames += frames;  
  368.         rsmp->mOutBufPos = frames;  
  369.         rsmp->mInOutBuf -= frames;  
  370.     }  
  371.   
  372.     return 0;  
  373. }  
  374.   
  375. static int down_resampler_resample_from_input(struct resampler_itfe *resampler,  
  376.                                   int16_t *in,  
  377.                                   size_t *inFrameCount,  
  378.                                   int16_t *out,  
  379.                                   size_t *outFrameCount)  
  380. {  
  381.     struct down_resampler *rsmp =   
  382.         (struct down_resampler *)((char *)resampler - offsetof(struct down_resampler, itfe));  
  383.       
  384.     if (rsmp == NULL || in == NULL || inFrameCount == NULL ||  
  385.         out == NULL || outFrameCount == NULL) {  
  386.         return -EINVAL;  
  387.     }  
  388.     if (rsmp->provider != NULL) {  
  389.         *outFrameCount = 0;  
  390.         return -ENOSYS;  
  391.     }  
  392.       
  393.     // Not implement yet!!!   
  394.       
  395.     return 0;  
  396. }  
  397.   
  398.   
  399. /*------------------------------------------------------------------------------*/  
  400.   
  401.   
  402. /*------------------------------------------------------------------------------*/  
  403.   
  404. /* DownSampler Export Interfaces */  
  405.   
  406. int create_down_resampler(uint32_t outSampleRate,  
  407.                     uint32_t channelCount,  
  408.                     uint32_t frame_count,  
  409.                     struct resampler_buffer_provider* provider,  
  410.                     struct resampler_itfe **resampler)  
  411. {  
  412.     int error;  
  413.     struct down_resampler *rsmp;  
  414.       
  415.     if (resampler == NULL) {  
  416.         return -EINVAL;  
  417.     }  
  418.   
  419.     if (outSampleRate != 8000 && outSampleRate != 11025 && outSampleRate != 16000 &&  
  420.             outSampleRate != 22050) {  
  421.         return -EINVAL;  
  422.     }  
  423.       
  424.     *resampler = NULL;  
  425.       
  426.     rsmp = (struct down_resampler *)malloc(sizeof(struct down_resampler));  
  427.     if (rsmp == NULL) return -ENOMEM;  
  428.   
  429.     rsmp->itfe.reset = down_resampler_reset;  
  430.     rsmp->itfe.resample_from_provider = down_resampler_resample_from_provider;  
  431.     rsmp->itfe.resample_from_input = down_resampler_resample_from_input;  
  432.   
  433.     rsmp->provider = provider;  
  434.     rsmp->out_sample_rate = outSampleRate;  
  435.     rsmp->channel_count = channelCount;  
  436.     rsmp->frame_count = frame_count;  
  437.   
  438.     rsmp->mInLeft = malloc(sizeof(int16_t) * frame_count);  
  439.     rsmp->mInRight = malloc(sizeof(int16_t) * frame_count);  
  440.     rsmp->mTmpLeft = malloc(sizeof(int16_t) * frame_count);  
  441.     rsmp->mTmpRight = malloc(sizeof(int16_t) * frame_count);  
  442.     rsmp->mTmp2Left = malloc(sizeof(int16_t) * frame_count);  
  443.     rsmp->mTmp2Right = malloc(sizeof(int16_t) * frame_count);  
  444.     rsmp->mOutLeft = malloc(sizeof(int16_t) * frame_count);  
  445.     rsmp->mOutRight = malloc(sizeof(int16_t) * frame_count);  
  446.     if (rsmp->mInLeft == NULL || rsmp->mInRight == NULL ||  
  447.             rsmp->mTmpLeft == NULL || rsmp->mTmpRight == NULL ||  
  448.             rsmp->mTmp2Left == NULL || rsmp->mTmp2Right == NULL ||  
  449.             rsmp->mOutLeft == NULL || rsmp->mOutRight == NULL)  
  450.         return -ENOMEM;  
  451.   
  452.     down_resampler_reset(&rsmp->itfe);  
  453.       
  454.     *resampler = &rsmp->itfe;  
  455.       
  456.     return 0;  
  457. }  
  458.   
  459. void release_down_resampler(struct resampler_itfe *resampler)  
  460. {  
  461.     struct down_resampler *rsmp =   
  462.         (struct down_resampler *)((char *)resampler - offsetof(struct down_resampler, itfe));  
  463.       
  464.     if (rsmp == NULL) {  
  465.         return;  
  466.     }  
  467.       
  468.     if (rsmp->mInLeft) free(rsmp->mInLeft);  
  469.     if (rsmp->mInRight) free(rsmp->mInRight);  
  470.     if (rsmp->mTmpLeft) free(rsmp->mTmpLeft);  
  471.     if (rsmp->mTmpRight) free(rsmp->mTmpRight);  
  472.     if (rsmp->mTmp2Left) free(rsmp->mTmp2Left);  
  473.     if (rsmp->mTmp2Right) free(rsmp->mTmp2Right);  
  474.     if (rsmp->mOutLeft) free(rsmp->mOutLeft);  
  475.     if (rsmp->mOutRight) free(rsmp->mOutRight);  
  476.       
  477.     free(rsmp);  
  478. }  
// File:   resampler.c
// Author: Loon <[email protected]>

#include <stdio.h>
#include <unistd.h>
#include <sys/types.h>
#include <fcntl.h>
#include <errno.h>
#include "resampler.h"

struct down_resampler {
	struct resampler_itfe itfe;
	
	struct resampler_buffer_provider *provider; // buffer provider installed by client
	uint32_t out_sample_rate;                   // output sampling rate in Hz
	uint32_t channel_count;                     // number of channels (interleaved)
	uint32_t frame_count;                       // number of frames
	
	int16_t *mInLeft;
	int16_t *mInRight;
	int16_t *mTmpLeft;
	int16_t *mTmpRight;
	int16_t *mTmp2Left;
	int16_t *mTmp2Right;
	int16_t *mOutLeft;
	int16_t *mOutRight;
	int mInInBuf;
	int mInTmpBuf;
	int mInTmp2Buf;
	int mOutBufPos;
	int mInOutBuf;
};


/*------------------------------------------------------------------------------*/


/*------------------------------------------------------------------------------*/

/*  DownSampler Algorithm Implement */

/*
 * 2.30 fixed point FIR filter coefficients for conversion 44100 -> 22050.
 * (Works equivalently for 22010 -> 11025 or any other halving, of course.)
 *
 * Transition band from about 18 kHz, passband ripple < 0.1 dB,
 * stopband ripple at about -55 dB, linear phase.
 *
 * Design and display in MATLAB or Octave using:
 *
 * filter = fir1(19, 0.5); filter = round(filter * 2**30); freqz(filter * 2**-30);
 */
static const int32_t filter_22khz_coeff[] = {
    2089257, 2898328, -5820678, -10484531,
    19038724, 30542725, -50469415, -81505260,
    152544464, 478517512, 478517512, 152544464,
    -81505260, -50469415, 30542725, 19038724,
    -10484531, -5820678, 2898328, 2089257,
};
#define NUM_COEFF_22KHZ (sizeof(filter_22khz_coeff) / sizeof(filter_22khz_coeff[0]))
#define OVERLAP_22KHZ (NUM_COEFF_22KHZ - 2)

/*
 * Convolution of signals A and reverse(B). (In our case, the filter response
 * is symmetric, so the reversing doesn't matter.)
 * A is taken to be in 0.16 fixed-point, and B is taken to be in 2.30 fixed-point.
 * The answer will be in 16.16 fixed-point, unclipped.
 *
 * This function would probably be the prime candidate for SIMD conversion if
 * you want more speed.
 */
int32_t fir_convolve(const int16_t* a, const int32_t* b, int num_samples)
{
        int32_t sum = 1 << 13;
        for (int i = 0; i < num_samples; ++i) {
                sum += a[i] * (b[i] >> 16);
        }
        return sum >> 14;
}

/* Clip from 16.16 fixed-point to 0.16 fixed-point. */
int16_t clip(int32_t x)
{
    if (x < -32768) {
        return -32768;
    } else if (x > 32767) {
        return 32767;
    } else {
        return x;
    }
}

/*
 * Convert a chunk from 44 kHz to 22 kHz. Will update num_samples_in and num_samples_out
 * accordingly, since it may leave input samples in the buffer due to overlap.
 *
 * Input and output are taken to be in 0.16 fixed-point.
 */
void resample_2_1(int16_t* input, int16_t* output, int* num_samples_in, int* num_samples_out)
{
    if (*num_samples_in < (int)NUM_COEFF_22KHZ) {
        *num_samples_out = 0;
        return;
    }

    int odd_smp = *num_samples_in & 0x1;
    int num_samples = *num_samples_in - odd_smp - OVERLAP_22KHZ;

    for (int i = 0; i < num_samples; i += 2) {
            output[i / 2] = clip(fir_convolve(input + i, filter_22khz_coeff, NUM_COEFF_22KHZ));
    }

    memmove(input, input + num_samples, (OVERLAP_22KHZ + odd_smp) * sizeof(*input));
    *num_samples_out = num_samples / 2;
    *num_samples_in = OVERLAP_22KHZ + odd_smp;
}

/*
 * 2.30 fixed point FIR filter coefficients for conversion 22050 -> 16000,
 * or 11025 -> 8000.
 *
 * Transition band from about 14 kHz, passband ripple < 0.1 dB,
 * stopband ripple at about -50 dB, linear phase.
 *
 * Design and display in MATLAB or Octave using:
 *
 * filter = fir1(23, 16000 / 22050); filter = round(filter * 2**30); freqz(filter * 2**-30);
 */
static const int32_t filter_16khz_coeff[] = {
    2057290, -2973608, 1880478, 4362037,
    -14639744, 18523609, -1609189, -38502470,
    78073125, -68353935, -59103896, 617555440,
    617555440, -59103896, -68353935, 78073125,
    -38502470, -1609189, 18523609, -14639744,
    4362037, 1880478, -2973608, 2057290,
};
#define NUM_COEFF_16KHZ (sizeof(filter_16khz_coeff) / sizeof(filter_16khz_coeff[0]))
#define OVERLAP_16KHZ (NUM_COEFF_16KHZ - 1)

/*
 * Convert a chunk from 22 kHz to 16 kHz. Will update num_samples_in and
 * num_samples_out accordingly, since it may leave input samples in the buffer
 * due to overlap.
 *
 * This implementation is rather ad-hoc; it first low-pass filters the data
 * into a temporary buffer, and then converts chunks of 441 input samples at a
 * time into 320 output samples by simple linear interpolation. A better
 * implementation would use a polyphase filter bank to do these two operations
 * in one step.
 *
 * Input and output are taken to be in 0.16 fixed-point.
 */

#define RESAMPLE_16KHZ_SAMPLES_IN 441
#define RESAMPLE_16KHZ_SAMPLES_OUT 320

void resample_441_320(int16_t* input, int16_t* output, int* num_samples_in, int* num_samples_out)
{
    const int num_blocks = (*num_samples_in - OVERLAP_16KHZ) / RESAMPLE_16KHZ_SAMPLES_IN;
    if (num_blocks < 1) {
        *num_samples_out = 0;
        return;
    }

    for (int i = 0; i < num_blocks; ++i) {
        uint32_t tmp[RESAMPLE_16KHZ_SAMPLES_IN];
        for (int j = 0; j < RESAMPLE_16KHZ_SAMPLES_IN; ++j) {
            tmp[j] = fir_convolve(input + i * RESAMPLE_16KHZ_SAMPLES_IN + j,
                          filter_16khz_coeff,
                          NUM_COEFF_16KHZ);
        }

        const float step_float = (float)RESAMPLE_16KHZ_SAMPLES_IN / (float)RESAMPLE_16KHZ_SAMPLES_OUT;

        uint32_t in_sample_num = 0;   // 16.16 fixed point
        const uint32_t step = (uint32_t)(step_float * 65536.0f + 0.5f);  // 16.16 fixed point
        for (int j = 0; j < RESAMPLE_16KHZ_SAMPLES_OUT; ++j, in_sample_num += step) {
            const uint32_t whole = in_sample_num >> 16;
            const uint32_t frac = (in_sample_num & 0xffff);  // 0.16 fixed point
            const int32_t s1 = tmp[whole];
            const int32_t s2 = tmp[whole + 1];
            *output++ = clip(s1 + (((s2 - s1) * (int32_t)frac) >> 16));
        }
    }

    const int samples_consumed = num_blocks * RESAMPLE_16KHZ_SAMPLES_IN;
    memmove(input, input + samples_consumed, (*num_samples_in - samples_consumed) * sizeof(*input));
    *num_samples_in -= samples_consumed;
    *num_samples_out = RESAMPLE_16KHZ_SAMPLES_OUT * num_blocks;
}


/*------------------------------------------------------------------------------*/


/*------------------------------------------------------------------------------*/

/* DownSampler Callback Functions */

static void down_resampler_reset(struct resampler_itfe *resampler)
{
	struct down_resampler *rsmp = 
		(struct down_resampler *)((char *)resampler - offsetof(struct down_resampler, itfe));

	if (rsmp == NULL) {
		return;
	}

	rsmp->mInInBuf = 0;
	rsmp->mInTmpBuf = 0;
	rsmp->mInTmp2Buf = 0;
	rsmp->mOutBufPos = 0;
	rsmp->mInOutBuf = 0;
}

// outputs a number of frames less or equal to *outFrameCount and updates *outFrameCount
// with the actual number of frames produced.
static int down_resampler_resample_from_provider(struct resampler_itfe *resampler,
                       int16_t *out,
                       size_t *outFrameCount)
{
	struct down_resampler *rsmp = 
		(struct down_resampler *)((char *)resampler - offsetof(struct down_resampler, itfe));
	
	if (rsmp == NULL || out == NULL || outFrameCount == NULL) {
		return -EINVAL;
	}
	if (rsmp->provider == NULL) {
		*outFrameCount = 0;
		return -ENOSYS;
	}
	
	int16_t *outLeft = rsmp->mTmp2Left;
	int16_t *outRight = rsmp->mTmp2Left;
	if (rsmp->out_sample_rate == 22050) {
		outLeft = rsmp->mTmpLeft;
		outRight = rsmp->mTmpRight;
	} else if (rsmp->out_sample_rate == 8000){
		outLeft = rsmp->mOutLeft;
		outRight = rsmp->mOutRight;
	}

	int outFrames = 0;
	int remaingFrames = *outFrameCount;

	if (rsmp->mInOutBuf) {
		int frames = (remaingFrames > rsmp->mInOutBuf) ? rsmp->mInOutBuf : remaingFrames;

		for (int i = 0; i < frames; ++i) {
			out[i] = outLeft[rsmp->mOutBufPos + i];
		}
		if (rsmp->channel_count == 2) {
			for (int i = 0; i < frames; ++i) {
				out[i * 2] = outLeft[rsmp->mOutBufPos + i];
				out[i * 2 + 1] = outRight[rsmp->mOutBufPos + i];
			}
		}
		remaingFrames -= frames;
		rsmp->mInOutBuf -= frames;
		rsmp->mOutBufPos += frames;
		outFrames += frames;
	}

	while (remaingFrames) {
		struct resampler_buffer buf;
		buf.frame_count =  rsmp->frame_count - rsmp->mInInBuf;
		int ret = rsmp->provider->get_next_buffer(rsmp->provider, &buf);
		if (buf.raw == NULL) {
			*outFrameCount = outFrames;
			return ret;
		}

		for (size_t i = 0; i < buf.frame_count; ++i) {
			rsmp->mInLeft[i + rsmp->mInInBuf] = buf.i16[i];
		}
		if (rsmp->channel_count == 2) {
			for (size_t i = 0; i < buf.frame_count; ++i) {
				rsmp->mInLeft[i + rsmp->mInInBuf] = buf.i16[i * 2];
				rsmp->mInRight[i + rsmp->mInInBuf] = buf.i16[i * 2 + 1];
			}
		}
		rsmp->mInInBuf += buf.frame_count;
		rsmp->provider->release_buffer(rsmp->provider, &buf);

		/* 44010 -> 22050 */
		{
			int samples_in_left = rsmp->mInInBuf;
			int samples_out_left;
			resample_2_1(rsmp->mInLeft, rsmp->mTmpLeft + rsmp->mInTmpBuf, &samples_in_left, &samples_out_left);

			if (rsmp->channel_count == 2) {
				int samples_in_right = rsmp->mInInBuf;
				int samples_out_right;
				resample_2_1(rsmp->mInRight, rsmp->mTmpRight + rsmp->mInTmpBuf, &samples_in_right, &samples_out_right);
			}

			rsmp->mInInBuf = samples_in_left;
			rsmp->mInTmpBuf += samples_out_left;
			rsmp->mInOutBuf = samples_out_left;
		}

		if (rsmp->out_sample_rate == 11025 || rsmp->out_sample_rate == 8000) {
			/* 22050 - > 11025 */
			int samples_in_left = rsmp->mInTmpBuf;
			int samples_out_left;
			resample_2_1(rsmp->mTmpLeft, rsmp->mTmp2Left + rsmp->mInTmp2Buf, &samples_in_left, &samples_out_left);

			if (rsmp->channel_count == 2) {
				int samples_in_right = rsmp->mInTmpBuf;
				int samples_out_right;
				resample_2_1(rsmp->mTmpRight, rsmp->mTmp2Right + rsmp->mInTmp2Buf, &samples_in_right, &samples_out_right);
			}


			rsmp->mInTmpBuf = samples_in_left;
			rsmp->mInTmp2Buf += samples_out_left;
			rsmp->mInOutBuf = samples_out_left;

			if (rsmp->out_sample_rate == 8000) {
				/* 11025 -> 8000*/
				int samples_in_left = rsmp->mInTmp2Buf;
				int samples_out_left;
				resample_441_320(rsmp->mTmp2Left, rsmp->mOutLeft, &samples_in_left, &samples_out_left);

				if (rsmp->channel_count == 2) {
					int samples_in_right = rsmp->mInTmp2Buf;
					int samples_out_right;
					resample_441_320(rsmp->mTmp2Right, rsmp->mOutRight, &samples_in_right, &samples_out_right);
				}

				rsmp->mInTmp2Buf = samples_in_left;
				rsmp->mInOutBuf = samples_out_left;
			} else {
				rsmp->mInTmp2Buf = 0;
			}

		} else if (rsmp->out_sample_rate == 16000) { 
			/* 22050 -> 16000*/
			int samples_in_left = rsmp->mInTmpBuf;
			int samples_out_left;
			resample_441_320(rsmp->mTmpLeft, rsmp->mTmp2Left, &samples_in_left, &samples_out_left);

			if (rsmp->channel_count == 2) {
				int samples_in_right = rsmp->mInTmpBuf;
				int samples_out_right;
				resample_441_320(rsmp->mTmpRight, rsmp->mTmp2Right, &samples_in_right, &samples_out_right);
			}

			rsmp->mInTmpBuf = samples_in_left;
			rsmp->mInOutBuf = samples_out_left;
		} else {
			rsmp->mInTmpBuf = 0;
		}

		int frames = (remaingFrames > rsmp->mInOutBuf) ? rsmp->mInOutBuf : remaingFrames;

		for (int i = 0; i < frames; ++i) {
			out[outFrames + i] = outLeft[i];
		}
		if (rsmp->channel_count == 2) {
			for (int i = 0; i < frames; ++i) {
				out[(outFrames + i) * 2] = outLeft[i];
				out[(outFrames + i) * 2 + 1] = outRight[i];
			}
		}
		remaingFrames -= frames;
		outFrames += frames;
		rsmp->mOutBufPos = frames;
		rsmp->mInOutBuf -= frames;
	}

	return 0;
}

static int down_resampler_resample_from_input(struct resampler_itfe *resampler,
                                  int16_t *in,
                                  size_t *inFrameCount,
                                  int16_t *out,
                                  size_t *outFrameCount)
{
	struct down_resampler *rsmp = 
		(struct down_resampler *)((char *)resampler - offsetof(struct down_resampler, itfe));
	
	if (rsmp == NULL || in == NULL || inFrameCount == NULL ||
		out == NULL || outFrameCount == NULL) {
		return -EINVAL;
	}
	if (rsmp->provider != NULL) {
		*outFrameCount = 0;
		return -ENOSYS;
	}
	
	// Not implement yet!!!
	
	return 0;
}


/*------------------------------------------------------------------------------*/


/*------------------------------------------------------------------------------*/

/* DownSampler Export Interfaces */

int create_down_resampler(uint32_t outSampleRate,
                    uint32_t channelCount,
                    uint32_t frame_count,
                    struct resampler_buffer_provider* provider,
                    struct resampler_itfe **resampler)
{
	int error;
	struct down_resampler *rsmp;
	
	if (resampler == NULL) {
		return -EINVAL;
	}

	if (outSampleRate != 8000 && outSampleRate != 11025 && outSampleRate != 16000 &&
			outSampleRate != 22050) {
		return -EINVAL;
	}
	
	*resampler = NULL;
	
	rsmp = (struct down_resampler *)malloc(sizeof(struct down_resampler));
	if (rsmp == NULL) return -ENOMEM;

	rsmp->itfe.reset = down_resampler_reset;
	rsmp->itfe.resample_from_provider = down_resampler_resample_from_provider;
	rsmp->itfe.resample_from_input = down_resampler_resample_from_input;

	rsmp->provider = provider;
	rsmp->out_sample_rate = outSampleRate;
	rsmp->channel_count = channelCount;
	rsmp->frame_count = frame_count;

	rsmp->mInLeft = malloc(sizeof(int16_t) * frame_count);
	rsmp->mInRight = malloc(sizeof(int16_t) * frame_count);
	rsmp->mTmpLeft = malloc(sizeof(int16_t) * frame_count);
	rsmp->mTmpRight = malloc(sizeof(int16_t) * frame_count);
	rsmp->mTmp2Left = malloc(sizeof(int16_t) * frame_count);
	rsmp->mTmp2Right = malloc(sizeof(int16_t) * frame_count);
	rsmp->mOutLeft = malloc(sizeof(int16_t) * frame_count);
	rsmp->mOutRight = malloc(sizeof(int16_t) * frame_count);
	if (rsmp->mInLeft == NULL || rsmp->mInRight == NULL ||
			rsmp->mTmpLeft == NULL || rsmp->mTmpRight == NULL ||
			rsmp->mTmp2Left == NULL || rsmp->mTmp2Right == NULL ||
			rsmp->mOutLeft == NULL || rsmp->mOutRight == NULL)
		return -ENOMEM;

	down_resampler_reset(&rsmp->itfe);
	
	*resampler = &rsmp->itfe;
	
	return 0;
}

void release_down_resampler(struct resampler_itfe *resampler)
{
	struct down_resampler *rsmp = 
		(struct down_resampler *)((char *)resampler - offsetof(struct down_resampler, itfe));
	
	if (rsmp == NULL) {
		return;
	}
	
	if (rsmp->mInLeft) free(rsmp->mInLeft);
    if (rsmp->mInRight) free(rsmp->mInRight);
    if (rsmp->mTmpLeft) free(rsmp->mTmpLeft);
    if (rsmp->mTmpRight) free(rsmp->mTmpRight);
    if (rsmp->mTmp2Left) free(rsmp->mTmp2Left);
    if (rsmp->mTmp2Right) free(rsmp->mTmp2Right);
    if (rsmp->mOutLeft) free(rsmp->mOutLeft);
    if (rsmp->mOutRight) free(rsmp->mOutRight);
	
	free(rsmp);
}

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章