PdmRx.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 <xcore/interrupt.h>
#include <xcore/channel_streaming.h>
#include <xcore/port.h>
 
#include "mic_array.h"
#include "Util.hpp"
 
// This has caused problems previously, so just catch the problems here.
#if defined(BLOCK_SIZE) || defined(CHANNELS_IN) || defined(CHANNELS_OUT) || defined(SUBBLOCKS)
# error Application must not define the following as precompiler macros: MIC_COUNT, CHANNELS_IN, CHANNELS_OUT, SUBBLOCKS.
#endif
 
using namespace std;
 
extern "C" {
  extern struct {
    port_t p_pdm_mics;
    uint32_t* pdm_buffer[2];
    unsigned phase;
    unsigned phase_reset;
    chanend_t c_pdm_data;
  } pdm_rx_isr_context;
 
  static inline 
  void enable_pdm_rx_isr(
      const port_t p_pdm_mics)
  {
    asm volatile(
      "setc res[%0], %1       \n"
      "ldap r11, pdm_rx_isr   \n"
      "setv res[%0], r11      \n"
      "eeu res[%0]              "
        :
        : "r"(p_pdm_mics), "r"(XS1_SETC_IE_MODE_INTERRUPT)
        : "r11" );
  }
 
}
 
 
namespace  mic_array {
 
 
 
  template <unsigned BLOCK_SIZE, class SubType>
  class PdmRxService
  {
    // @todo: Use a static assertion to check that SubType is in fact a sub-type
    //        of `PdmRxService`. 
 
    public:
 
      static constexpr unsigned BlockSize = BLOCK_SIZE;
 
    protected:
      port_t p_pdm_mics;  
 
      unsigned phase = BLOCK_SIZE;
 
      uint32_t block_data[2][BLOCK_SIZE];
 
      uint32_t* blocks[2] = {&block_data[0][0], &block_data[1][0]};
 
    public:
 
      void SetPort(port_t p_pdm_mics);
 
      void ProcessNext();
 
      void ThreadEntry();
      
  };
 
 
  template <unsigned CHANNELS_IN, unsigned CHANNELS_OUT, unsigned SUBBLOCKS>
  class StandardPdmRxService : public PdmRxService<CHANNELS_IN * SUBBLOCKS, 
                                      StandardPdmRxService<CHANNELS_IN, CHANNELS_OUT, SUBBLOCKS>>
  {
    using Super = PdmRxService<CHANNELS_IN * SUBBLOCKS, 
                    StandardPdmRxService<CHANNELS_IN, CHANNELS_OUT, SUBBLOCKS>>;
 
    private:
      streaming_channel_t c_pdm_blocks;
 
      unsigned channel_map[CHANNELS_OUT];
 
      uint32_t out_block[CHANNELS_OUT][SUBBLOCKS];
   
    public:
 
      uint32_t ReadPort();
 
      void SendBlock(uint32_t block[CHANNELS_IN * SUBBLOCKS]);
 
      void Init(port_t p_pdm_mics);
 
      void MapChannels(unsigned map[CHANNELS_OUT]);
 
      void MapChannel(unsigned out_channel, unsigned in_channel);
 
      void InstallISR();
 
      void UnmaskISR();
 
      uint32_t* GetPdmBlock();
  };
 
}
 
// Template function implementations below. //
 
 
//              PdmRxService                //
 
template <unsigned BLOCK_SIZE, class SubType>
void mic_array::PdmRxService<BLOCK_SIZE,SubType>::SetPort(port_t p_pdm_mics)
{
  this->p_pdm_mics = p_pdm_mics;
}
 
 
template <unsigned BLOCK_SIZE, class SubType>
void mic_array::PdmRxService<BLOCK_SIZE,SubType>::ProcessNext()
{
  this->blocks[0][--phase] =  static_cast<SubType*>(this)->ReadPort();
 
  if(!phase){
    this->phase = BLOCK_SIZE;
    uint32_t* ready_block = this->blocks[0];
    this->blocks[0] = this->blocks[1];
    this->blocks[1] = ready_block;
 
    static_cast<SubType*>(this)->SendBlock(ready_block);
  }
}
 
 
template <unsigned BLOCK_SIZE, class SubType>
void mic_array::PdmRxService<BLOCK_SIZE,SubType>::ThreadEntry()
{
  while(1){
    this->ProcessNext();
  }
}
 
 
 
