You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

476 lines
17KB

  1. /* Audio Library for Teensy 3.X
  2. * Copyright (c) 2019, Paul Stoffregen, paul@pjrc.com
  3. *
  4. * Development of this audio library was funded by PJRC.COM, LLC by sales of
  5. * Teensy and Audio Adaptor boards. Please support PJRC's efforts to develop
  6. * open source software by purchasing Teensy or other PJRC products.
  7. *
  8. * Permission is hereby granted, free of charge, to any person obtaining a copy
  9. * of this software and associated documentation files (the "Software"), to deal
  10. * in the Software without restriction, including without limitation the rights
  11. * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  12. * copies of the Software, and to permit persons to whom the Software is
  13. * furnished to do so, subject to the following conditions:
  14. *
  15. * The above copyright notice, development funding notice, and this permission
  16. * notice shall be included in all copies or substantial portions of the Software.
  17. *
  18. * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  19. * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  20. * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  21. * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  22. * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  23. * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  24. * THE SOFTWARE.
  25. */
  26. /*
  27. by Alexander Walch
  28. */
  29. #if defined(__IMXRT1052__) || defined(__IMXRT1062__)
  30. #include "async_input_spdif3.h"
  31. #include "biquad.h"
  32. #include <utility/imxrt_hw.h>
  33. //Parameters
  34. namespace {
  35. #define SPDIF_RX_BUFFER_LENGTH AUDIO_BLOCK_SAMPLES
  36. const int32_t bufferLength=8*AUDIO_BLOCK_SAMPLES;
  37. const uint16_t noSamplerPerIsr=SPDIF_RX_BUFFER_LENGTH/4;
  38. const float toFloatAudio= 1.f/pow(2., 23.);
  39. }
  40. volatile bool AsyncAudioInputSPDIF3::resetResampler=true;
  41. #ifdef DEBUG_SPDIF_IN
  42. volatile bool AsyncAudioInputSPDIF3::bufferOverflow=false;
  43. #endif
  44. volatile uint32_t AsyncAudioInputSPDIF3::microsLast;
  45. DMAMEM __attribute__((aligned(32)))
  46. static int32_t spdif_rx_buffer[SPDIF_RX_BUFFER_LENGTH];
  47. static float bufferR[bufferLength];
  48. static float bufferL[bufferLength];
  49. 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
  50. int32_t AsyncAudioInputSPDIF3::resample_offset = 0; // read/written by resample/ read in spdif input isr -> no protection needed?
  51. volatile bool AsyncAudioInputSPDIF3::lockChanged=false;
  52. volatile bool AsyncAudioInputSPDIF3::locked=false;
  53. DMAChannel AsyncAudioInputSPDIF3::dma(false);
  54. AsyncAudioInputSPDIF3::~AsyncAudioInputSPDIF3(){
  55. delete [] _bufferLPFilter.pCoeffs;
  56. delete [] _bufferLPFilter.pState;
  57. delete quantizer[0];
  58. delete quantizer[1];
  59. }
  60. PROGMEM
  61. AsyncAudioInputSPDIF3::AsyncAudioInputSPDIF3(bool dither, bool noiseshaping,float attenuation, int32_t minHalfFilterLength) : AudioStream(0, NULL) {
  62. _attenuation=attenuation;
  63. _minHalfFilterLength=minHalfFilterLength;
  64. const float factor = powf(2, 15)-1.f; // to 16 bit audio
  65. quantizer[0]=new Quantizer(AUDIO_SAMPLE_RATE_EXACT);
  66. quantizer[0]->configure(noiseshaping, dither, factor);
  67. quantizer[1]=new Quantizer(AUDIO_SAMPLE_RATE_EXACT);
  68. quantizer[1]->configure(noiseshaping, dither, factor);
  69. begin();
  70. }
  71. PROGMEM
  72. void AsyncAudioInputSPDIF3::begin()
  73. {
  74. dma.begin(true); // Allocate the DMA channel first
  75. const uint32_t noByteMinorLoop=2*4;
  76. dma.TCD->SOFF = 4;
  77. dma.TCD->ATTR = DMA_TCD_ATTR_SSIZE(2) | DMA_TCD_ATTR_DSIZE(2);
  78. dma.TCD->NBYTES_MLNO = DMA_TCD_NBYTES_MLOFFYES_NBYTES(noByteMinorLoop) | DMA_TCD_NBYTES_SMLOE |
  79. DMA_TCD_NBYTES_MLOFFYES_MLOFF(-8);
  80. dma.TCD->SLAST = -8;
  81. dma.TCD->DOFF = 4;
  82. dma.TCD->CITER_ELINKNO = sizeof(spdif_rx_buffer) / noByteMinorLoop;
  83. dma.TCD->DLASTSGA = -sizeof(spdif_rx_buffer);
  84. dma.TCD->BITER_ELINKNO = sizeof(spdif_rx_buffer) / noByteMinorLoop;
  85. dma.TCD->CSR = DMA_TCD_CSR_INTHALF | DMA_TCD_CSR_INTMAJOR;
  86. dma.TCD->SADDR = (void *)((uint32_t)&SPDIF_SRL);
  87. dma.TCD->DADDR = spdif_rx_buffer;
  88. dma.triggerAtHardwareEvent(DMAMUX_SOURCE_SPDIF_RX);
  89. SPDIF_SCR |=SPDIF_SCR_DMA_RX_EN; //DMA Receive Request Enable
  90. dma.enable();
  91. dma.attachInterrupt(isr);
  92. config_spdifIn();
  93. #ifdef DEBUG_SPDIF_IN
  94. while (!Serial);
  95. #endif
  96. _bufferLPFilter.pCoeffs=new float[5];
  97. _bufferLPFilter.numStages=1;
  98. _bufferLPFilter.pState=new float[2];
  99. getCoefficients(_bufferLPFilter.pCoeffs, BiquadType::LOW_PASS, 0., 5., AUDIO_SAMPLE_RATE_EXACT/AUDIO_BLOCK_SAMPLES, 0.5);
  100. }
  101. bool AsyncAudioInputSPDIF3::isLocked() const {
  102. __disable_irq();
  103. bool l=locked;
  104. __enable_irq();
  105. return l;
  106. }
  107. void AsyncAudioInputSPDIF3::spdif_interrupt(){
  108. if(SPDIF_SIS & SPDIF_SIS_LOCK){
  109. if (!locked){
  110. locked=true;
  111. lockChanged=true;
  112. }
  113. }
  114. else if(SPDIF_SIS & SPDIF_SIS_LOCKLOSS){
  115. if (locked){
  116. locked=false;
  117. lockChanged=true;
  118. resetResampler=true;
  119. }
  120. }
  121. SPDIF_SIC |= SPDIF_SIC_LOCKLOSS;//clear SPDIF_SIC_LOCKLOSS interrupt
  122. SPDIF_SIC |= SPDIF_SIC_LOCK; //clear SPDIF_SIC_LOCK interrupt
  123. }
  124. void AsyncAudioInputSPDIF3::resample(int16_t* data_left, int16_t* data_right, int32_t& block_offset){
  125. block_offset=0;
  126. if(!_resampler.initialized()){
  127. return;
  128. }
  129. __disable_irq();
  130. if(!locked){
  131. __enable_irq();
  132. return;
  133. }
  134. int32_t bOffset=buffer_offset;
  135. int32_t resOffset=resample_offset;
  136. __enable_irq();
  137. uint16_t inputBufferStop = bOffset >= resOffset ? bOffset-resOffset : bufferLength-resOffset;
  138. if (inputBufferStop==0){
  139. return;
  140. }
  141. uint16_t processedLength;
  142. uint16_t outputCount=0;
  143. uint16_t outputLength=AUDIO_BLOCK_SAMPLES;
  144. float resampledBufferL[AUDIO_BLOCK_SAMPLES];
  145. float resampledBufferR[AUDIO_BLOCK_SAMPLES];
  146. _resampler.resample(&bufferL[resOffset],&bufferR[resOffset], inputBufferStop, processedLength, resampledBufferL, resampledBufferR, outputLength, outputCount);
  147. resOffset=(resOffset+processedLength)%bufferLength;
  148. block_offset=outputCount;
  149. if (bOffset > resOffset && block_offset< AUDIO_BLOCK_SAMPLES){
  150. inputBufferStop= bOffset-resOffset;
  151. outputLength=AUDIO_BLOCK_SAMPLES-block_offset;
  152. _resampler.resample(&bufferL[resOffset],&bufferR[resOffset], inputBufferStop, processedLength, resampledBufferL+block_offset, resampledBufferR+block_offset, outputLength, outputCount);
  153. resOffset=(resOffset+processedLength)%bufferLength;
  154. block_offset+=outputCount;
  155. }
  156. quantizer[0]->quantize(resampledBufferL, data_left, block_offset);
  157. quantizer[1]->quantize(resampledBufferR, data_right, block_offset);
  158. __disable_irq();
  159. resample_offset=resOffset;
  160. __enable_irq();
  161. }
  162. void AsyncAudioInputSPDIF3::isr(void)
  163. {
  164. dma.clearInterrupt();
  165. microsLast=micros();
  166. const int32_t *src, *end;
  167. uint32_t daddr = (uint32_t)(dma.TCD->DADDR);
  168. if (daddr < (uint32_t)spdif_rx_buffer + sizeof(spdif_rx_buffer) / 2) {
  169. // DMA is receiving to the first half of the buffer
  170. // need to remove data from the second half
  171. src = (int32_t *)&spdif_rx_buffer[SPDIF_RX_BUFFER_LENGTH/2];
  172. end = (int32_t *)&spdif_rx_buffer[SPDIF_RX_BUFFER_LENGTH];
  173. //if (AsyncAudioInputSPDIF3::update_responsibility) AudioStream::update_all();
  174. } else {
  175. // DMA is receiving to the second half of the buffer
  176. // need to remove data from the first half
  177. src = (int32_t *)&spdif_rx_buffer[0];
  178. end = (int32_t *)&spdif_rx_buffer[SPDIF_RX_BUFFER_LENGTH/2];
  179. }
  180. if (buffer_offset >=resample_offset ||
  181. (buffer_offset + SPDIF_RX_BUFFER_LENGTH/4) < resample_offset) {
  182. #if IMXRT_CACHE_ENABLED >=1
  183. arm_dcache_delete((void*)src, sizeof(spdif_rx_buffer) / 2);
  184. #endif
  185. float *destR = &(bufferR[buffer_offset]);
  186. float *destL = &(bufferL[buffer_offset]);
  187. do {
  188. int32_t n=(*src) & 0x800000 ? (*src)|0xFF800000 : (*src) & 0xFFFFFF;
  189. *destL++ = (float)(n)*toFloatAudio;
  190. ++src;
  191. n=(*src) & 0x800000 ? (*src)|0xFF800000 : (*src) & 0xFFFFFF;
  192. *destR++ = (float)(n)*toFloatAudio;
  193. ++src;
  194. } while (src < end);
  195. buffer_offset=(buffer_offset+SPDIF_RX_BUFFER_LENGTH/4)%bufferLength;
  196. }
  197. #ifdef DEBUG_SPDIF_IN
  198. else {
  199. bufferOverflow=true;
  200. }
  201. #endif
  202. }
  203. double AsyncAudioInputSPDIF3::getNewValidInputFrequ(){
  204. //page 2129: FrequMeas[23:0]=FreqMeas_CLK / BUS_CLK * 2^10 * GAIN
  205. if (SPDIF_SRPC & SPDIF_SRPC_LOCK){
  206. const double f=(float)F_BUS_ACTUAL/(1024.*1024.*24.*128.);// bit clock = 128 * sampling frequency
  207. const double freqMeas=(SPDIF_SRFM & 0xFFFFFF)*f;
  208. if (_lastValidInputFrequ != freqMeas){//frequency not stable yet;
  209. _lastValidInputFrequ=freqMeas;
  210. return -1.;
  211. }
  212. return _lastValidInputFrequ;
  213. }
  214. return -1.;
  215. }
  216. double AsyncAudioInputSPDIF3::getBufferedTime() const{
  217. __disable_irq();
  218. double n=_bufferedTime;
  219. __enable_irq();
  220. return n;
  221. }
  222. void AsyncAudioInputSPDIF3::configure(){
  223. __disable_irq();
  224. if(resetResampler){
  225. _resampler.reset();
  226. resetResampler=false;
  227. }
  228. if(!locked){
  229. __enable_irq();
  230. #ifdef DEBUG_SPDIF_IN
  231. Serial.println("lock lost");
  232. #endif
  233. return;
  234. }
  235. #ifdef DEBUG_SPDIF_IN
  236. const bool bOverf=bufferOverflow;
  237. bufferOverflow=false;
  238. #endif
  239. const bool lc=lockChanged;
  240. __enable_irq();
  241. #ifdef DEBUG_SPDIF_IN
  242. if (bOverf){
  243. Serial.print("buffer overflow, buffer offset: ");
  244. Serial.print(buffer_offset);
  245. Serial.print(", resample_offset: ");
  246. Serial.println(resample_offset);
  247. if (!_resampler.initialized()){
  248. Serial.println("_resampler not initialized. ");
  249. }
  250. }
  251. #endif
  252. if (lc || !_resampler.initialized()){
  253. const double inputF=getNewValidInputFrequ(); //returns: -1 ... invalid frequency
  254. if (inputF > 0.){
  255. __disable_irq();
  256. lockChanged=false; //only reset lockChanged if a valid frequency was received (inputFrequ > 0.)
  257. __enable_irq();
  258. //we got a valid sample frequency
  259. const double frequDiff=inputF/_inputFrequency-1.;
  260. if (abs(frequDiff) > 0.01 || !_resampler.initialized()){
  261. //the new sample frequency differs from the last one -> configure the _resampler again
  262. _inputFrequency=inputF;
  263. _targetLatencyS=max(0.001,(noSamplerPerIsr*3./2./_inputFrequency));
  264. _maxLatency=max(2.*_blockDuration, 2*noSamplerPerIsr/_inputFrequency);
  265. const int32_t targetLatency=round(_targetLatencyS*inputF);
  266. __disable_irq();
  267. resample_offset = targetLatency <= buffer_offset ? buffer_offset - targetLatency : bufferLength -(targetLatency-buffer_offset);
  268. __enable_irq();
  269. _resampler.configure(inputF, AUDIO_SAMPLE_RATE_EXACT, _attenuation, _minHalfFilterLength);
  270. #ifdef DEBUG_SPDIF_IN
  271. Serial.print("_maxLatency: ");
  272. Serial.println(_maxLatency);
  273. Serial.print("targetLatency: ");
  274. Serial.println(targetLatency);
  275. Serial.print("relative frequ diff: ");
  276. Serial.println(frequDiff, 8);
  277. Serial.print("configure _resampler with frequency ");
  278. Serial.println(inputF,8);
  279. #endif
  280. }
  281. }
  282. }
  283. }
  284. void AsyncAudioInputSPDIF3::monitorResampleBuffer(){
  285. if(!_resampler.initialized()){
  286. return;
  287. }
  288. __disable_irq();
  289. const double dmaOffset=(micros()-microsLast)*1e-6; //[seconds]
  290. double bTime = resample_offset <= buffer_offset ? (buffer_offset-resample_offset-_resampler.getXPos())/_lastValidInputFrequ+dmaOffset : (bufferLength-resample_offset +buffer_offset-_resampler.getXPos())/_lastValidInputFrequ+dmaOffset; //[seconds]
  291. double diff = bTime- (_blockDuration+ _targetLatencyS); //seconds
  292. biquad_cascade_df2T<double, arm_biquad_cascade_df2T_instance_f32, float>(&_bufferLPFilter, &diff, &diff, 1);
  293. bool settled=_resampler.addToSampleDiff(diff);
  294. if (bTime > _maxLatency || bTime-dmaOffset<= _blockDuration || settled) {
  295. double distance=(_blockDuration+_targetLatencyS-dmaOffset)*_lastValidInputFrequ+_resampler.getXPos();
  296. diff=0.;
  297. if (distance > bufferLength-noSamplerPerIsr){
  298. diff=bufferLength-noSamplerPerIsr-distance;
  299. distance=bufferLength-noSamplerPerIsr;
  300. }
  301. if (distance < 0.){
  302. distance=0.;
  303. diff=- (_blockDuration+ _targetLatencyS);
  304. }
  305. double resample_offsetF=buffer_offset-distance;
  306. resample_offset=(int32_t)floor(resample_offsetF);
  307. _resampler.addToPos(resample_offsetF-resample_offset);
  308. while (resample_offset<0){
  309. resample_offset+=bufferLength;
  310. }
  311. //int32_t b_offset=buffer_offset;
  312. #ifdef DEBUG_SPDIF_IN
  313. double bTimeFixed = resample_offset <= buffer_offset ? (buffer_offset-resample_offset-_resampler.getXPos())/_lastValidInputFrequ+dmaOffset : (bufferLength-resample_offset +buffer_offset-_resampler.getXPos())/_lastValidInputFrequ+dmaOffset; //[seconds]
  314. #endif
  315. __enable_irq();
  316. #ifdef DEBUG_SPDIF_IN
  317. // Serial.print("settled: ");
  318. // Serial.println(settled);
  319. Serial.print("bTime: ");
  320. Serial.print(bTime*1e6,3);
  321. Serial.print("_maxLatency: ");
  322. Serial.println(_maxLatency*1e6,3);
  323. Serial.print("bTime-dmaOffset: ");
  324. Serial.print((bTime-dmaOffset)*1e6,3);
  325. Serial.print(", _blockDuration: ");
  326. Serial.print(_blockDuration*1e6,3);
  327. Serial.print("bTimeFixed: ");
  328. Serial.print(bTimeFixed*1e6,3);
  329. #endif
  330. preload(&_bufferLPFilter, diff);
  331. _resampler.fixStep();
  332. }
  333. else {
  334. __enable_irq();
  335. }
  336. _bufferedTime=_targetLatencyS+diff;
  337. }
  338. void AsyncAudioInputSPDIF3::update(void)
  339. {
  340. configure();
  341. monitorResampleBuffer(); //important first call 'monitorResampleBuffer' then 'resample'
  342. audio_block_t *block_left =allocate();
  343. audio_block_t *block_right =nullptr;
  344. if (block_left!= nullptr) {
  345. block_right = allocate();
  346. if (block_right == nullptr) {
  347. release(block_left);
  348. block_left = nullptr;
  349. }
  350. }
  351. if (block_left && block_right) {
  352. int32_t block_offset;
  353. resample(block_left->data, block_right->data,block_offset);
  354. if(block_offset < AUDIO_BLOCK_SAMPLES){
  355. memset(block_left->data+block_offset, 0, (AUDIO_BLOCK_SAMPLES-block_offset)*sizeof(int16_t));
  356. memset(block_right->data+block_offset, 0, (AUDIO_BLOCK_SAMPLES-block_offset)*sizeof(int16_t));
  357. #ifdef DEBUG_SPDIF_IN
  358. Serial.print("filled only ");
  359. Serial.print(block_offset);
  360. Serial.println(" samples.");
  361. #endif
  362. }
  363. transmit(block_left, 0);
  364. release(block_left);
  365. block_left=nullptr;
  366. transmit(block_right, 1);
  367. release(block_right);
  368. block_right=nullptr;
  369. }
  370. #ifdef DEBUG_SPDIF_IN
  371. else {
  372. Serial.println("Not enough blocks available. Too few audio memory?");
  373. }
  374. #endif
  375. }
  376. double AsyncAudioInputSPDIF3::getInputFrequency() const{
  377. __disable_irq();
  378. double f=_lastValidInputFrequ;
  379. bool l=locked;
  380. __enable_irq();
  381. return l ? f : 0.;
  382. }
  383. double AsyncAudioInputSPDIF3::getTargetLantency() const {
  384. __disable_irq();
  385. double l=_targetLatencyS;
  386. __enable_irq();
  387. return l ;
  388. }
  389. double AsyncAudioInputSPDIF3::getAttenuation() const{
  390. return _resampler.getAttenuation();
  391. }
  392. int32_t AsyncAudioInputSPDIF3::getHalfFilterLength() const{
  393. return _resampler.getHalfFilterLength();
  394. }
  395. void AsyncAudioInputSPDIF3::config_spdifIn(){
  396. //CCM Clock Gating Register 5, imxrt1060_rev1.pdf page 1145
  397. CCM_CCGR5 |=CCM_CCGR5_SPDIF(CCM_CCGR_ON); //turn spdif clock on - necessary for receiver!
  398. SPDIF_SCR |=SPDIF_SCR_RXFIFO_OFF_ON; //turn receive fifo off 1->off, 0->on
  399. SPDIF_SCR&=~(SPDIF_SCR_RXFIFO_CTR); //reset rx fifo control: normal opertation
  400. SPDIF_SCR&=~(SPDIF_SCR_RXFIFOFULL_SEL(3)); //reset rx full select
  401. SPDIF_SCR|=SPDIF_SCR_RXFIFOFULL_SEL(2); //full interrupt if at least 8 sample in Rx left and right FIFOs
  402. SPDIF_SCR|=SPDIF_SCR_RXAUTOSYNC; //Rx FIFO auto sync on
  403. SPDIF_SCR&=(~SPDIF_SCR_USRC_SEL(3)); //No embedded U channel
  404. CORE_PIN15_CONFIG = 3; //pin 15 set to alt3 -> spdif input
  405. /// from eval board sample code
  406. // IOMUXC_SetPinConfig(
  407. // IOMUXC_GPIO_AD_B1_03_SPDIF_IN, /* GPIO_AD_B1_03 PAD functional properties : */
  408. // 0x10B0u); /* Slew Rate Field: Slow Slew Rate
  409. // Drive Strength Field: R0/6
  410. // Speed Field: medium(100MHz)
  411. // Open Drain Enable Field: Open Drain Disabled
  412. // Pull / Keep Enable Field: Pull/Keeper Enabled
  413. // Pull / Keep Select Field: Keeper
  414. // Pull Up / Down Config. Field: 100K Ohm Pull Down
  415. // Hyst. Enable Field: Hysteresis Disabled */
  416. CORE_PIN15_PADCONFIG=0x10B0;
  417. SPDIF_SCR &=(~SPDIF_SCR_RXFIFO_OFF_ON); //receive fifo is turned on again
  418. SPDIF_SRPC &= ~SPDIF_SRPC_CLKSRC_SEL(15); //reset clock selection page 2136
  419. //SPDIF_SRPC |=SPDIF_SRPC_CLKSRC_SEL(6); //if (DPLL Locked) SPDIF_RxClk else tx_clk (SPDIF0_CLK_ROOT)
  420. //page 2129: FrequMeas[23:0]=FreqMeas_CLK / BUS_CLK * 2^10 * GAIN
  421. SPDIF_SRPC &=~SPDIF_SRPC_GAINSEL(7); //reset gain select 0 -> gain = 24*2^10
  422. //SPDIF_SRPC |= SPDIF_SRPC_GAINSEL(3); //gain select: 8*2^10
  423. //==============================================
  424. //interrupts
  425. SPDIF_SIE |= SPDIF_SIE_LOCK; //enable spdif receiver lock interrupt
  426. SPDIF_SIE |=SPDIF_SIE_LOCKLOSS;
  427. lockChanged=true;
  428. attachInterruptVector(IRQ_SPDIF, spdif_interrupt);
  429. NVIC_SET_PRIORITY(IRQ_SPDIF, 208); // 255 = lowest priority, 208 = priority of update
  430. NVIC_ENABLE_IRQ(IRQ_SPDIF);
  431. SPDIF_SIC |= SPDIF_SIC_LOCK; //clear SPDIF_SIC_LOCK interrupt
  432. SPDIF_SIC |= SPDIF_SIC_LOCKLOSS;//clear SPDIF_SIC_LOCKLOSS interrupt
  433. locked=(SPDIF_SRPC & SPDIF_SRPC_LOCK);
  434. }
  435. #endif