Decimator.hpp

// Copyright 2022 XMOS LIMITED.
// This Software is subject to the terms of the XMOS Public Licence: Version 1.
 
#pragma once
 
#include <cstdint>
#include <string>
#include <cassert>
 
#include "xs3_math.h"
#include "mic_array/etc/fir_1x16_bit.h"
 
// This has caused problems previously, so just catch the problems here.
#if defined(MIC_COUNT) || defined(S2_DEC_FACTOR) || defined(S2_TAP_COUNT)
# error Application must not define the following as precompiler macros: MIC_COUNT, S2_DEC_FACTOR, S2_TAP_COUNT.
#endif
 
 
namespace  mic_array {
 
static inline 
void shift_buffer(uint32_t* buff);
 
 
template <unsigned MIC_COUNT, unsigned S2_DEC_FACTOR, unsigned S2_TAP_COUNT>
class TwoStageDecimator 
{
 
  public:
    static constexpr unsigned BLOCK_SIZE = MIC_COUNT * S2_DEC_FACTOR;
 
    static constexpr unsigned MicCount = MIC_COUNT;
 
    static const struct {
      unsigned DecimationFactor = S2_DEC_FACTOR;
      unsigned TapCount = S2_TAP_COUNT;
    } Stage2;
 
  private:
 
    struct {
      const uint32_t* filter_coef;
      uint32_t pdm_history[MIC_COUNT][8]
#ifndef __DOXYGEN__ // doxygen breaks if it encounters this.
        = {[0 ... (MIC_COUNT-1)] = { [0 ... 7] = 0x55555555 } }
#endif
      ;
    } stage1;
    
    struct {
      xs3_filter_fir_s32_t filters[MIC_COUNT];
      int32_t filter_state[MIC_COUNT][S2_TAP_COUNT] = {{0}};
    } stage2;
 
  public:
 
    constexpr TwoStageDecimator() noexcept { }
 
    void Init(
        const uint32_t* s1_filter_coef,
        const int32_t* s2_filter_coef,
        const right_shift_t s2_filter_shr);
 
    void ProcessBlock(
        int32_t sample_out[MIC_COUNT],
        uint32_t pdm_block[BLOCK_SIZE]);
  };
}
 
// Template function implementations below. //
 
 
template <unsigned MIC_COUNT, unsigned S2_DEC_FACTOR, unsigned S2_TAP_COUNT>
void mic_array::TwoStageDecimator<MIC_COUNT,S2_DEC_FACTOR,S2_TAP_COUNT>::Init(
    const uint32_t* s1_filter_coef,
    const int32_t* s2_filter_coef,
    const right_shift_t s2_shr) 
{      
  this->stage1.filter_coef = s1_filter_coef;
 
  for(int k = 0; k < MIC_COUNT; k++){
    xs3_filter_fir_s32_init(&this->stage2.filters[k], &this->stage2.filter_state[k][0],
                            S2_TAP_COUNT, s2_filter_coef, s2_shr);
  }
}
 
 
 
template <unsigned MIC_COUNT, unsigned S2_DEC_FACTOR, unsigned S2_TAP_COUNT>
void mic_array::TwoStageDecimator<MIC_COUNT,S2_DEC_FACTOR,S2_TAP_COUNT>
    ::ProcessBlock(
        int32_t sample_out[MIC_COUNT],
        uint32_t pdm_block[BLOCK_SIZE])
{
  uint32_t (*pdm_data)[S2_DEC_FACTOR] = (uint32_t (*)[S2_DEC_FACTOR]) pdm_block;
 
  for(unsigned mic = 0; mic < MIC_COUNT; mic++){
    uint32_t* hist = &this->stage1.pdm_history[mic][0];
 
    for(unsigned k = 0; k < S2_DEC_FACTOR; k++){
      hist[0] = pdm_data[mic][k];
      int32_t streamA_sample = fir_1x16_bit(hist, this->stage1.filter_coef);
      shift_buffer(hist);
 
      if(k < (S2_DEC_FACTOR-1)){
        xs3_filter_fir_s32_add_sample(&this->stage2.filters[mic], streamA_sample);
      } else {
        sample_out[mic] = xs3_filter_fir_s32(&this->stage2.filters[mic], streamA_sample);
      }
    }
  }
}
 
 
static inline 
void mic_array::shift_buffer(uint32_t* buff)
{
  uint32_t* src = &buff[-1];
  asm volatile("vldd %0[0]; vstd %1[0];" :: "r"(src), "r"(buff) : "memory" );
}