//          StandardPdmRxService            //
 
 
template <unsigned CHANNELS_IN, unsigned CHANNELS_OUT, unsigned SUBBLOCKS>
uint32_t mic_array::StandardPdmRxService<CHANNELS_IN, CHANNELS_OUT, SUBBLOCKS>
    ::ReadPort()
{
  return port_in(this->p_pdm_mics);
}
 
 
template <unsigned CHANNELS_IN, unsigned CHANNELS_OUT, unsigned SUBBLOCKS>
void mic_array::StandardPdmRxService<CHANNELS_IN, CHANNELS_OUT, SUBBLOCKS>
    ::SendBlock(uint32_t block[CHANNELS_IN*SUBBLOCKS])
{
  s_chan_out_word(this->c_pdm_blocks.end_a, 
                  reinterpret_cast<uint32_t>( &block[0] ));
}
 
 
template <unsigned CHANNELS_IN, unsigned CHANNELS_OUT, unsigned SUBBLOCKS>
void mic_array::StandardPdmRxService<CHANNELS_IN, CHANNELS_OUT, SUBBLOCKS>
    ::Init(port_t p_pdm_mics) 
{
  for(int k = 0; k < CHANNELS_OUT; k++)
    this->channel_map[k] = k;
 
  this->c_pdm_blocks = s_chan_alloc();
 
  this->SetPort(p_pdm_mics);
}
 
 
template <unsigned CHANNELS_IN, unsigned CHANNELS_OUT, unsigned SUBBLOCKS>
void mic_array::StandardPdmRxService<CHANNELS_IN, CHANNELS_OUT, SUBBLOCKS>
    ::MapChannels(unsigned map[CHANNELS_OUT]) 
{
  for(int k = 0; k < CHANNELS_OUT; k++)
    this->channel_map[k] = map[k];
}
 
 
template <unsigned CHANNELS_IN, unsigned CHANNELS_OUT, unsigned SUBBLOCKS>
void mic_array::StandardPdmRxService<CHANNELS_IN, CHANNELS_OUT, SUBBLOCKS>
    ::MapChannel(unsigned out_channel, unsigned in_channel) 
{
  this->channel_map[out_channel] = in_channel;
}
 
 
template <unsigned CHANNELS_IN, unsigned CHANNELS_OUT, unsigned SUBBLOCKS>
void mic_array::StandardPdmRxService<CHANNELS_IN, CHANNELS_OUT, SUBBLOCKS>
    ::InstallISR() 
{
  pdm_rx_isr_context.p_pdm_mics = this->p_pdm_mics;
  pdm_rx_isr_context.c_pdm_data = this->c_pdm_blocks.end_a;
  pdm_rx_isr_context.pdm_buffer[0] = &this->block_data[0][0];
  pdm_rx_isr_context.pdm_buffer[1] = &this->block_data[1][0];
  pdm_rx_isr_context.phase_reset = CHANNELS_IN*SUBBLOCKS-1;
  pdm_rx_isr_context.phase = CHANNELS_IN*SUBBLOCKS-1;
 
  enable_pdm_rx_isr(this->p_pdm_mics);
}
 
 
template <unsigned CHANNELS_IN, unsigned CHANNELS_OUT, unsigned SUBBLOCKS>
void mic_array::StandardPdmRxService<CHANNELS_IN, CHANNELS_OUT, SUBBLOCKS>
    ::UnmaskISR() 
{
  interrupt_unmask_all();
}
 
 
template <unsigned CHANNELS_IN, unsigned CHANNELS_OUT, unsigned SUBBLOCKS>
uint32_t* mic_array::StandardPdmRxService<CHANNELS_IN, CHANNELS_OUT, SUBBLOCKS>
    ::GetPdmBlock() 
{
  uint32_t* full_block = (uint32_t*) s_chan_in_word(this->c_pdm_blocks.end_b);
  mic_array::deinterleave_pdm_samples<CHANNELS_IN>(full_block, SUBBLOCKS);
 
  uint32_t (*block)[CHANNELS_IN] = (uint32_t (*)[CHANNELS_IN]) full_block;
 
  for(int ch = 0; ch < CHANNELS_OUT; ch++) {
    for(int sb = 0; sb < SUBBLOCKS; sb++) {
      unsigned d = this->channel_map[ch];
      this->out_block[ch][sb] = block[SUBBLOCKS-1-sb][d];
    } 
  }
  return &this->out_block[0][0];
}