async_input_spdif3.cpp

/* Audio Library for Teensy 3.X
 * Copyright (c) 2019, Paul Stoffregen, paul@pjrc.com
 *
 * Development of this audio library was funded by PJRC.COM, LLC by sales of
 * Teensy and Audio Adaptor boards.  Please support PJRC's efforts to develop
 * open source software by purchasing Teensy or other PJRC products.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice, development funding notice, and this permission
 * notice shall be included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */
/*
 by Alexander Walch
 */
#if defined(__IMXRT1062__)

#include "async_input_spdif3.h"
#include "output_spdif3.h"

#include "biquad.h"
#include <utility/imxrt_hw.h>
//Parameters
namespace {
	#define SPDIF_RX_BUFFER_LENGTH AUDIO_BLOCK_SAMPLES
	const int32_t bufferLength=8*AUDIO_BLOCK_SAMPLES;
	const uint16_t noSamplerPerIsr=SPDIF_RX_BUFFER_LENGTH/4;
	const float toFloatAudio= (float)(1./pow(2., 23.));
	uint32_t updateLast=-1;
	uint32_t cyclesBeforeLast=0;
	volatile uint32_t isrDiffCycles=0;
	uint32_t cyclesLast=0;
}

#ifdef DEBUG_SPDIF_IN
volatile bool AsyncAudioInputSPDIF3::bufferOverflow=false;
#endif

DMAMEM __attribute__((aligned(32)))
static int32_t spdif_rx_buffer[SPDIF_RX_BUFFER_LENGTH];
static float bufferR[bufferLength];
static float bufferL[bufferLength];

volatile int32_t AsyncAudioInputSPDIF3::buffer_offset = 0;	// read by resample/ written in spdif input isr -> copied at the beginning of 'resmaple' protected by __disable_irq() in resample
int32_t AsyncAudioInputSPDIF3::resample_offset = 0; // read/written by resample/ read in spdif input isr -> no protection needed?

DMAChannel AsyncAudioInputSPDIF3::dma(false);

AsyncAudioInputSPDIF3::~AsyncAudioInputSPDIF3(){
	delete [] _bufferLPFilter.pCoeffs;
	delete [] _bufferLPFilter.pState;
	delete quantizer[0];
	delete quantizer[1];
}

FLASHMEM
AsyncAudioInputSPDIF3::AsyncAudioInputSPDIF3(bool dither, bool noiseshaping,float attenuation, int32_t minHalfFilterLength, int32_t maxHalfFilterLength):
	AudioStream(0, NULL),
	_resampler(attenuation, minHalfFilterLength, maxHalfFilterLength)
	{
	const float factor = powf(2, 15)-1.f; // to 16 bit audio
	quantizer[0]=new Quantizer(AUDIO_SAMPLE_RATE_EXACT);
	quantizer[0]->configure(noiseshaping, dither, factor);
	quantizer[1]=new Quantizer(AUDIO_SAMPLE_RATE_EXACT);
	quantizer[1]->configure(noiseshaping, dither, factor);
	begin();
	}
