File content as of revision 16:4ab2eac7be21:

/**
******************************************************************************
* @file    OpenPDMFilter.h
* @author  CL
* @version V1.0.0
* @date    9-September-2015
* @brief   Open PDM audio software decoding Library.   
*          This Library is used to decode and reconstruct the audio signal
*          produced by ST MEMS microphone (MP45Dxxx, MP34Dxxx). 
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
*
* Licensed under MCD-ST Image SW License Agreement V2, (the "License");
* You may not use this file except in compliance with the License.
* You may obtain a copy of the License at:
*
*        http://www.st.com/software_license_agreement_image_v2
*
* Unless required by applicable law or agreed to in writing, software 
* distributed under the License is distributed on an "AS IS" BASIS, 
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
******************************************************************************
*/


/* Includes ------------------------------------------------------------------*/

#include "OpenPDMFilter.h"


/* Definitions ---------------------------------------------------------------*/

#define RoundDiv(a, b)    (((a)>0)?(((a)+(b)/2)/(b)):(((a)-(b)/2)/(b)))
#define SaturaLH(N, L, H) (((N)<(L))?(L):(((N)>(H))?(H):(N)))

#define VOLUME_MAX     64
#define FILTER_GAIN    16

#define SINCN_MAX       3
#define DECIMATION_MAX 64


/* Variables -----------------------------------------------------------------*/

uint32_t div_const = 0;
int64_t sub_const = 0;
uint32_t sinc[DECIMATION_MAX * SINCN_MAX];
uint32_t sinc1[DECIMATION_MAX];
uint32_t sinc2[DECIMATION_MAX * 2];
uint32_t coef[SINCN_MAX][DECIMATION_MAX];
#ifdef USE_LUT
int32_t lut[256][DECIMATION_MAX / 8][SINCN_MAX];
#endif


/* Functions -----------------------------------------------------------------*/

int32_t filter_table(uint8_t *data, uint8_t sincn, uint8_t channels);

void convolve(uint32_t Signal[/* SignalLen */], unsigned short SignalLen,
              uint32_t Kernel[/* KernelLen */], unsigned short KernelLen,
              uint32_t Result[/* SignalLen + KernelLen - 1 */]);

#ifdef USE_LUT
int32_t filter_table_mono(uint8_t *data, uint8_t sincn)
{
  return (int32_t)
    lut[data[0]][0][sincn] +
    lut[data[1]][1][sincn] +
    lut[data[2]][2][sincn] +
    lut[data[3]][3][sincn] +
    lut[data[4]][4][sincn] +
    lut[data[5]][5][sincn] +
    lut[data[6]][6][sincn] +
    lut[data[7]][7][sincn];
}
int32_t filter_table_stereo(uint8_t *data, uint8_t sincn)
{
  return (int32_t)
    lut[data[0]][0][sincn] +
    lut[data[2]][1][sincn] +
    lut[data[4]][2][sincn] +
    lut[data[6]][3][sincn] +
    lut[data[8]][4][sincn] +
    lut[data[10]][5][sincn] +
    lut[data[12]][6][sincn] +
    lut[data[14]][7][sincn];
}
int32_t (* filter_tables[2]) (uint8_t *data, uint8_t sincn) = {filter_table_mono, filter_table_stereo};
#else
int64_t filter_table(uint8_t *data, uint8_t sincn, uint8_t channels)
{
  uint8_t c, i;
  uint16_t data_index = 0;
  uint32_t *coef_p = &coef[table][0];
  int64_t F = 0;

  for (i = 0; i < DECIMATION_MAX; i += 8)
  {
    c = data[data_index];
    F += ((c >> 7)       ) * coef_p[i    ] +
         ((c >> 6) & 0x01) * coef_p[i + 1] +
         ((c >> 5) & 0x01) * coef_p[i + 2] +
         ((c >> 4) & 0x01) * coef_p[i + 3] +
         ((c >> 3) & 0x01) * coef_p[i + 4] +
         ((c >> 2) & 0x01) * coef_p[i + 5] +
         ((c >> 1) & 0x01) * coef_p[i + 6] +
         ((c     ) & 0x01) * coef_p[i + 7];
    data_index += channels;
  }
  return F;
}
#endif

void Open_PDM_Filter_Init(TPDMFilter_InitStruct *Param)
{
  uint16_t i, j;
  int64_t sum = 0;
  for (i = 0; i < SINCN_MAX; i++)
  {
    Param->Coef[i] = 0;
    Param->bit[i] = 0;
  }
  for (i = 0; i < DECIMATION_MAX; i++)
  {
    sinc1[i] = 1;
  }

  Param->OldOut = Param->OldIn = Param->OldZ = 0;
  Param->LP_ALFA = (Param->LP_HZ != 0 ? (uint16_t) (Param->LP_HZ * 256 / (Param->LP_HZ + Param->Fs / (2 * 3.14))) : 0);
  Param->HP_ALFA = (Param->HP_HZ != 0 ? (uint16_t) (Param->Fs * 256 / (2 * 3.14 * Param->HP_HZ + Param->Fs)) : 0);

  Param->FilterLen = DECIMATION_MAX * 3;       
  sinc[0] = 0;
  sinc[DECIMATION_MAX * 3 - 1] = 0;      
  convolve(sinc1, DECIMATION_MAX, sinc1, DECIMATION_MAX, sinc2);
  convolve(sinc2, DECIMATION_MAX * 2 - 1, sinc1, DECIMATION_MAX, &sinc[1]);     
  for(j = 0; j < 3; j++)
  {
    for (i = 0; i < DECIMATION_MAX ;i++)
    {
      coef[j][i] = sinc[j * DECIMATION_MAX + i];
      sum += sinc[j * DECIMATION_MAX + i];
    }
  }

  sub_const = sum >> 1;
  div_const = sub_const * VOLUME_MAX / 32768 / FILTER_GAIN;
  div_const = (div_const == 0 ? 1 : div_const);

#ifdef USE_LUT
  /* Look-Up Table. */
  uint16_t c, d, s;
  for (s = 0; s < SINCN_MAX; s++)
  {
    uint32_t *coef_p = &coef[s][0];
    for (c = 0; c < 256; c++)
      for (d = 0; d < DECIMATION_MAX / 8; d++)
        lut[c][d][s] = ((c >> 7)       ) * coef_p[d * 8    ] +
                       ((c >> 6) & 0x01) * coef_p[d * 8 + 1] +
                       ((c >> 5) & 0x01) * coef_p[d * 8 + 2] +
                       ((c >> 4) & 0x01) * coef_p[d * 8 + 3] +
                       ((c >> 3) & 0x01) * coef_p[d * 8 + 4] +
                       ((c >> 2) & 0x01) * coef_p[d * 8 + 5] +
                       ((c >> 1) & 0x01) * coef_p[d * 8 + 6] +
                       ((c     ) & 0x01) * coef_p[d * 8 + 7];
  }
#endif
}

void convolve(uint32_t Signal[/* SignalLen */], unsigned short SignalLen,
              uint32_t Kernel[/* KernelLen */], unsigned short KernelLen,
              uint32_t Result[/* SignalLen + KernelLen - 1 */])
{
  uint16_t n;
  for (n = 0; n < SignalLen + KernelLen - 1; n++)
  {
    unsigned short kmin, kmax, k;
    
    Result[n] = 0;
    
    kmin = (n >= KernelLen - 1) ? n - (KernelLen - 1) : 0;
    kmax = (n < SignalLen - 1) ? n : SignalLen - 1;
    
    for (k = kmin; k <= kmax; k++)
    {
      Result[n] += Signal[k] * Kernel[n - k];
    }
  }
}

void Open_PDM_Filter(uint8_t* data, uint16_t* dataOut, uint16_t volume, TPDMFilter_InitStruct *Param)
{
  uint8_t i, data_out_index;
  uint8_t decimation = DECIMATION_MAX;
  uint8_t channels = Param->In_MicChannels;
  uint8_t data_inc = ((decimation >> 3) * channels);
  int64_t Z, Z0, Z1, Z2;
  int64_t OldOut, OldIn, OldZ;       

  OldOut = Param->OldOut;
  OldIn = Param->OldIn;
  OldZ = Param->OldZ;

  for (i = 0, data_out_index = 0; i < Param->Fs / 1000; i++, data_out_index += channels) 
  {
#ifdef USE_LUT
    Z0 = filter_tables[channels - 1](data, 0);
    Z1 = filter_tables[channels - 1](data, 1);
    Z2 = filter_tables[channels - 1](data, 2);
#else
    Z0 = filter_table(data, 0, channels);
    Z1 = filter_table(data, 1, channels);
    Z2 = filter_table(data, 2, channels);
#endif

    Z = Param->Coef[1] + Z2 - sub_const;
    Param->Coef[1] = Param->Coef[0] + Z1;
    Param->Coef[0] = Z0;

    OldOut = (Param->HP_ALFA * (OldOut + Z - OldIn)) >> 8;
    OldIn = Z;
    Z = OldOut;
    OldZ = ((256 - Param->LP_ALFA) * OldZ + Param->LP_ALFA * Z) >> 8;
    Z = OldZ;

    Z *= volume;
    Z = RoundDiv(Z, div_const);
    Z = SaturaLH(Z, -32700, 32700);

    dataOut[data_out_index] = Z;
    data += data_inc;
  }

  Param->OldOut = OldOut;
  Param->OldIn = OldIn;
  Param->OldZ = OldZ;
}