FLASHMEM
void AsyncAudioInputSPDIF3::begin()
{
	
	AudioOutputSPDIF3::config_spdif3();
		
	dma.begin(true); // Allocate the DMA channel first
	const uint32_t noByteMinorLoop=2*4;
	dma.TCD->SOFF = 4;
	dma.TCD->ATTR = DMA_TCD_ATTR_SSIZE(2) | DMA_TCD_ATTR_DSIZE(2);
	dma.TCD->NBYTES_MLNO = DMA_TCD_NBYTES_MLOFFYES_NBYTES(noByteMinorLoop) | DMA_TCD_NBYTES_SMLOE | 
						DMA_TCD_NBYTES_MLOFFYES_MLOFF(-8);
	dma.TCD->SLAST = -8;
	dma.TCD->DOFF = 4;
	dma.TCD->CITER_ELINKNO = sizeof(spdif_rx_buffer) / noByteMinorLoop;
	dma.TCD->DLASTSGA = -sizeof(spdif_rx_buffer);
	dma.TCD->BITER_ELINKNO = sizeof(spdif_rx_buffer) / noByteMinorLoop;
	dma.TCD->CSR = DMA_TCD_CSR_INTHALF | DMA_TCD_CSR_INTMAJOR;	
	dma.TCD->SADDR = (void *)((uint32_t)&SPDIF_SRL);
	dma.TCD->DADDR = spdif_rx_buffer;
	dma.triggerAtHardwareEvent(DMAMUX_SOURCE_SPDIF_RX);

	//SPDIF_SCR |=SPDIF_SCR_DMA_RX_EN;		//DMA Receive Request Enable
	dma.enable();
	dma.attachInterrupt(isr);
#ifdef DEBUG_SPDIF_IN
	while (!Serial);
#endif
	_bufferLPFilter.pCoeffs=new float[5];
	_bufferLPFilter.numStages=1;
	_bufferLPFilter.pState=new float[2];
	getCoefficients(_bufferLPFilter.pCoeffs, BiquadType::LOW_PASS, 0., 5., AUDIO_SAMPLE_RATE_EXACT/AUDIO_BLOCK_SAMPLES, 0.5);
	SPDIF_SCR &=(~SPDIF_SCR_RXFIFO_OFF_ON);	//receive fifo is turned on again
	
	SPDIF_SRCD = 0;
	SPDIF_SCR |= SPDIF_SCR_DMA_RX_EN;
	CORE_PIN15_CONFIG = 3;
	IOMUXC_SPDIF_IN_SELECT_INPUT = 0; // GPIO_AD_B1_03_ALT3
}
bool AsyncAudioInputSPDIF3::isLocked() {
	return (SPDIF_SRPC & SPDIF_SRPC_LOCK) == SPDIF_SRPC_LOCK;
}

void AsyncAudioInputSPDIF3::resample(int16_t* data_left, int16_t* data_right, int32_t& block_offset){
	block_offset=0;
	if(!_resampler.initialized() || !isLocked()){
		return;
	}
	int32_t bOffset=buffer_offset;
	int32_t resOffset=resample_offset;
		
	int32_t inputBufferStop = bOffset >= resOffset ? bOffset-resOffset : bufferLength-resOffset;
	if (inputBufferStop==0){
		return;
	}
	int32_t processedLength;
	int32_t outputCount=0;

	float resampledBufferL[AUDIO_BLOCK_SAMPLES];
	float resampledBufferR[AUDIO_BLOCK_SAMPLES];
	_resampler.resample(&bufferL[resOffset],&bufferR[resOffset], inputBufferStop, processedLength, resampledBufferL, resampledBufferR, AUDIO_BLOCK_SAMPLES, outputCount);
		
	resOffset=(resOffset+processedLength)%bufferLength;
	block_offset=outputCount;	

	if (bOffset > resOffset && block_offset< AUDIO_BLOCK_SAMPLES){
		inputBufferStop= bOffset-resOffset;
		int32_t outputLength=AUDIO_BLOCK_SAMPLES-block_offset;
		_resampler.resample(&bufferL[resOffset],&bufferR[resOffset], inputBufferStop, processedLength, resampledBufferL+block_offset, resampledBufferR+block_offset, outputLength, outputCount);
		resOffset=(resOffset+processedLength)%bufferLength;
		block_offset+=outputCount;
	}
	quantizer[0]->quantize(resampledBufferL, data_left, block_offset);
	quantizer[1]->quantize(resampledBufferR, data_right, block_offset);
	__disable_irq();
	resample_offset=resOffset;
	__enable_irq();		
}

void AsyncAudioInputSPDIF3::isr(void)
{
	dma.clearInterrupt();
	
	cyclesLast=ARM_DWT_CYCCNT;
	isrDiffCycles=cyclesLast - cyclesBeforeLast;
	cyclesBeforeLast=cyclesLast;

	const int32_t *src, *end;
	uint32_t daddr = (uint32_t)(dma.TCD->DADDR);

	if (daddr < (uint32_t)spdif_rx_buffer + sizeof(spdif_rx_buffer) / 2) {
		// DMA is receiving to the first half of the buffer
		// need to remove data from the second half
		src = (int32_t *)&spdif_rx_buffer[SPDIF_RX_BUFFER_LENGTH/2];
		end = (int32_t *)&spdif_rx_buffer[SPDIF_RX_BUFFER_LENGTH];
		//if (AsyncAudioInputSPDIF3::update_responsibility) AudioStream::update_all();
	} else {
		// DMA is receiving to the second half of the buffer
		// need to remove data from the first half
		src = (int32_t *)&spdif_rx_buffer[0];
		end = (int32_t *)&spdif_rx_buffer[SPDIF_RX_BUFFER_LENGTH/2];
	}
	if (buffer_offset >=resample_offset ||
		(buffer_offset + SPDIF_RX_BUFFER_LENGTH/4) < resample_offset) {
		#if IMXRT_CACHE_ENABLED >=1
		arm_dcache_delete((void*)src, sizeof(spdif_rx_buffer) / 2);
		#endif
		float *destR = &(bufferR[buffer_offset]);
		float *destL = &(bufferL[buffer_offset]);
		do {			
			int32_t n=(*src) & 0x800000 ? (*src)|0xFF800000  : (*src) & 0xFFFFFF;
			*destL++ = (float)(n)*toFloatAudio;
			++src;

			n=(*src) & 0x800000 ? (*src)|0xFF800000  : (*src) & 0xFFFFFF;
			*destR++ = (float)(n)*toFloatAudio;
			++src;
		} while (src < end);
		buffer_offset=(buffer_offset+SPDIF_RX_BUFFER_LENGTH/4)%bufferLength;
	}
#ifdef DEBUG_SPDIF_IN
	else {
		bufferOverflow=true;
	}
#endif
}
double AsyncAudioInputSPDIF3::getNewValidInputFrequ(){
	//page 2129: FrequMeas[23:0]=FreqMeas_CLK / BUS_CLK * 2^10 * GAIN
	if (isLocked()){
		const double f=(double)F_BUS_ACTUAL/(1048576.*(double)AudioOutputSPDIF3::dpll_Gain()*128.);// bit clock = 128 * sampling frequency
		const double freqMeas=(double)(SPDIF_SRFM & 0xFFFFFF)*f;
		if (_lastValidInputFrequ != freqMeas){//frequency not stable yet;
			_lastValidInputFrequ=freqMeas;
			return -1.;	
		}
		return _lastValidInputFrequ;
	}
	return -1.;
}

double AsyncAudioInputSPDIF3::getBufferedTime() const{
	__disable_irq();
	double n=_bufferedTime;
	__enable_irq();
	return n;
}

bool AsyncAudioInputSPDIF3::configure(){
	bool newConfiguration=false;
	if(!isLocked()){
		_resampler.reset();
		return newConfiguration;
	}
		
#ifdef DEBUG_SPDIF_IN
	const bool bOverf=bufferOverflow;
	bufferOverflow=false;
	if (bOverf){
		Serial.print("buffer overflow, buffer offset: ");
		Serial.print(buffer_offset);
		Serial.print(", resample_offset: ");
		Serial.println(resample_offset);
		if (!_resampler.initialized()){
			Serial.println("_resampler not initialized. ");
		}
	}
#endif
	const double inputF=getNewValidInputFrequ();	//returns: -1 ... invalid frequency
	if (inputF > 0.){
		//we got a valid sample frequency
		const double frequDiff=inputF/_inputFrequency-1.;
		if (abs(frequDiff) > 0.01 || !_resampler.initialized()){
			//the new sample frequency differs from the last one -> configure the _resampler again
			_inputFrequency=inputF;		
			_targetLatencyS=max(0.001,(noSamplerPerIsr*3./2./_inputFrequency));
			_maxLatency=max(2.*_blockDuration, 2*noSamplerPerIsr/_inputFrequency);
			__disable_irq();
			resample_offset = buffer_offset;	//we want 'monitorResampleBuffer' to set the correct resample_offset!!!
			__enable_irq();
			_resampler.configure(inputF, AUDIO_SAMPLE_RATE_EXACT);
			newConfiguration=true;
	#ifdef DEBUG_SPDIF_IN
			Serial.print("_maxLatency: ");
			Serial.println(_maxLatency);
			Serial.print("_targetLatencyS: ");
			Serial.println(_targetLatencyS);
			Serial.print("relative frequ diff: ");
			Serial.println(frequDiff, 8);
			Serial.print("configure _resampler with frequency ");
			Serial.println(inputF,8);			
	#endif			
		}
	}
	return newConfiguration;
}

void AsyncAudioInputSPDIF3::monitorResampleBuffer(bool newConfiguration){
	__disable_irq();
	uint32_t updateCurrent=ARM_DWT_CYCCNT;
	double dmaOffset=(double)(updateCurrent-cyclesLast)/(double)F_CPU_ACTUAL;	//[seconds]
	uint32_t isrDC=isrDiffCycles;
	int32_t bOffset = buffer_offset;
	__enable_irq();

	if(!_resampler.initialized()){
		return;
	}
	double inputFrequency= AUDIO_SAMPLE_RATE*_resampler.getStep();

	//here we try to compensate for timing variations in the calls of 'update' and 'isr'
	double updateDiff=0.;
	if (updateLast != (uint32_t)-1){
		double timeSinceLast = (double)(updateCurrent-updateLast)/(double)F_CPU_ACTUAL;	//[seconds]
		updateDiff = _blockDuration-timeSinceLast;
		if (updateDiff > 0.){
			//if updateDiff > 0 then 'update' was called too fast. This only happens, if the preceding call was too late
			updateDiff=0.;
		}
		//Until now we 'updateDiff' captures timing variations of 'update'. But if 'update' and 'isr' were both delayed, we apply a wrong correction.
		//Therefore we try to estimate now if the 'isr' call was also delayed:
		double isrDiff= noSamplerPerIsr / inputFrequency - (double)isrDC/(double)F_CPU_ACTUAL;	// noSamplerPerIsr / inputFrequency is the exptect time between isr calls
		if (isrDiff < 0.){
			//the isr call was also delayed -> dmaOffset is already quite correct and 'updateDiff' needs to be corrected
			updateDiff-=isrDiff;
		}
	}
	updateLast=updateCurrent;
	double resamplerPos = _resampler.getXPos();
	double bTime = resample_offset <= bOffset ?
		(bOffset-(resample_offset+resamplerPos))/inputFrequency :
		(bufferLength-(resample_offset+resamplerPos) + bOffset )/inputFrequency;	//[seconds]
	double targetOffset = _blockDuration + _targetLatencyS - updateDiff - dmaOffset;
	double diff = bTime-targetOffset;	//seconds
	
	biquad_cascade_df2T<double, arm_biquad_cascade_df2T_instance_f32, float>(&_bufferLPFilter, &diff, &diff, 1);
	bool settled=false;
	if (!newConfiguration){
		// we just configured the resampler and can there not trust the computed 'diff'
		settled=_resampler.updateIncrement(diff);
	#ifdef DEBUG_SPDIF_IN
		if(Serial && !_resampler.initialized()){
			Serial.println("resampler not initialized anymore.");
		}
	#endif
		//we updated the step width -> lets update the buffered time, so that we can see if there are really enough samples in the buffer
		bTime*=inputFrequency;
		inputFrequency = AUDIO_SAMPLE_RATE * _resampler.getStep();	//we need to update the inputfrequency since we called 'addToSampleDiff'
		bTime/=inputFrequency;
		//================================================================================================================================
	}
	
	if (bTime > _maxLatency  || bTime<= _blockDuration|| settled || newConfiguration) {
		if (targetOffset < _blockDuration + resamplerPos/inputFrequency ||
			targetOffset > 2.*_blockDuration + _targetLatencyS){
			if (dmaOffset < 0. || dmaOffset > noSamplerPerIsr / inputFrequency){
				dmaOffset =0.5*noSamplerPerIsr / inputFrequency; //we don't know when the last dma transfer happened. The best we can do is to estimate it.
			}
			targetOffset = _blockDuration + _targetLatencyS-  dmaOffset;	
		}
		double targetOffsetSamples = targetOffset*inputFrequency;
		const int32_t targetOffsetI=ceil(targetOffsetSamples);
		__disable_irq();
		resample_offset =  targetOffsetI <= bOffset ? bOffset - targetOffsetI : bufferLength -(targetOffsetI-bOffset);
		__enable_irq();
		//correct for the integer resample offset
		_resampler.setPos((double)targetOffsetI-targetOffsetSamples);

		preload(&_bufferLPFilter, 0.);
		_resampler.fixIncrement();
		
		// #ifdef DEBUG_SPDIF_IN
		// 		if(Serial){

		// 			double bTime = resample_offset <= bOffset ?
		// 				(bOffset-(resample_offset+_resampler.getXPos()))/inputFrequency :
		// 				(bufferLength-(resample_offset+_resampler.getXPos()) + bOffset)/inputFrequency;	//[seconds]
		// 			double targetOffset = _blockDuration + _targetLatencyS - updateDiff - dmaOffset;
		// 			double diff = bTime-targetOffset;	//seconds

		// 			Serial.print("resample_offset: ");
		// 			Serial.println(resample_offset);
		// 			Serial.print("buffer_offset: ");
		// 			Serial.println(bOffset);
		// 			Serial.print("targetOffsetI: ");
		// 			Serial.println(targetOffsetI,4);
		// 			Serial.print("settled: ");
		// 			Serial.println(settled);
		// 			Serial.print("dmaOffset: ");
		// 			Serial.println(dmaOffset*1e6,3);
		// 			Serial.print("targetOffset: ");
		// 			Serial.println(targetOffset*1e6,3);
		// 			Serial.print("diff: ");
		// 			Serial.println(diff*1e6,4);
		// 			Serial.print("bTime-dmaOffset: ");
		// 			Serial.println((bTime-dmaOffset)*1e6,4);
		// 		}
		// #endif		
		diff = 0.;
	}
	_bufferedTime=_targetLatencyS+diff;
}

void AsyncAudioInputSPDIF3::update(void)
{
	bool newConfiguration = configure();
	monitorResampleBuffer(newConfiguration);	//important first call 'monitorResampleBuffer' then 'resample'
	audio_block_t *block_left =allocate();
	audio_block_t *block_right =nullptr;
	if (block_left!= nullptr) {
		block_right = allocate();
		if (block_right == nullptr) {
			release(block_left);
			block_left = nullptr;
		}
	}
	if (block_left && block_right) {
		int32_t block_offset;
		resample(block_left->data, block_right->data,block_offset);
		if(block_offset < AUDIO_BLOCK_SAMPLES){
			memset(block_left->data+block_offset, 0, (AUDIO_BLOCK_SAMPLES-block_offset)*sizeof(int16_t)); 
			memset(block_right->data+block_offset, 0, (AUDIO_BLOCK_SAMPLES-block_offset)*sizeof(int16_t)); 
#ifdef DEBUG_SPDIF_IN	
			Serial.print("filled only ");
			Serial.print(block_offset);
			Serial.println(" samples.");
#endif
		}
		transmit(block_left, 0);
		release(block_left);
		block_left=nullptr;
		transmit(block_right, 1);
		release(block_right);
		block_right=nullptr;	
	}
#ifdef DEBUG_SPDIF_IN
	else {		
		Serial.println("Not enough blocks available. Too few audio memory?");
	}
#endif
}
double AsyncAudioInputSPDIF3::getInputFrequency() const{
	__disable_irq();
	double f=_lastValidInputFrequ;
	__enable_irq();
	return isLocked() ? f : 0.;
}
double AsyncAudioInputSPDIF3::getTargetLantency() const {
	__disable_irq();
	double l=_targetLatencyS;
	__enable_irq();
	return l ;
}
double AsyncAudioInputSPDIF3::getAttenuation() const{
	return _resampler.getAttenuation();
}
int32_t AsyncAudioInputSPDIF3::getHalfFilterLength() const{
	return _resampler.getHalfFilterLength();
}

#endif // __IMXRT1062__


#if defined(__MK66FX1M0__) || defined(__MK64FX512__) || defined(__MK20DX256__) || defined(__MKL26Z64__)
// empty code to allow compile (but no sound input) on other Teensy models

#include "async_input_spdif3.h"
AsyncAudioInputSPDIF3::AsyncAudioInputSPDIF3(bool dither, bool noiseshaping,float attenuation, int32_t minHalfFilterLength, int32_t maxHalfFilterLength):
	AudioStream(0, NULL), _resampler(attenuation, minHalfFilterLength, maxHalfFilterLength)
	{ }
void AsyncAudioInputSPDIF3::begin() { }
void AsyncAudioInputSPDIF3::update(void) { }
double AsyncAudioInputSPDIF3::getBufferedTime() const { return 0; }
double AsyncAudioInputSPDIF3::getInputFrequency() const { return 0; }
bool AsyncAudioInputSPDIF3::isLocked() { return false; }
double AsyncAudioInputSPDIF3::getTargetLantency() const { return 0; }
double AsyncAudioInputSPDIF3::getAttenuation() const { return 0; }
int32_t AsyncAudioInputSPDIF3::getHalfFilterLength() const { return 0; }
AsyncAudioInputSPDIF3::~AsyncAudioInputSPDIF3() { }


#endif