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.

3072 lines
84KB

  1. #include "Audio.h"
  2. #include "arm_math.h"
  3. static arm_cfft_radix4_instance_q15 fft_inst;
  4. void AudioAnalyzeFFT256::init(void)
  5. {
  6. // TODO: replace this with static const version
  7. arm_cfft_radix4_init_q15(&fft_inst, 256, 0, 1);
  8. //for (int i=0; i<2048; i++) {
  9. //buffer[i] = i * 3;
  10. //}
  11. //__disable_irq();
  12. //ARM_DEMCR |= ARM_DEMCR_TRCENA;
  13. //ARM_DWT_CTRL |= ARM_DWT_CTRL_CYCCNTENA;
  14. //uint32_t n = ARM_DWT_CYCCNT;
  15. //arm_cfft_radix2_q15(&fft_inst, buffer);
  16. //n = ARM_DWT_CYCCNT - n;
  17. //__enable_irq();
  18. //cycles = n;
  19. //arm_cmplx_mag_q15(buffer, buffer, 512);
  20. // each audio block is 278525 cycles @ 96 MHz
  21. // 256 point fft2 takes 65408 cycles
  22. // 256 point fft4 takes 49108 cycles
  23. // 128 point cmag takes 10999 cycles
  24. // 1024 point fft2 takes 125948 cycles
  25. // 1024 point fft4 takes 125840 cycles
  26. // 512 point cmag takes 43764 cycles
  27. //state = 0;
  28. //outputflag = false;
  29. }
  30. static void copy_to_fft_buffer(void *destination, const void *source)
  31. {
  32. const int16_t *src = (const int16_t *)source;
  33. int16_t *dst = (int16_t *)destination;
  34. // TODO: optimize this
  35. for (int i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
  36. *dst++ = *src++; // real
  37. *dst++ = 0; // imaginary
  38. }
  39. }
  40. // computes limit((val >> rshift), 2**bits)
  41. static inline int32_t signed_saturate_rshift(int32_t val, int bits, int rshift) __attribute__((always_inline));
  42. static inline int32_t signed_saturate_rshift(int32_t val, int bits, int rshift)
  43. {
  44. int32_t out;
  45. asm volatile("ssat %0, %1, %2, asr %3" : "=r" (out) : "I" (bits), "r" (val), "I" (rshift));
  46. return out;
  47. }
  48. static void apply_window_to_fft_buffer(void *buffer, const void *window)
  49. {
  50. int16_t *buf = (int16_t *)buffer;
  51. const int16_t *win = (int16_t *)window;;
  52. for (int i=0; i < 256; i++) {
  53. int32_t val = *buf * *win++;
  54. //*buf = signed_saturate_rshift(val, 16, 15);
  55. *buf = val >> 15;
  56. buf += 2;
  57. }
  58. }
  59. void AudioAnalyzeFFT256::update(void)
  60. {
  61. audio_block_t *block;
  62. block = receiveReadOnly();
  63. if (!block) return;
  64. if (!prevblock) {
  65. prevblock = block;
  66. return;
  67. }
  68. copy_to_fft_buffer(buffer, prevblock->data);
  69. copy_to_fft_buffer(buffer+256, block->data);
  70. //window = AudioWindowBlackmanNuttall256;
  71. //window = NULL;
  72. if (window) apply_window_to_fft_buffer(buffer, window);
  73. arm_cfft_radix4_q15(&fft_inst, buffer);
  74. // TODO: is this averaging correct? G. Heinzel's paper says we're
  75. // supposed to average the magnitude squared, then do the square
  76. // root at the end after dividing by naverage.
  77. arm_cmplx_mag_q15(buffer, buffer, 128);
  78. if (count == 0) {
  79. for (int i=0; i < 128; i++) {
  80. output[i] = buffer[i];
  81. }
  82. } else {
  83. for (int i=0; i < 128; i++) {
  84. output[i] += buffer[i];
  85. }
  86. }
  87. if (++count == naverage) {
  88. count = 0;
  89. for (int i=0; i < 128; i++) {
  90. output[i] /= naverage;
  91. }
  92. outputflag = true;
  93. }
  94. release(prevblock);
  95. prevblock = block;
  96. #if 0
  97. block = receiveReadOnly();
  98. if (state == 0) {
  99. //Serial.print("0");
  100. if (block == NULL) return;
  101. copy_to_fft_buffer(buffer, block->data);
  102. state = 1;
  103. } else if (state == 1) {
  104. //Serial.print("1");
  105. if (block == NULL) return;
  106. copy_to_fft_buffer(buffer+256, block->data);
  107. arm_cfft_radix4_q15(&fft_inst, buffer);
  108. state = 2;
  109. } else {
  110. //Serial.print("2");
  111. arm_cmplx_mag_q15(buffer, output, 128);
  112. outputflag = true;
  113. if (block == NULL) return;
  114. copy_to_fft_buffer(buffer, block->data);
  115. state = 1;
  116. }
  117. release(block);
  118. #endif
  119. }
  120. #ifdef ORIGINAL_AUDIOSYNTHWAVEFORM
  121. /******************************************************************/
  122. // PAH - add ramp-up and ramp-down to the onset of the wave
  123. // the length is specified in samples
  124. void AudioSynthWaveform::set_ramp_length(uint16_t r_length)
  125. {
  126. if(r_length < 0) {
  127. ramp_length = 0;
  128. return;
  129. }
  130. // Don't set the ramp length longer than about 4 milliseconds
  131. if(r_length > 44*4) {
  132. ramp_length = 44*4;
  133. return;
  134. }
  135. ramp_length = r_length;
  136. }
  137. void AudioSynthWaveform::update(void)
  138. {
  139. audio_block_t *block;
  140. uint32_t i, ph, inc, index, scale;
  141. int32_t val1, val2, val3;
  142. //Serial.println("AudioSynthWaveform::update");
  143. if (((magnitude > 0) || ramp_down) && (block = allocate()) != NULL) {
  144. ph = phase;
  145. inc = phase_increment;
  146. for (i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
  147. index = ph >> 24;
  148. val1 = wavetable[index];
  149. val2 = wavetable[index+1];
  150. scale = (ph >> 8) & 0xFFFF;
  151. val2 *= scale;
  152. val1 *= 0xFFFF - scale;
  153. val3 = (val1 + val2) >> 16;
  154. // The value of ramp_up is always initialized to RAMP_LENGTH and then is
  155. // decremented each time through here until it reaches zero.
  156. // The value of ramp_up is used to generate a Q15 fraction which varies
  157. // from [0 - 1), and multiplies this by the current sample
  158. if(ramp_up) {
  159. // ramp up to the new magnitude
  160. // ramp_mag is the Q15 representation of the fraction
  161. // Since ramp_up can't be zero, this cannot generate +1
  162. ramp_mag = ((ramp_length-ramp_up)<<15)/ramp_length;
  163. ramp_up--;
  164. block->data[i] = (val3 * ((ramp_mag * magnitude)>>15)) >> 15;
  165. } else if(ramp_down) {
  166. // ramp down to zero from the last magnitude
  167. // The value of ramp_down is always initialized to RAMP_LENGTH and then is
  168. // decremented each time through here until it reaches zero.
  169. // The value of ramp_down is used to generate a Q15 fraction which varies
  170. // from (1 - 0], and multiplies this by the current sample
  171. // avoid RAMP_LENGTH/RAMP_LENGTH because Q15 format
  172. // cannot represent +1
  173. ramp_mag = ((ramp_down - 1)<<15)/ramp_length;
  174. ramp_down--;
  175. block->data[i] = (val3 * ((ramp_mag * last_magnitude)>>15)) >> 15;
  176. } else {
  177. block->data[i] = (val3 * magnitude) >> 15;
  178. }
  179. //Serial.print(block->data[i]);
  180. //Serial.print(", ");
  181. //if ((i % 12) == 11) Serial.println();
  182. ph += inc;
  183. }
  184. //Serial.println();
  185. phase = ph;
  186. transmit(block);
  187. release(block);
  188. } else {
  189. // is this numerical overflow ok?
  190. phase += phase_increment * AUDIO_BLOCK_SAMPLES;
  191. }
  192. }
  193. #else
  194. /******************************************************************/
  195. // PAH - add ramp-up and ramp-down to the onset of the wave
  196. // the length is specified in samples
  197. void AudioSynthWaveform::set_ramp_length(uint16_t r_length)
  198. {
  199. if(r_length < 0) {
  200. ramp_length = 0;
  201. return;
  202. }
  203. // Don't set the ramp length longer than about 4 milliseconds
  204. if(r_length > 44*4) {
  205. ramp_length = 44*4;
  206. return;
  207. }
  208. ramp_length = r_length;
  209. }
  210. boolean AudioSynthWaveform::begin(float t_amp,int t_hi,short type)
  211. {
  212. tone_type = type;
  213. // tone_amp = t_amp;
  214. amplitude(t_amp);
  215. tone_freq = t_hi;
  216. if(t_hi < 1)return false;
  217. if(t_hi >= AUDIO_SAMPLE_RATE_EXACT/2)return false;
  218. tone_phase = 0;
  219. tone_incr = (0x100000000LL*t_hi)/AUDIO_SAMPLE_RATE_EXACT;
  220. if(0) {
  221. Serial.print("AudioSynthWaveform.begin(tone_amp = ");
  222. Serial.print(t_amp);
  223. Serial.print(", tone_hi = ");
  224. Serial.print(t_hi);
  225. Serial.print(", tone_incr = ");
  226. Serial.print(tone_incr,HEX);
  227. // Serial.print(", tone_hi = ");
  228. // Serial.print(t_hi);
  229. Serial.println(")");
  230. }
  231. return(true);
  232. }
  233. void AudioSynthWaveform::update(void)
  234. {
  235. audio_block_t *block;
  236. short *bp;
  237. // temporary for ramp in sine
  238. uint32_t ramp_mag;
  239. // temporaries for TRIANGLE
  240. uint32_t mag;
  241. short tmp_amp;
  242. if(tone_freq == 0)return;
  243. // L E F T C H A N N E L O N L Y
  244. block = allocate();
  245. if(block) {
  246. bp = block->data;
  247. switch(tone_type) {
  248. case TONE_TYPE_SINE:
  249. for(int i = 0;i < AUDIO_BLOCK_SAMPLES;i++) {
  250. // The value of ramp_up is always initialized to RAMP_LENGTH and then is
  251. // decremented each time through here until it reaches zero.
  252. // The value of ramp_up is used to generate a Q15 fraction which varies
  253. // from [0 - 1), and multiplies this by the current sample
  254. if(ramp_up) {
  255. // ramp up to the new magnitude
  256. // ramp_mag is the Q15 representation of the fraction
  257. // Since ramp_up can't be zero, this cannot generate +1
  258. ramp_mag = ((ramp_length-ramp_up)<<15)/ramp_length;
  259. ramp_up--;
  260. // adjust tone_phase to Q15 format and then adjust the result
  261. // of the multiplication
  262. *bp = (short)((arm_sin_q15(tone_phase>>17) * tone_amp) >> 15);
  263. *bp++ = (*bp * ramp_mag)>>15;
  264. }
  265. else if(ramp_down) {
  266. // ramp down to zero from the last magnitude
  267. // The value of ramp_down is always initialized to RAMP_LENGTH and then is
  268. // decremented each time through here until it reaches zero.
  269. // The value of ramp_down is used to generate a Q15 fraction which varies
  270. // from (1 - 0], and multiplies this by the current sample
  271. // avoid RAMP_LENGTH/RAMP_LENGTH because Q15 format
  272. // cannot represent +1
  273. ramp_mag = ((ramp_down - 1)<<15)/ramp_length;
  274. ramp_down--;
  275. // adjust tone_phase to Q15 format and then adjust the result
  276. // of the multiplication
  277. *bp = (short)((arm_sin_q15(tone_phase>>17) * last_tone_amp) >> 15);
  278. *bp++ = (*bp * ramp_mag)>>15;
  279. } else {
  280. // adjust tone_phase to Q15 format and then adjust the result
  281. // of the multiplication
  282. *bp++ = (short)((arm_sin_q15(tone_phase>>17) * tone_amp) >> 15);
  283. }
  284. // phase and incr are both unsigned 32-bit fractions
  285. tone_phase += tone_incr;
  286. }
  287. break;
  288. case TONE_TYPE_SQUARE:
  289. for(int i = 0;i < AUDIO_BLOCK_SAMPLES;i++) {
  290. if(tone_phase & 0x80000000)*bp++ = -tone_amp;
  291. else *bp++ = tone_amp;
  292. // phase and incr are both unsigned 32-bit fractions
  293. tone_phase += tone_incr;
  294. }
  295. break;
  296. case TONE_TYPE_SAWTOOTH:
  297. for(int i = 0;i < AUDIO_BLOCK_SAMPLES;i++) {
  298. *bp = ((short)(tone_phase>>16)*tone_amp) >> 15;
  299. bp++;
  300. // phase and incr are both unsigned 32-bit fractions
  301. tone_phase += tone_incr;
  302. }
  303. break;
  304. case TONE_TYPE_TRIANGLE:
  305. for(int i = 0;i < AUDIO_BLOCK_SAMPLES;i++) {
  306. if(tone_phase & 0x80000000) {
  307. // negative half-cycle
  308. tmp_amp = -tone_amp;
  309. }
  310. else {
  311. // positive half-cycle
  312. tmp_amp = tone_amp;
  313. }
  314. mag = tone_phase << 2;
  315. // Determine which quadrant
  316. if(tone_phase & 0x40000000) {
  317. // negate the magnitude
  318. mag = ~mag + 1;
  319. }
  320. *bp++ = ((short)(mag>>17)*tmp_amp) >> 15;
  321. tone_phase += tone_incr;
  322. }
  323. break;
  324. }
  325. // send the samples to the left channel
  326. transmit(block,0);
  327. release(block);
  328. }
  329. }
  330. #endif
  331. #if 0
  332. void AudioSineWaveMod::frequency(float f)
  333. {
  334. if (f > AUDIO_SAMPLE_RATE_EXACT / 2 || f < 0.0) return;
  335. phase_increment = (f / AUDIO_SAMPLE_RATE_EXACT) * 4294967296.0f;
  336. }
  337. void AudioSineWaveMod::update(void)
  338. {
  339. audio_block_t *block, *modinput;
  340. uint32_t i, ph, inc, index, scale;
  341. int32_t val1, val2;
  342. //Serial.println("AudioSineWave::update");
  343. modinput = receiveReadOnly();
  344. ph = phase;
  345. inc = phase_increment;
  346. block = allocate();
  347. if (!block) {
  348. // unable to allocate memory, so we'll send nothing
  349. if (modinput) {
  350. // but if we got modulation data, update the phase
  351. for (i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
  352. ph += inc + modinput->data[i] * modulation_factor;
  353. }
  354. release(modinput);
  355. } else {
  356. ph += phase_increment * AUDIO_BLOCK_SAMPLES;
  357. }
  358. phase = ph;
  359. return;
  360. }
  361. if (modinput) {
  362. for (i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
  363. index = ph >> 24;
  364. val1 = sine_table[index];
  365. val2 = sine_table[index+1];
  366. scale = (ph >> 8) & 0xFFFF;
  367. val2 *= scale;
  368. val1 *= 0xFFFF - scale;
  369. block->data[i] = (val1 + val2) >> 16;
  370. //Serial.print(block->data[i]);
  371. //Serial.print(", ");
  372. //if ((i % 12) == 11) Serial.println();
  373. ph += inc + modinput->data[i] * modulation_factor;
  374. }
  375. release(modinput);
  376. } else {
  377. ph = phase;
  378. inc = phase_increment;
  379. for (i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
  380. index = ph >> 24;
  381. val1 = sine_table[index];
  382. val2 = sine_table[index+1];
  383. scale = (ph >> 8) & 0xFFFF;
  384. val2 *= scale;
  385. val1 *= 0xFFFF - scale;
  386. block->data[i] = (val1 + val2) >> 16;
  387. //Serial.print(block->data[i]);
  388. //Serial.print(", ");
  389. //if ((i % 12) == 11) Serial.println();
  390. ph += inc;
  391. }
  392. }
  393. phase = ph;
  394. transmit(block);
  395. release(block);
  396. }
  397. #endif
  398. /******************************************************************/
  399. void AudioPrint::update(void)
  400. {
  401. audio_block_t *block;
  402. uint32_t i;
  403. Serial.println("AudioPrint::update");
  404. Serial.println(name);
  405. block = receiveReadOnly();
  406. if (block) {
  407. for (i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
  408. Serial.print(block->data[i]);
  409. Serial.print(", ");
  410. if ((i % 12) == 11) Serial.println();
  411. }
  412. Serial.println();
  413. release(block);
  414. }
  415. }
  416. /******************************************************************/
  417. #define STATE_DIRECT_8BIT_MONO 0 // playing mono at native sample rate
  418. #define STATE_DIRECT_8BIT_STEREO 1 // playing stereo at native sample rate
  419. #define STATE_DIRECT_16BIT_MONO 2 // playing mono at native sample rate
  420. #define STATE_DIRECT_16BIT_STEREO 3 // playing stereo at native sample rate
  421. #define STATE_CONVERT_8BIT_MONO 4 // playing mono, converting sample rate
  422. #define STATE_CONVERT_8BIT_STEREO 5 // playing stereo, converting sample rate
  423. #define STATE_CONVERT_16BIT_MONO 6 // playing mono, converting sample rate
  424. #define STATE_CONVERT_16BIT_STEREO 7 // playing stereo, converting sample rate
  425. #define STATE_PARSE1 8 // looking for 20 byte ID header
  426. #define STATE_PARSE2 9 // looking for 16 byte format header
  427. #define STATE_PARSE3 10 // looking for 8 byte data header
  428. #define STATE_PARSE4 11 // ignoring unknown chunk
  429. #define STATE_STOP 12
  430. void AudioPlaySDcardWAV::begin(void)
  431. {
  432. state = STATE_STOP;
  433. state_play = STATE_STOP;
  434. data_length = 0;
  435. if (block_left) {
  436. release(block_left);
  437. block_left = NULL;
  438. }
  439. if (block_right) {
  440. release(block_right);
  441. block_right = NULL;
  442. }
  443. }
  444. bool AudioPlaySDcardWAV::play(const char *filename)
  445. {
  446. stop();
  447. wavfile = SD.open(filename);
  448. if (!wavfile) return false;
  449. buffer_remaining = 0;
  450. state_play = STATE_STOP;
  451. data_length = 0;
  452. state = STATE_PARSE1;
  453. return true;
  454. }
  455. void AudioPlaySDcardWAV::stop(void)
  456. {
  457. __disable_irq();
  458. if (state != STATE_STOP) {
  459. audio_block_t *b1 = block_left;
  460. block_left = NULL;
  461. audio_block_t *b2 = block_right;
  462. block_right = NULL;
  463. state = STATE_STOP;
  464. __enable_irq();
  465. if (b1) release(b1);
  466. if (b2) release(b2);
  467. wavfile.close();
  468. } else {
  469. __enable_irq();
  470. }
  471. }
  472. bool AudioPlaySDcardWAV::start(void)
  473. {
  474. __disable_irq();
  475. if (state == STATE_STOP) {
  476. if (state_play == STATE_STOP) {
  477. __enable_irq();
  478. return false;
  479. }
  480. state = state_play;
  481. }
  482. __enable_irq();
  483. return true;
  484. }
  485. void AudioPlaySDcardWAV::update(void)
  486. {
  487. // only update if we're playing
  488. if (state == STATE_STOP) return;
  489. // allocate the audio blocks to transmit
  490. block_left = allocate();
  491. if (block_left == NULL) return;
  492. if (state < 8 && (state & 1) == 1) {
  493. // if we're playing stereo, allocate another
  494. // block for the right channel output
  495. block_right = allocate();
  496. if (block_right == NULL) {
  497. release(block_left);
  498. return;
  499. }
  500. } else {
  501. // if we're playing mono or just parsing
  502. // the WAV file header, no right-side block
  503. block_right = NULL;
  504. }
  505. block_offset = 0;
  506. //Serial.println("update");
  507. // is there buffered data?
  508. if (buffer_remaining > 0) {
  509. // we have buffered data
  510. if (consume()) return; // it was enough to transmit audio
  511. }
  512. // we only get to this point when buffer[512] is empty
  513. if (state != STATE_STOP && wavfile.available()) {
  514. // we can read more data from the file...
  515. buffer_remaining = wavfile.read(buffer, 512);
  516. if (consume()) {
  517. // good, it resulted in audio transmit
  518. return;
  519. } else {
  520. // not good, no audio was transmitted
  521. buffer_remaining = 0;
  522. if (block_left) {
  523. release(block_left);
  524. block_left = NULL;
  525. }
  526. if (block_right) {
  527. release(block_right);
  528. block_right = NULL;
  529. }
  530. // if we're still playing, well, there's going to
  531. // be a gap in output, but we can't keep burning
  532. // time trying to read more data. Hopefully things
  533. // will go better next time?
  534. if (state != STATE_STOP) return;
  535. }
  536. }
  537. // end of file reached or other reason to stop
  538. wavfile.close();
  539. if (block_left) {
  540. release(block_left);
  541. block_left = NULL;
  542. }
  543. if (block_right) {
  544. release(block_right);
  545. block_right = NULL;
  546. }
  547. state_play = STATE_STOP;
  548. state = STATE_STOP;
  549. }
  550. // https://ccrma.stanford.edu/courses/422/projects/WaveFormat/
  551. // Consume already buffered data. Returns true if audio transmitted.
  552. bool AudioPlaySDcardWAV::consume(void)
  553. {
  554. uint32_t len, size;
  555. uint8_t lsb, msb;
  556. const uint8_t *p;
  557. size = buffer_remaining;
  558. p = buffer + 512 - size;
  559. start:
  560. if (size == 0) return false;
  561. //Serial.print("AudioPlaySDcardWAV write, size = ");
  562. //Serial.print(size);
  563. //Serial.print(", data_length = ");
  564. //Serial.print(data_length);
  565. //Serial.print(", state = ");
  566. //Serial.println(state);
  567. switch (state) {
  568. // parse wav file header, is this really a .wav file?
  569. case STATE_PARSE1:
  570. len = 20 - data_length;
  571. if (size < len) len = size;
  572. memcpy((uint8_t *)header + data_length, p, len);
  573. data_length += len;
  574. if (data_length < 20) return false;
  575. // parse the header...
  576. if (header[0] == 0x46464952 && header[2] == 0x45564157
  577. && header[3] == 0x20746D66 && header[4] == 16) {
  578. //Serial.println("header ok");
  579. state = STATE_PARSE2;
  580. p += len;
  581. size -= len;
  582. data_length = 0;
  583. goto start;
  584. }
  585. //Serial.println("unknown WAV header");
  586. break;
  587. // check & extract key audio parameters
  588. case STATE_PARSE2:
  589. len = 16 - data_length;
  590. if (size < len) len = size;
  591. memcpy((uint8_t *)header + data_length, p, len);
  592. data_length += len;
  593. if (data_length < 16) return false;
  594. if (parse_format()) {
  595. //Serial.println("audio format ok");
  596. p += len;
  597. size -= len;
  598. data_length = 0;
  599. state = STATE_PARSE3;
  600. goto start;
  601. }
  602. //Serial.println("unknown audio format");
  603. break;
  604. // find the data chunk
  605. case STATE_PARSE3:
  606. len = 8 - data_length;
  607. if (size < len) len = size;
  608. memcpy((uint8_t *)header + data_length, p, len);
  609. data_length += len;
  610. if (data_length < 8) return false;
  611. //Serial.print("chunk id = ");
  612. //Serial.print(header[0], HEX);
  613. //Serial.print(", length = ");
  614. //Serial.println(header[1]);
  615. p += len;
  616. size -= len;
  617. data_length = header[1];
  618. if (header[0] == 0x61746164) {
  619. //Serial.println("found data chunk");
  620. // TODO: verify offset in file is an even number
  621. // as required by WAV format. abort if odd. Code
  622. // below will depend upon this and fail if not even.
  623. leftover_bytes = 0;
  624. state = state_play;
  625. if (state & 1) {
  626. // if we're going to start stereo
  627. // better allocate another output block
  628. block_right = allocate();
  629. if (!block_right) return false;
  630. }
  631. } else {
  632. state = STATE_PARSE4;
  633. }
  634. goto start;
  635. // ignore any extra unknown chunks (title & artist info)
  636. case STATE_PARSE4:
  637. if (size < data_length) {
  638. data_length -= size;
  639. return false;
  640. }
  641. p += data_length;
  642. size -= data_length;
  643. data_length = 0;
  644. state = STATE_PARSE3;
  645. goto start;
  646. // playing mono at native sample rate
  647. case STATE_DIRECT_8BIT_MONO:
  648. return false;
  649. // playing stereo at native sample rate
  650. case STATE_DIRECT_8BIT_STEREO:
  651. return false;
  652. // playing mono at native sample rate
  653. case STATE_DIRECT_16BIT_MONO:
  654. if (size > data_length) size = data_length;
  655. data_length -= size;
  656. while (1) {
  657. lsb = *p++;
  658. msb = *p++;
  659. size -= 2;
  660. block_left->data[block_offset++] = (msb << 8) | lsb;
  661. if (block_offset >= AUDIO_BLOCK_SAMPLES) {
  662. transmit(block_left, 0);
  663. transmit(block_left, 1);
  664. //Serial1.print('%');
  665. //delayMicroseconds(90);
  666. release(block_left);
  667. block_left = NULL;
  668. data_length += size;
  669. buffer_remaining = size;
  670. if (block_right) release(block_right);
  671. return true;
  672. }
  673. if (size == 0) {
  674. if (data_length == 0) break;
  675. return false;
  676. }
  677. }
  678. // end of file reached
  679. if (block_offset > 0) {
  680. // TODO: fill remainder of last block with zero and transmit
  681. }
  682. state = STATE_STOP;
  683. return false;
  684. // playing stereo at native sample rate
  685. case STATE_DIRECT_16BIT_STEREO:
  686. if (size > data_length) size = data_length;
  687. data_length -= size;
  688. if (leftover_bytes) {
  689. block_left->data[block_offset] = header[0];
  690. goto right16;
  691. }
  692. while (1) {
  693. lsb = *p++;
  694. msb = *p++;
  695. size -= 2;
  696. if (size == 0) {
  697. if (data_length == 0) break;
  698. header[0] = (msb << 8) | lsb;
  699. leftover_bytes = 2;
  700. return false;
  701. }
  702. block_left->data[block_offset] = (msb << 8) | lsb;
  703. right16:
  704. lsb = *p++;
  705. msb = *p++;
  706. size -= 2;
  707. block_right->data[block_offset++] = (msb << 8) | lsb;
  708. if (block_offset >= AUDIO_BLOCK_SAMPLES) {
  709. transmit(block_left, 0);
  710. release(block_left);
  711. block_left = NULL;
  712. transmit(block_right, 1);
  713. release(block_right);
  714. block_right = NULL;
  715. data_length += size;
  716. buffer_remaining = size;
  717. return true;
  718. }
  719. if (size == 0) {
  720. if (data_length == 0) break;
  721. leftover_bytes = 0;
  722. return false;
  723. }
  724. }
  725. // end of file reached
  726. if (block_offset > 0) {
  727. // TODO: fill remainder of last block with zero and transmit
  728. }
  729. state = STATE_STOP;
  730. return false;
  731. // playing mono, converting sample rate
  732. case STATE_CONVERT_8BIT_MONO :
  733. return false;
  734. // playing stereo, converting sample rate
  735. case STATE_CONVERT_8BIT_STEREO:
  736. return false;
  737. // playing mono, converting sample rate
  738. case STATE_CONVERT_16BIT_MONO:
  739. return false;
  740. // playing stereo, converting sample rate
  741. case STATE_CONVERT_16BIT_STEREO:
  742. return false;
  743. // ignore any extra data after playing
  744. // or anything following any error
  745. case STATE_STOP:
  746. return false;
  747. // this is not supposed to happen!
  748. //default:
  749. //Serial.println("AudioPlaySDcardWAV, unknown state");
  750. }
  751. state_play = STATE_STOP;
  752. state = STATE_STOP;
  753. return false;
  754. }
  755. /*
  756. 00000000 52494646 66EA6903 57415645 666D7420 RIFFf.i.WAVEfmt
  757. 00000010 10000000 01000200 44AC0000 10B10200 ........D.......
  758. 00000020 04001000 4C495354 3A000000 494E464F ....LIST:...INFO
  759. 00000030 494E414D 14000000 49205761 6E742054 INAM....I Want T
  760. 00000040 6F20436F 6D65204F 76657200 49415254 o Come Over.IART
  761. 00000050 12000000 4D656C69 73736120 45746865 ....Melissa Ethe
  762. 00000060 72696467 65006461 746100EA 69030100 ridge.data..i...
  763. 00000070 FEFF0300 FCFF0400 FDFF0200 0000FEFF ................
  764. 00000080 0300FDFF 0200FFFF 00000100 FEFF0300 ................
  765. 00000090 FDFF0300 FDFF0200 FFFF0100 0000FFFF ................
  766. */
  767. // SD library on Teensy3 at 96 MHz
  768. // 256 byte chunks, speed is 443272 bytes/sec
  769. // 512 byte chunks, speed is 468023 bytes/sec
  770. bool AudioPlaySDcardWAV::parse_format(void)
  771. {
  772. uint8_t num = 0;
  773. uint16_t format;
  774. uint16_t channels;
  775. uint32_t rate;
  776. uint16_t bits;
  777. format = header[0];
  778. //Serial.print(" format = ");
  779. //Serial.println(format);
  780. if (format != 1) return false;
  781. channels = header[0] >> 16;
  782. //Serial.print(" channels = ");
  783. //Serial.println(channels);
  784. if (channels == 1) {
  785. } else if (channels == 2) {
  786. num = 1;
  787. } else {
  788. return false;
  789. }
  790. bits = header[3] >> 16;
  791. //Serial.print(" bits = ");
  792. //Serial.println(bits);
  793. if (bits == 8) {
  794. } else if (bits == 16) {
  795. num |= 2;
  796. } else {
  797. return false;
  798. }
  799. rate = header[1];
  800. //Serial.print(" rate = ");
  801. //Serial.println(rate);
  802. if (rate == AUDIO_SAMPLE_RATE) {
  803. } else if (rate >= 8000 && rate <= 48000) {
  804. num |= 4;
  805. } else {
  806. return false;
  807. }
  808. // we're not checking the byte rate and block align fields
  809. // if they're not the expected values, all we could do is
  810. // return false. Do any real wav files have unexpected
  811. // values in these other fields?
  812. state_play = num;
  813. return true;
  814. }
  815. /******************************************************************/
  816. void AudioPlaySDcardRAW::begin(void)
  817. {
  818. playing = false;
  819. if (block) {
  820. release(block);
  821. block = NULL;
  822. }
  823. }
  824. bool AudioPlaySDcardRAW::play(const char *filename)
  825. {
  826. stop();
  827. rawfile = SD.open(filename);
  828. if (!rawfile) {
  829. Serial.println("unable to open file");
  830. return false;
  831. }
  832. Serial.println("able to open file");
  833. playing = true;
  834. return true;
  835. }
  836. void AudioPlaySDcardRAW::stop(void)
  837. {
  838. __disable_irq();
  839. if (playing) {
  840. playing = false;
  841. __enable_irq();
  842. rawfile.close();
  843. } else {
  844. __enable_irq();
  845. }
  846. }
  847. void AudioPlaySDcardRAW::update(void)
  848. {
  849. unsigned int i, n;
  850. // only update if we're playing
  851. if (!playing) return;
  852. // allocate the audio blocks to transmit
  853. block = allocate();
  854. if (block == NULL) return;
  855. if (rawfile.available()) {
  856. // we can read more data from the file...
  857. n = rawfile.read(block->data, AUDIO_BLOCK_SAMPLES*2);
  858. for (i=n/2; i < AUDIO_BLOCK_SAMPLES; i++) {
  859. block->data[i] = 0;
  860. }
  861. transmit(block);
  862. release(block);
  863. } else {
  864. rawfile.close();
  865. playing = false;
  866. }
  867. }
  868. /******************************************************************/
  869. void AudioPlayMemory::play(const unsigned int *data)
  870. {
  871. uint32_t format;
  872. playing = 0;
  873. prior = 0;
  874. format = *data++;
  875. next = data;
  876. length = format & 0xFFFFFF;
  877. playing = format >> 24;
  878. }
  879. void AudioPlayMemory::stop(void)
  880. {
  881. playing = 0;
  882. }
  883. extern "C" {
  884. extern const int16_t ulaw_decode_table[256];
  885. };
  886. void AudioPlayMemory::update(void)
  887. {
  888. audio_block_t *block;
  889. const unsigned int *in;
  890. int16_t *out;
  891. uint32_t tmp32, consumed;
  892. int16_t s0, s1, s2, s3, s4;
  893. int i;
  894. if (!playing) return;
  895. block = allocate();
  896. if (block == NULL) return;
  897. //Serial.write('.');
  898. out = block->data;
  899. in = next;
  900. s0 = prior;
  901. switch (playing) {
  902. case 0x01: // u-law encoded, 44100 Hz
  903. for (i=0; i < AUDIO_BLOCK_SAMPLES; i += 4) {
  904. tmp32 = *in++;
  905. *out++ = ulaw_decode_table[(tmp32 >> 0) & 255];
  906. *out++ = ulaw_decode_table[(tmp32 >> 8) & 255];
  907. *out++ = ulaw_decode_table[(tmp32 >> 16) & 255];
  908. *out++ = ulaw_decode_table[(tmp32 >> 24) & 255];
  909. }
  910. consumed = 128;
  911. break;
  912. case 0x81: // 16 bit PCM, 44100 Hz
  913. for (i=0; i < AUDIO_BLOCK_SAMPLES; i += 2) {
  914. tmp32 = *in++;
  915. *out++ = (int16_t)(tmp32 & 65535);
  916. *out++ = (int16_t)(tmp32 >> 16);
  917. }
  918. consumed = 128;
  919. break;
  920. case 0x02: // u-law encoded, 22050 Hz
  921. for (i=0; i < AUDIO_BLOCK_SAMPLES; i += 8) {
  922. tmp32 = *in++;
  923. s1 = ulaw_decode_table[(tmp32 >> 0) & 255];
  924. s2 = ulaw_decode_table[(tmp32 >> 8) & 255];
  925. s3 = ulaw_decode_table[(tmp32 >> 16) & 255];
  926. s4 = ulaw_decode_table[(tmp32 >> 24) & 255];
  927. *out++ = (s0 + s1) >> 1;
  928. *out++ = s1;
  929. *out++ = (s1 + s2) >> 1;
  930. *out++ = s2;
  931. *out++ = (s2 + s3) >> 1;
  932. *out++ = s3;
  933. *out++ = (s3 + s4) >> 1;
  934. *out++ = s4;
  935. s0 = s4;
  936. }
  937. consumed = 64;
  938. break;
  939. case 0x82: // 16 bits PCM, 22050 Hz
  940. for (i=0; i < AUDIO_BLOCK_SAMPLES; i += 4) {
  941. tmp32 = *in++;
  942. s1 = (int16_t)(tmp32 & 65535);
  943. s2 = (int16_t)(tmp32 >> 16);
  944. *out++ = (s0 + s1) >> 1;
  945. *out++ = s1;
  946. *out++ = (s1 + s2) >> 1;
  947. *out++ = s2;
  948. s0 = s2;
  949. }
  950. consumed = 64;
  951. break;
  952. case 0x03: // u-law encoded, 11025 Hz
  953. for (i=0; i < AUDIO_BLOCK_SAMPLES; i += 16) {
  954. tmp32 = *in++;
  955. s1 = ulaw_decode_table[(tmp32 >> 0) & 255];
  956. s2 = ulaw_decode_table[(tmp32 >> 8) & 255];
  957. s3 = ulaw_decode_table[(tmp32 >> 16) & 255];
  958. s4 = ulaw_decode_table[(tmp32 >> 24) & 255];
  959. *out++ = (s0 * 3 + s1) >> 2;
  960. *out++ = (s0 + s1) >> 1;
  961. *out++ = (s0 + s1 * 3) >> 2;
  962. *out++ = s1;
  963. *out++ = (s1 * 3 + s2) >> 2;
  964. *out++ = (s1 + s2) >> 1;
  965. *out++ = (s1 + s2 * 3) >> 2;
  966. *out++ = s2;
  967. *out++ = (s2 * 3 + s3) >> 2;
  968. *out++ = (s2 + s3) >> 1;
  969. *out++ = (s2 + s3 * 3) >> 2;
  970. *out++ = s3;
  971. *out++ = (s3 * 3 + s4) >> 2;
  972. *out++ = (s3 + s4) >> 1;
  973. *out++ = (s3 + s4 * 3) >> 2;
  974. *out++ = s4;
  975. s0 = s4;
  976. }
  977. consumed = 32;
  978. break;
  979. case 0x83: // 16 bit PCM, 11025 Hz
  980. for (i=0; i < AUDIO_BLOCK_SAMPLES; i += 8) {
  981. tmp32 = *in++;
  982. s1 = (int16_t)(tmp32 & 65535);
  983. s2 = (int16_t)(tmp32 >> 16);
  984. *out++ = (s0 * 3 + s1) >> 2;
  985. *out++ = (s0 + s1) >> 1;
  986. *out++ = (s0 + s1 * 3) >> 2;
  987. *out++ = s1;
  988. *out++ = (s1 * 3 + s2) >> 2;
  989. *out++ = (s1 + s2) >> 1;
  990. *out++ = (s1 + s2 * 3) >> 2;
  991. *out++ = s2;
  992. s0 = s2;
  993. }
  994. consumed = 32;
  995. break;
  996. default:
  997. release(block);
  998. playing = 0;
  999. return;
  1000. }
  1001. prior = s0;
  1002. next = in;
  1003. if (length > consumed) {
  1004. length -= consumed;
  1005. } else {
  1006. playing = 0;
  1007. }
  1008. transmit(block);
  1009. release(block);
  1010. }
  1011. /******************************************************************/
  1012. // computes ((a[31:0] * b[15:0]) >> 16)
  1013. static inline int32_t signed_multiply_32x16b(int32_t a, uint32_t b) __attribute__((always_inline));
  1014. static inline int32_t signed_multiply_32x16b(int32_t a, uint32_t b)
  1015. {
  1016. int32_t out;
  1017. asm volatile("smulwb %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));
  1018. return out;
  1019. }
  1020. // computes ((a[31:0] * b[31:16]) >> 16)
  1021. static inline int32_t signed_multiply_32x16t(int32_t a, uint32_t b) __attribute__((always_inline));
  1022. static inline int32_t signed_multiply_32x16t(int32_t a, uint32_t b)
  1023. {
  1024. int32_t out;
  1025. asm volatile("smulwt %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));
  1026. return out;
  1027. }
  1028. // computes (((int64_t)a[31:0] * (int64_t)b[31:0]) >> 32)
  1029. static inline int32_t multiply_32x32_rshift32(int32_t a, int32_t b) __attribute__((always_inline));
  1030. static inline int32_t multiply_32x32_rshift32(int32_t a, int32_t b)
  1031. {
  1032. int32_t out;
  1033. asm volatile("smmul %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));
  1034. return out;
  1035. }
  1036. // computes (((int64_t)a[31:0] * (int64_t)b[31:0] + 0x8000000) >> 32)
  1037. static inline int32_t multiply_32x32_rshift32_rounded(int32_t a, int32_t b) __attribute__((always_inline));
  1038. static inline int32_t multiply_32x32_rshift32_rounded(int32_t a, int32_t b)
  1039. {
  1040. int32_t out;
  1041. asm volatile("smmulr %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));
  1042. return out;
  1043. }
  1044. // computes sum + (((int64_t)a[31:0] * (int64_t)b[31:0] + 0x8000000) >> 32)
  1045. static inline int32_t multiply_accumulate_32x32_rshift32_rounded(int32_t sum, int32_t a, int32_t b) __attribute__((always_inline));
  1046. static inline int32_t multiply_accumulate_32x32_rshift32_rounded(int32_t sum, int32_t a, int32_t b)
  1047. {
  1048. int32_t out;
  1049. asm volatile("smmlar %0, %2, %3, %1" : "=r" (out) : "r" (sum), "r" (a), "r" (b));
  1050. return out;
  1051. }
  1052. // computes sum - (((int64_t)a[31:0] * (int64_t)b[31:0] + 0x8000000) >> 32)
  1053. static inline int32_t multiply_subtract_32x32_rshift32_rounded(int32_t sum, int32_t a, int32_t b) __attribute__((always_inline));
  1054. static inline int32_t multiply_subtract_32x32_rshift32_rounded(int32_t sum, int32_t a, int32_t b)
  1055. {
  1056. int32_t out;
  1057. asm volatile("smmlsr %0, %2, %3, %1" : "=r" (out) : "r" (sum), "r" (a), "r" (b));
  1058. return out;
  1059. }
  1060. // computes ((a[15:0] << 16) | b[15:0])
  1061. static inline uint32_t pack_16x16(int32_t a, int32_t b) __attribute__((always_inline));
  1062. static inline uint32_t pack_16x16(int32_t a, int32_t b)
  1063. {
  1064. int32_t out;
  1065. asm volatile("pkhbt %0, %1, %2, lsl #16" : "=r" (out) : "r" (b), "r" (a));
  1066. return out;
  1067. }
  1068. // computes (((a[31:16] + b[31:16]) << 16) | (a[15:0 + b[15:0]))
  1069. static inline uint32_t signed_add_16_and_16(uint32_t a, uint32_t b) __attribute__((always_inline));
  1070. static inline uint32_t signed_add_16_and_16(uint32_t a, uint32_t b)
  1071. {
  1072. int32_t out;
  1073. asm volatile("qadd16 %0, %1, %2" : "=r" (out) : "r" (a), "r" (b));
  1074. return out;
  1075. }
  1076. // computes (sum + ((a[31:0] * b[15:0]) >> 16))
  1077. static inline int32_t signed_multiply_accumulate_32x16b(int32_t sum, int32_t a, uint32_t b)
  1078. {
  1079. int32_t out;
  1080. asm volatile("smlawb %0, %2, %3, %1" : "=r" (out) : "r" (sum), "r" (a), "r" (b));
  1081. return out;
  1082. }
  1083. // computes (sum + ((a[31:0] * b[31:16]) >> 16))
  1084. static inline int32_t signed_multiply_accumulate_32x16t(int32_t sum, int32_t a, uint32_t b)
  1085. {
  1086. int32_t out;
  1087. asm volatile("smlawt %0, %2, %3, %1" : "=r" (out) : "r" (sum), "r" (a), "r" (b));
  1088. return out;
  1089. }
  1090. // computes logical and, forces compiler to allocate register and use single cycle instruction
  1091. static inline uint32_t logical_and(uint32_t a, uint32_t b)
  1092. {
  1093. asm volatile("and %0, %1" : "+r" (a) : "r" (b));
  1094. return a;
  1095. }
  1096. void applyGain(int16_t *data, int32_t mult)
  1097. {
  1098. uint32_t *p = (uint32_t *)data;
  1099. const uint32_t *end = (uint32_t *)(data + AUDIO_BLOCK_SAMPLES);
  1100. do {
  1101. uint32_t tmp32 = *p; // read 2 samples from *data
  1102. int32_t val1 = signed_multiply_32x16b(mult, tmp32);
  1103. int32_t val2 = signed_multiply_32x16t(mult, tmp32);
  1104. val1 = signed_saturate_rshift(val1, 16, 0);
  1105. val2 = signed_saturate_rshift(val2, 16, 0);
  1106. *p++ = pack_16x16(val2, val1);
  1107. } while (p < end);
  1108. }
  1109. // page 133
  1110. void applyGainThenAdd(int16_t *data, const int16_t *in, int32_t mult)
  1111. {
  1112. uint32_t *dst = (uint32_t *)data;
  1113. const uint32_t *src = (uint32_t *)in;
  1114. const uint32_t *end = (uint32_t *)(data + AUDIO_BLOCK_SAMPLES);
  1115. if (mult == 65536) {
  1116. do {
  1117. uint32_t tmp32 = *dst;
  1118. *dst++ = signed_add_16_and_16(tmp32, *src++);
  1119. tmp32 = *dst;
  1120. *dst++ = signed_add_16_and_16(tmp32, *src++);
  1121. } while (dst < end);
  1122. } else {
  1123. do {
  1124. uint32_t tmp32 = *src++; // read 2 samples from *data
  1125. int32_t val1 = signed_multiply_32x16b(mult, tmp32);
  1126. int32_t val2 = signed_multiply_32x16t(mult, tmp32);
  1127. val1 = signed_saturate_rshift(val1, 16, 0);
  1128. val2 = signed_saturate_rshift(val2, 16, 0);
  1129. tmp32 = pack_16x16(val2, val1);
  1130. uint32_t tmp32b = *dst;
  1131. *dst++ = signed_add_16_and_16(tmp32, tmp32b);
  1132. } while (dst < end);
  1133. }
  1134. }
  1135. void AudioMixer4::update(void)
  1136. {
  1137. audio_block_t *in, *out=NULL;
  1138. unsigned int channel;
  1139. for (channel=0; channel < 4; channel++) {
  1140. if (!out) {
  1141. out = receiveWritable(channel);
  1142. if (out) {
  1143. int32_t mult = multiplier[channel];
  1144. if (mult != 65536) applyGain(out->data, mult);
  1145. }
  1146. } else {
  1147. in = receiveReadOnly(channel);
  1148. if (in) {
  1149. applyGainThenAdd(out->data, in->data, multiplier[channel]);
  1150. release(in);
  1151. }
  1152. }
  1153. }
  1154. if (out) {
  1155. transmit(out);
  1156. release(out);
  1157. }
  1158. }
  1159. /******************************************************************/
  1160. void AudioFilterBiquad::update(void)
  1161. {
  1162. audio_block_t *block;
  1163. int32_t a0, a1, a2, b1, b2, sum;
  1164. uint32_t in2, out2, aprev, bprev, flag;
  1165. uint32_t *data, *end;
  1166. int32_t *state;
  1167. block = receiveWritable();
  1168. if (!block) return;
  1169. data = (uint32_t *)(block->data);
  1170. end = data + AUDIO_BLOCK_SAMPLES/2;
  1171. state = (int32_t *)definition;
  1172. do {
  1173. a0 = *state++;
  1174. a1 = *state++;
  1175. a2 = *state++;
  1176. b1 = *state++;
  1177. b2 = *state++;
  1178. aprev = *state++;
  1179. bprev = *state++;
  1180. sum = *state & 0x3FFF;
  1181. do {
  1182. in2 = *data;
  1183. sum = signed_multiply_accumulate_32x16b(sum, a0, in2);
  1184. sum = signed_multiply_accumulate_32x16t(sum, a1, aprev);
  1185. sum = signed_multiply_accumulate_32x16b(sum, a2, aprev);
  1186. sum = signed_multiply_accumulate_32x16t(sum, b1, bprev);
  1187. sum = signed_multiply_accumulate_32x16b(sum, b2, bprev);
  1188. out2 = (uint32_t)sum >> 14;
  1189. sum &= 0x3FFF;
  1190. sum = signed_multiply_accumulate_32x16t(sum, a0, in2);
  1191. sum = signed_multiply_accumulate_32x16b(sum, a1, in2);
  1192. sum = signed_multiply_accumulate_32x16t(sum, a2, aprev);
  1193. sum = signed_multiply_accumulate_32x16b(sum, b1, out2);
  1194. sum = signed_multiply_accumulate_32x16t(sum, b2, bprev);
  1195. aprev = in2;
  1196. bprev = pack_16x16(sum >> 14, out2);
  1197. sum &= 0x3FFF;
  1198. aprev = in2;
  1199. *data++ = bprev;
  1200. } while (data < end);
  1201. flag = *state & 0x80000000;
  1202. *state++ = sum | flag;
  1203. *(state-2) = bprev;
  1204. *(state-3) = aprev;
  1205. } while (flag);
  1206. transmit(block);
  1207. release(block);
  1208. }
  1209. void AudioFilterBiquad::updateCoefs(int *source, bool doReset)
  1210. {
  1211. int32_t *dest=(int32_t *)definition;
  1212. int32_t *src=(int32_t *)source;
  1213. __disable_irq();
  1214. for(uint8_t index=0;index<5;index++)
  1215. {
  1216. *dest++=*src++;
  1217. }
  1218. if(doReset)
  1219. {
  1220. *dest++=0;
  1221. *dest++=0;
  1222. *dest++=0;
  1223. }
  1224. __enable_irq();
  1225. }
  1226. void AudioFilterBiquad::updateCoefs(int *source)
  1227. {
  1228. updateCoefs(source,false);
  1229. }
  1230. /******************************************************************/
  1231. extern "C" {
  1232. extern const int16_t fader_table[256];
  1233. };
  1234. void AudioEffectFade::update(void)
  1235. {
  1236. audio_block_t *block;
  1237. uint32_t i, pos, inc, index, scale;
  1238. int32_t val1, val2, val, sample;
  1239. uint8_t dir;
  1240. pos = position;
  1241. if (pos == 0) {
  1242. // output is silent
  1243. block = receiveReadOnly();
  1244. if (block) release(block);
  1245. return;
  1246. } else if (pos == 0xFFFFFFFF) {
  1247. // output is 100%
  1248. block = receiveReadOnly();
  1249. if (!block) return;
  1250. transmit(block);
  1251. release(block);
  1252. return;
  1253. }
  1254. block = receiveWritable();
  1255. if (!block) return;
  1256. inc = rate;
  1257. dir = direction;
  1258. for (i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
  1259. index = pos >> 24;
  1260. val1 = fader_table[index];
  1261. val2 = fader_table[index+1];
  1262. scale = (pos >> 8) & 0xFFFF;
  1263. val2 *= scale;
  1264. val1 *= 0x10000 - scale;
  1265. val = (val1 + val2) >> 16;
  1266. sample = block->data[i];
  1267. sample = (sample * val) >> 15;
  1268. block->data[i] = sample;
  1269. if (dir > 0) {
  1270. // output is increasing
  1271. if (inc < 0xFFFFFFFF - pos) pos += inc;
  1272. else pos = 0xFFFFFFFF;
  1273. } else {
  1274. // output is decreasing
  1275. if (inc < pos) pos -= inc;
  1276. else pos = 0;
  1277. }
  1278. }
  1279. position = pos;
  1280. transmit(block);
  1281. release(block);
  1282. }
  1283. void AudioEffectFade::fadeBegin(uint32_t newrate, uint8_t dir)
  1284. {
  1285. __disable_irq();
  1286. uint32_t pos = position;
  1287. if (pos == 0) position = 1;
  1288. else if (pos == 0xFFFFFFFF) position = 0xFFFFFFFE;
  1289. rate = newrate;
  1290. direction = dir;
  1291. __enable_irq();
  1292. }
  1293. /******************************************************************/
  1294. static inline int32_t multiply_32x32_rshift30(int32_t a, int32_t b) __attribute__((always_inline));
  1295. static inline int32_t multiply_32x32_rshift30(int32_t a, int32_t b)
  1296. {
  1297. return ((int64_t)a * (int64_t)b) >> 30;
  1298. }
  1299. //#define TONE_DETECT_FAST
  1300. void AudioAnalyzeToneDetect::update(void)
  1301. {
  1302. audio_block_t *block;
  1303. int32_t q0, q1, q2, coef;
  1304. const int16_t *p, *end;
  1305. uint16_t n;
  1306. block = receiveReadOnly();
  1307. if (!block) return;
  1308. if (!enabled) {
  1309. release(block);
  1310. return;
  1311. }
  1312. p = block->data;
  1313. end = p + AUDIO_BLOCK_SAMPLES;
  1314. n = count;
  1315. coef = coefficient;
  1316. q1 = s1;
  1317. q2 = s2;
  1318. do {
  1319. // the Goertzel algorithm is kinda magical ;-)
  1320. #ifdef TONE_DETECT_FAST
  1321. q0 = (*p++) + (multiply_32x32_rshift32_rounded(coef, q1) << 2) - q2;
  1322. #else
  1323. q0 = (*p++) + multiply_32x32_rshift30(coef, q1) - q2;
  1324. // TODO: is this only 1 cycle slower? if so, always use it
  1325. #endif
  1326. q2 = q1;
  1327. q1 = q0;
  1328. if (--n == 0) {
  1329. out1 = q1;
  1330. out2 = q2;
  1331. q1 = 0; // TODO: does clearing these help or hinder?
  1332. q2 = 0;
  1333. new_output = true;
  1334. n = length;
  1335. }
  1336. } while (p < end);
  1337. count = n;
  1338. s1 = q1;
  1339. s2 = q2;
  1340. release(block);
  1341. }
  1342. void AudioAnalyzeToneDetect::set_params(int32_t coef, uint16_t cycles, uint16_t len)
  1343. {
  1344. __disable_irq();
  1345. coefficient = coef;
  1346. ncycles = cycles;
  1347. length = len;
  1348. count = len;
  1349. s1 = 0;
  1350. s2 = 0;
  1351. enabled = true;
  1352. __enable_irq();
  1353. //Serial.printf("Tone: coef=%d, ncycles=%d, length=%d\n", coefficient, ncycles, length);
  1354. }
  1355. float AudioAnalyzeToneDetect::read(void)
  1356. {
  1357. int32_t coef, q1, q2, power;
  1358. uint16_t len;
  1359. __disable_irq();
  1360. coef = coefficient;
  1361. q1 = out1;
  1362. q2 = out2;
  1363. len = length;
  1364. __enable_irq();
  1365. #ifdef TONE_DETECT_FAST
  1366. power = multiply_32x32_rshift32_rounded(q2, q2);
  1367. power = multiply_accumulate_32x32_rshift32_rounded(power, q1, q1);
  1368. power = multiply_subtract_32x32_rshift32_rounded(power,
  1369. multiply_32x32_rshift30(q1, q2), coef);
  1370. power <<= 4;
  1371. #else
  1372. int64_t power64;
  1373. power64 = (int64_t)q2 * (int64_t)q2;
  1374. power64 += (int64_t)q1 * (int64_t)q1;
  1375. power64 -= (((int64_t)q1 * (int64_t)q2) >> 30) * (int64_t)coef;
  1376. power = power64 >> 28;
  1377. #endif
  1378. return sqrtf((float)power) / (float)len;
  1379. }
  1380. AudioAnalyzeToneDetect::operator bool()
  1381. {
  1382. int32_t coef, q1, q2, power, trigger;
  1383. uint16_t len;
  1384. __disable_irq();
  1385. coef = coefficient;
  1386. q1 = out1;
  1387. q2 = out2;
  1388. len = length;
  1389. __enable_irq();
  1390. #ifdef TONE_DETECT_FAST
  1391. power = multiply_32x32_rshift32_rounded(q2, q2);
  1392. power = multiply_accumulate_32x32_rshift32_rounded(power, q1, q1);
  1393. power = multiply_subtract_32x32_rshift32_rounded(power,
  1394. multiply_32x32_rshift30(q1, q2), coef);
  1395. power <<= 4;
  1396. #else
  1397. int64_t power64;
  1398. power64 = (int64_t)q2 * (int64_t)q2;
  1399. power64 += (int64_t)q1 * (int64_t)q1;
  1400. power64 -= (((int64_t)q1 * (int64_t)q2) >> 30) * (int64_t)coef;
  1401. power = power64 >> 28;
  1402. #endif
  1403. trigger = (uint32_t)len * thresh;
  1404. trigger = multiply_32x32_rshift32(trigger, trigger);
  1405. Serial.printf("bool: power=%d, trig=%d\n", power, trigger);
  1406. return (power >= trigger);
  1407. }
  1408. /******************************************************************/
  1409. #include "Wire.h"
  1410. #define WM8731_I2C_ADDR 0x1A
  1411. //#define WM8731_I2C_ADDR 0x1B
  1412. #define WM8731_REG_LLINEIN 0
  1413. #define WM8731_REG_RLINEIN 1
  1414. #define WM8731_REG_LHEADOUT 2
  1415. #define WM8731_REG_RHEADOUT 3
  1416. #define WM8731_REG_ANALOG 4
  1417. #define WM8731_REG_DIGITAL 5
  1418. #define WM8731_REG_POWERDOWN 6
  1419. #define WM8731_REG_INTERFACE 7
  1420. #define WM8731_REG_SAMPLING 8
  1421. #define WM8731_REG_ACTIVE 9
  1422. #define WM8731_REG_RESET 15
  1423. bool AudioControlWM8731::enable(void)
  1424. {
  1425. Wire.begin();
  1426. delay(5);
  1427. //write(WM8731_REG_RESET, 0);
  1428. write(WM8731_REG_INTERFACE, 0x02); // I2S, 16 bit, MCLK slave
  1429. write(WM8731_REG_SAMPLING, 0x20); // 256*Fs, 44.1 kHz, MCLK/1
  1430. // In order to prevent pops, the DAC should first be soft-muted (DACMU),
  1431. // the output should then be de-selected from the line and headphone output
  1432. // (DACSEL), then the DAC powered down (DACPD).
  1433. write(WM8731_REG_DIGITAL, 0x08); // DAC soft mute
  1434. write(WM8731_REG_ANALOG, 0x00); // disable all
  1435. write(WM8731_REG_POWERDOWN, 0x00); // codec powerdown
  1436. write(WM8731_REG_LHEADOUT, 0x80); // volume off
  1437. write(WM8731_REG_RHEADOUT, 0x80);
  1438. delay(100); // how long to power up?
  1439. write(WM8731_REG_ACTIVE, 1);
  1440. delay(5);
  1441. write(WM8731_REG_DIGITAL, 0x00); // DAC unmuted
  1442. write(WM8731_REG_ANALOG, 0x10); // DAC selected
  1443. return true;
  1444. }
  1445. bool AudioControlWM8731::write(unsigned int reg, unsigned int val)
  1446. {
  1447. Wire.beginTransmission(WM8731_I2C_ADDR);
  1448. Wire.write((reg << 1) | ((val >> 8) & 1));
  1449. Wire.write(val & 0xFF);
  1450. Wire.endTransmission();
  1451. return true;
  1452. }
  1453. bool AudioControlWM8731::volumeInteger(unsigned int n)
  1454. {
  1455. if (n > 127) n = 127;
  1456. //Serial.print("volumeInteger, n = ");
  1457. //Serial.println(n);
  1458. write(WM8731_REG_LHEADOUT, n | 0x180);
  1459. write(WM8731_REG_RHEADOUT, n | 0x80);
  1460. return true;
  1461. }
  1462. /******************************************************************/
  1463. bool AudioControlWM8731master::enable(void)
  1464. {
  1465. Wire.begin();
  1466. delay(5);
  1467. //write(WM8731_REG_RESET, 0);
  1468. write(WM8731_REG_INTERFACE, 0x42); // I2S, 16 bit, MCLK master
  1469. write(WM8731_REG_SAMPLING, 0x20); // 256*Fs, 44.1 kHz, MCLK/1
  1470. // In order to prevent pops, the DAC should first be soft-muted (DACMU),
  1471. // the output should then be de-selected from the line and headphone output
  1472. // (DACSEL), then the DAC powered down (DACPD).
  1473. write(WM8731_REG_DIGITAL, 0x08); // DAC soft mute
  1474. write(WM8731_REG_ANALOG, 0x00); // disable all
  1475. write(WM8731_REG_POWERDOWN, 0x00); // codec powerdown
  1476. write(WM8731_REG_LHEADOUT, 0x80); // volume off
  1477. write(WM8731_REG_RHEADOUT, 0x80);
  1478. delay(100); // how long to power up?
  1479. write(WM8731_REG_ACTIVE, 1);
  1480. delay(5);
  1481. write(WM8731_REG_DIGITAL, 0x00); // DAC unmuted
  1482. write(WM8731_REG_ANALOG, 0x10); // DAC selected
  1483. return true;
  1484. }
  1485. /******************************************************************/
  1486. #define CHIP_ID 0x0000
  1487. // 15:8 PARTID 0xA0 - 8 bit identifier for SGTL5000
  1488. // 7:0 REVID 0x00 - revision number for SGTL5000.
  1489. #define CHIP_DIG_POWER 0x0002
  1490. // 6 ADC_POWERUP 1=Enable, 0=disable the ADC block, both digital & analog,
  1491. // 5 DAC_POWERUP 1=Enable, 0=disable the DAC block, both analog and digital
  1492. // 4 DAP_POWERUP 1=Enable, 0=disable the DAP block
  1493. // 1 I2S_OUT_POWERUP 1=Enable, 0=disable the I2S data output
  1494. // 0 I2S_IN_POWERUP 1=Enable, 0=disable the I2S data input
  1495. #define CHIP_CLK_CTRL 0x0004
  1496. // 5:4 RATE_MODE Sets the sample rate mode. MCLK_FREQ is still specified
  1497. // relative to the rate in SYS_FS
  1498. // 0x0 = SYS_FS specifies the rate
  1499. // 0x1 = Rate is 1/2 of the SYS_FS rate
  1500. // 0x2 = Rate is 1/4 of the SYS_FS rate
  1501. // 0x3 = Rate is 1/6 of the SYS_FS rate
  1502. // 3:2 SYS_FS Sets the internal system sample rate (default=2)
  1503. // 0x0 = 32 kHz
  1504. // 0x1 = 44.1 kHz
  1505. // 0x2 = 48 kHz
  1506. // 0x3 = 96 kHz
  1507. // 1:0 MCLK_FREQ Identifies incoming SYS_MCLK frequency and if the PLL should be used
  1508. // 0x0 = 256*Fs
  1509. // 0x1 = 384*Fs
  1510. // 0x2 = 512*Fs
  1511. // 0x3 = Use PLL
  1512. // The 0x3 (Use PLL) setting must be used if the SYS_MCLK is not
  1513. // a standard multiple of Fs (256, 384, or 512). This setting can
  1514. // also be used if SYS_MCLK is a standard multiple of Fs.
  1515. // Before this field is set to 0x3 (Use PLL), the PLL must be
  1516. // powered up by setting CHIP_ANA_POWER->PLL_POWERUP and
  1517. // CHIP_ANA_POWER->VCOAMP_POWERUP. Also, the PLL dividers must
  1518. // be calculated based on the external MCLK rate and
  1519. // CHIP_PLL_CTRL register must be set (see CHIP_PLL_CTRL register
  1520. // description details on how to calculate the divisors).
  1521. #define CHIP_I2S_CTRL 0x0006
  1522. // 8 SCLKFREQ Sets frequency of I2S_SCLK when in master mode (MS=1). When in slave
  1523. // mode (MS=0), this field must be set appropriately to match SCLK input
  1524. // rate.
  1525. // 0x0 = 64Fs
  1526. // 0x1 = 32Fs - Not supported for RJ mode (I2S_MODE = 1)
  1527. // 7 MS Configures master or slave of I2S_LRCLK and I2S_SCLK.
  1528. // 0x0 = Slave: I2S_LRCLK an I2S_SCLK are inputs
  1529. // 0x1 = Master: I2S_LRCLK and I2S_SCLK are outputs
  1530. // NOTE: If the PLL is used (CHIP_CLK_CTRL->MCLK_FREQ==0x3),
  1531. // the SGTL5000 must be a master of the I2S port (MS==1)
  1532. // 6 SCLK_INV Sets the edge that data (input and output) is clocked in on for I2S_SCLK
  1533. // 0x0 = data is valid on rising edge of I2S_SCLK
  1534. // 0x1 = data is valid on falling edge of I2S_SCLK
  1535. // 5:4 DLEN I2S data length (default=1)
  1536. // 0x0 = 32 bits (only valid when SCLKFREQ=0),
  1537. // not valid for Right Justified Mode
  1538. // 0x1 = 24 bits (only valid when SCLKFREQ=0)
  1539. // 0x2 = 20 bits
  1540. // 0x3 = 16 bits
  1541. // 3:2 I2S_MODE Sets the mode for the I2S port
  1542. // 0x0 = I2S mode or Left Justified (Use LRALIGN to select)
  1543. // 0x1 = Right Justified Mode
  1544. // 0x2 = PCM Format A/B
  1545. // 0x3 = RESERVED
  1546. // 1 LRALIGN I2S_LRCLK Alignment to data word. Not used for Right Justified mode
  1547. // 0x0 = Data word starts 1 I2S_SCLK delay after I2S_LRCLK
  1548. // transition (I2S format, PCM format A)
  1549. // 0x1 = Data word starts after I2S_LRCLK transition (left
  1550. // justified format, PCM format B)
  1551. // 0 LRPOL I2S_LRCLK Polarity when data is presented.
  1552. // 0x0 = I2S_LRCLK = 0 - Left, 1 - Right
  1553. // 1x0 = I2S_LRCLK = 0 - Right, 1 - Left
  1554. // The left subframe should be presented first regardless of
  1555. // the setting of LRPOL.
  1556. #define CHIP_SSS_CTRL 0x000A
  1557. // 14 DAP_MIX_LRSWAP DAP Mixer Input Swap
  1558. // 0x0 = Normal Operation
  1559. // 0x1 = Left and Right channels for the DAP MIXER Input are swapped.
  1560. // 13 DAP_LRSWAP DAP Mixer Input Swap
  1561. // 0x0 = Normal Operation
  1562. // 0x1 = Left and Right channels for the DAP Input are swapped
  1563. // 12 DAC_LRSWAP DAC Input Swap
  1564. // 0x0 = Normal Operation
  1565. // 0x1 = Left and Right channels for the DAC are swapped
  1566. // 10 I2S_LRSWAP I2S_DOUT Swap
  1567. // 0x0 = Normal Operation
  1568. // 0x1 = Left and Right channels for the I2S_DOUT are swapped
  1569. // 9:8 DAP_MIX_SELECT Select data source for DAP mixer
  1570. // 0x0 = ADC
  1571. // 0x1 = I2S_IN
  1572. // 0x2 = Reserved
  1573. // 0x3 = Reserved
  1574. // 7:6 DAP_SELECT Select data source for DAP
  1575. // 0x0 = ADC
  1576. // 0x1 = I2S_IN
  1577. // 0x2 = Reserved
  1578. // 0x3 = Reserved
  1579. // 5:4 DAC_SELECT Select data source for DAC (default=1)
  1580. // 0x0 = ADC
  1581. // 0x1 = I2S_IN
  1582. // 0x2 = Reserved
  1583. // 0x3 = DAP
  1584. // 1:0 I2S_SELECT Select data source for I2S_DOUT
  1585. // 0x0 = ADC
  1586. // 0x1 = I2S_IN
  1587. // 0x2 = Reserved
  1588. // 0x3 = DAP
  1589. #define CHIP_ADCDAC_CTRL 0x000E
  1590. // 13 VOL_BUSY_DAC_RIGHT Volume Busy DAC Right
  1591. // 0x0 = Ready
  1592. // 0x1 = Busy - This indicates the channel has not reached its
  1593. // programmed volume/mute level
  1594. // 12 VOL_BUSY_DAC_LEFT Volume Busy DAC Left
  1595. // 0x0 = Ready
  1596. // 0x1 = Busy - This indicates the channel has not reached its
  1597. // programmed volume/mute level
  1598. // 9 VOL_RAMP_EN Volume Ramp Enable (default=1)
  1599. // 0x0 = Disables volume ramp. New volume settings take immediate
  1600. // effect without a ramp
  1601. // 0x1 = Enables volume ramp
  1602. // This field affects DAC_VOL. The volume ramp effects both
  1603. // volume settings and mute When set to 1 a soft mute is enabled.
  1604. // 8 VOL_EXPO_RAMP Exponential Volume Ramp Enable
  1605. // 0x0 = Linear ramp over top 4 volume octaves
  1606. // 0x1 = Exponential ramp over full volume range
  1607. // This bit only takes effect if VOL_RAMP_EN is 1.
  1608. // 3 DAC_MUTE_RIGHT DAC Right Mute (default=1)
  1609. // 0x0 = Unmute
  1610. // 0x1 = Muted
  1611. // If VOL_RAMP_EN = 1, this is a soft mute.
  1612. // 2 DAC_MUTE_LEFT DAC Left Mute (default=1)
  1613. // 0x0 = Unmute
  1614. // 0x1 = Muted
  1615. // If VOL_RAMP_EN = 1, this is a soft mute.
  1616. // 1 ADC_HPF_FREEZE ADC High Pass Filter Freeze
  1617. // 0x0 = Normal operation
  1618. // 0x1 = Freeze the ADC high-pass filter offset register. The
  1619. // offset continues to be subtracted from the ADC data stream.
  1620. // 0 ADC_HPF_BYPASS ADC High Pass Filter Bypass
  1621. // 0x0 = Normal operation
  1622. // 0x1 = Bypassed and offset not updated
  1623. #define CHIP_DAC_VOL 0x0010
  1624. // 15:8 DAC_VOL_RIGHT DAC Right Channel Volume. Set the Right channel DAC volume
  1625. // with 0.5017 dB steps from 0 to -90 dB
  1626. // 0x3B and less = Reserved
  1627. // 0x3C = 0 dB
  1628. // 0x3D = -0.5 dB
  1629. // 0xF0 = -90 dB
  1630. // 0xFC and greater = Muted
  1631. // If VOL_RAMP_EN = 1, there is an automatic ramp to the
  1632. // new volume setting.
  1633. // 7:0 DAC_VOL_LEFT DAC Left Channel Volume. Set the Left channel DAC volume
  1634. // with 0.5017 dB steps from 0 to -90 dB
  1635. // 0x3B and less = Reserved
  1636. // 0x3C = 0 dB
  1637. // 0x3D = -0.5 dB
  1638. // 0xF0 = -90 dB
  1639. // 0xFC and greater = Muted
  1640. // If VOL_RAMP_EN = 1, there is an automatic ramp to the
  1641. // new volume setting.
  1642. #define CHIP_PAD_STRENGTH 0x0014
  1643. // 9:8 I2S_LRCLK I2S LRCLK Pad Drive Strength (default=1)
  1644. // Sets drive strength for output pads per the table below.
  1645. // VDDIO 1.8 V 2.5 V 3.3 V
  1646. // 0x0 = Disable
  1647. // 0x1 = 1.66 mA 2.87 mA 4.02 mA
  1648. // 0x2 = 3.33 mA 5.74 mA 8.03 mA
  1649. // 0x3 = 4.99 mA 8.61 mA 12.05 mA
  1650. // 7:6 I2S_SCLK I2S SCLK Pad Drive Strength (default=1)
  1651. // 5:4 I2S_DOUT I2S DOUT Pad Drive Strength (default=1)
  1652. // 3:2 CTRL_DATA I2C DATA Pad Drive Strength (default=3)
  1653. // 1:0 CTRL_CLK I2C CLK Pad Drive Strength (default=3)
  1654. // (all use same table as I2S_LRCLK)
  1655. #define CHIP_ANA_ADC_CTRL 0x0020
  1656. // 8 ADC_VOL_M6DB ADC Volume Range Reduction
  1657. // This bit shifts both right and left analog ADC volume
  1658. // range down by 6.0 dB.
  1659. // 0x0 = No change in ADC range
  1660. // 0x1 = ADC range reduced by 6.0 dB
  1661. // 7:4 ADC_VOL_RIGHT ADC Right Channel Volume
  1662. // Right channel analog ADC volume control in 1.5 dB steps.
  1663. // 0x0 = 0 dB
  1664. // 0x1 = +1.5 dB
  1665. // ...
  1666. // 0xF = +22.5 dB
  1667. // This range is -6.0 dB to +16.5 dB if ADC_VOL_M6DB is set to 1.
  1668. // 3:0 ADC_VOL_LEFT ADC Left Channel Volume
  1669. // (same scale as ADC_VOL_RIGHT)
  1670. #define CHIP_ANA_HP_CTRL 0x0022
  1671. // 14:8 HP_VOL_RIGHT Headphone Right Channel Volume (default 0x18)
  1672. // Right channel headphone volume control with 0.5 dB steps.
  1673. // 0x00 = +12 dB
  1674. // 0x01 = +11.5 dB
  1675. // 0x18 = 0 dB
  1676. // ...
  1677. // 0x7F = -51.5 dB
  1678. // 6:0 HP_VOL_LEFT Headphone Left Channel Volume (default 0x18)
  1679. // (same scale as HP_VOL_RIGHT)
  1680. #define CHIP_ANA_CTRL 0x0024
  1681. // 8 MUTE_LO LINEOUT Mute, 0 = Unmute, 1 = Mute (default 1)
  1682. // 6 SELECT_HP Select the headphone input, 0 = DAC, 1 = LINEIN
  1683. // 5 EN_ZCD_HP Enable the headphone zero cross detector (ZCD)
  1684. // 0x0 = HP ZCD disabled
  1685. // 0x1 = HP ZCD enabled
  1686. // 4 MUTE_HP Mute the headphone outputs, 0 = Unmute, 1 = Mute (default)
  1687. // 2 SELECT_ADC Select the ADC input, 0 = Microphone, 1 = LINEIN
  1688. // 1 EN_ZCD_ADC Enable the ADC analog zero cross detector (ZCD)
  1689. // 0x0 = ADC ZCD disabled
  1690. // 0x1 = ADC ZCD enabled
  1691. // 0 MUTE_ADC Mute the ADC analog volume, 0 = Unmute, 1 = Mute (default)
  1692. #define CHIP_LINREG_CTRL 0x0026
  1693. // 6 VDDC_MAN_ASSN Determines chargepump source when VDDC_ASSN_OVRD is set.
  1694. // 0x0 = VDDA
  1695. // 0x1 = VDDIO
  1696. // 5 VDDC_ASSN_OVRD Charge pump Source Assignment Override
  1697. // 0x0 = Charge pump source is automatically assigned based
  1698. // on higher of VDDA and VDDIO
  1699. // 0x1 = the source of charge pump is manually assigned by
  1700. // VDDC_MAN_ASSN If VDDIO and VDDA are both the same
  1701. // and greater than 3.1 V, VDDC_ASSN_OVRD and
  1702. // VDDC_MAN_ASSN should be used to manually assign
  1703. // VDDIO as the source for charge pump.
  1704. // 3:0 D_PROGRAMMING Sets the VDDD linear regulator output voltage in 50 mV steps.
  1705. // Must clear the LINREG_SIMPLE_POWERUP and STARTUP_POWERUP bits
  1706. // in the 0x0030 (CHIP_ANA_POWER) register after power-up, for
  1707. // this setting to produce the proper VDDD voltage.
  1708. // 0x0 = 1.60
  1709. // 0xF = 0.85
  1710. #define CHIP_REF_CTRL 0x0028 // bandgap reference bias voltage and currents
  1711. // 8:4 VAG_VAL Analog Ground Voltage Control
  1712. // These bits control the analog ground voltage in 25 mV steps.
  1713. // This should usually be set to VDDA/2 or lower for best
  1714. // performance (maximum output swing at minimum THD). This VAG
  1715. // reference is also used for the DAC and ADC voltage reference.
  1716. // So changing this voltage scales the output swing of the DAC
  1717. // and the output signal of the ADC.
  1718. // 0x00 = 0.800 V
  1719. // 0x1F = 1.575 V
  1720. // 3:1 BIAS_CTRL Bias control
  1721. // These bits adjust the bias currents for all of the analog
  1722. // blocks. By lowering the bias current a lower quiescent power
  1723. // is achieved. It should be noted that this mode can affect
  1724. // performance by 3-4 dB.
  1725. // 0x0 = Nominal
  1726. // 0x1-0x3=+12.5%
  1727. // 0x4=-12.5%
  1728. // 0x5=-25%
  1729. // 0x6=-37.5%
  1730. // 0x7=-50%
  1731. // 0 SMALL_POP VAG Ramp Control
  1732. // Setting this bit slows down the VAG ramp from ~200 to ~400 ms
  1733. // to reduce the startup pop, but increases the turn on/off time.
  1734. // 0x0 = Normal VAG ramp
  1735. // 0x1 = Slow down VAG ramp
  1736. #define CHIP_MIC_CTRL 0x002A // microphone gain & internal microphone bias
  1737. // 9:8 BIAS_RESISTOR MIC Bias Output Impedance Adjustment
  1738. // Controls an adjustable output impedance for the microphone bias.
  1739. // If this is set to zero the micbias block is powered off and
  1740. // the output is highZ.
  1741. // 0x0 = Powered off
  1742. // 0x1 = 2.0 kohm
  1743. // 0x2 = 4.0 kohm
  1744. // 0x3 = 8.0 kohm
  1745. // 6:4 BIAS_VOLT MIC Bias Voltage Adjustment
  1746. // Controls an adjustable bias voltage for the microphone bias
  1747. // amp in 250 mV steps. This bias voltage setting should be no
  1748. // more than VDDA-200 mV for adequate power supply rejection.
  1749. // 0x0 = 1.25 V
  1750. // ...
  1751. // 0x7 = 3.00 V
  1752. // 1:0 GAIN MIC Amplifier Gain
  1753. // Sets the microphone amplifier gain. At 0 dB setting the THD
  1754. // can be slightly higher than other paths- typically around
  1755. // ~65 dB. At other gain settings the THD are better.
  1756. // 0x0 = 0 dB
  1757. // 0x1 = +20 dB
  1758. // 0x2 = +30 dB
  1759. // 0x3 = +40 dB
  1760. #define CHIP_LINE_OUT_CTRL 0x002C
  1761. // 11:8 OUT_CURRENT Controls the output bias current for the LINEOUT amplifiers. The
  1762. // nominal recommended setting for a 10 kohm load with 1.0 nF load cap
  1763. // is 0x3. There are only 5 valid settings.
  1764. // 0x0=0.18 mA
  1765. // 0x1=0.27 mA
  1766. // 0x3=0.36 mA
  1767. // 0x7=0.45 mA
  1768. // 0xF=0.54 mA
  1769. // 5:0 LO_VAGCNTRL LINEOUT Amplifier Analog Ground Voltage
  1770. // Controls the analog ground voltage for the LINEOUT amplifiers
  1771. // in 25 mV steps. This should usually be set to VDDIO/2.
  1772. // 0x00 = 0.800 V
  1773. // ...
  1774. // 0x1F = 1.575 V
  1775. // ...
  1776. // 0x23 = 1.675 V
  1777. // 0x24-0x3F are invalid
  1778. #define CHIP_LINE_OUT_VOL 0x002E
  1779. // 12:8 LO_VOL_RIGHT LINEOUT Right Channel Volume (default=4)
  1780. // Controls the right channel LINEOUT volume in 0.5 dB steps.
  1781. // Higher codes have more attenuation.
  1782. // 4:0 LO_VOL_LEFT LINEOUT Left Channel Output Level (default=4)
  1783. // Used to normalize the output level of the left line output
  1784. // to full scale based on the values used to set
  1785. // LINE_OUT_CTRL->LO_VAGCNTRL and CHIP_REF_CTRL->VAG_VAL.
  1786. // In general this field should be set to:
  1787. // 40*log((VAG_VAL)/(LO_VAGCNTRL)) + 15
  1788. // Suggested values based on typical VDDIO and VDDA voltages.
  1789. // VDDA VAG_VAL VDDIO LO_VAGCNTRL LO_VOL_*
  1790. // 1.8 V 0.9 3.3 V 1.55 0x06
  1791. // 1.8 V 0.9 1.8 V 0.9 0x0F
  1792. // 3.3 V 1.55 1.8 V 0.9 0x19
  1793. // 3.3 V 1.55 3.3 V 1.55 0x0F
  1794. // After setting to the nominal voltage, this field can be used
  1795. // to adjust the output level in +/-0.5 dB increments by using
  1796. // values higher or lower than the nominal setting.
  1797. #define CHIP_ANA_POWER 0x0030 // power down controls for the analog blocks.
  1798. // The only other power-down controls are BIAS_RESISTOR in the MIC_CTRL register
  1799. // and the EN_ZCD control bits in ANA_CTRL.
  1800. // 14 DAC_MONO While DAC_POWERUP is set, this allows the DAC to be put into left only
  1801. // mono operation for power savings. 0=mono, 1=stereo (default)
  1802. // 13 LINREG_SIMPLE_POWERUP Power up the simple (low power) digital supply regulator.
  1803. // After reset, this bit can be cleared IF VDDD is driven
  1804. // externally OR the primary digital linreg is enabled with
  1805. // LINREG_D_POWERUP
  1806. // 12 STARTUP_POWERUP Power up the circuitry needed during the power up ramp and reset.
  1807. // After reset this bit can be cleared if VDDD is coming from
  1808. // an external source.
  1809. // 11 VDDC_CHRGPMP_POWERUP Power up the VDDC charge pump block. If neither VDDA or VDDIO
  1810. // is 3.0 V or larger this bit should be cleared before analog
  1811. // blocks are powered up.
  1812. // 10 PLL_POWERUP PLL Power Up, 0 = Power down, 1 = Power up
  1813. // When cleared, the PLL is turned off. This must be set before
  1814. // CHIP_CLK_CTRL->MCLK_FREQ is programmed to 0x3. The
  1815. // CHIP_PLL_CTRL register must be configured correctly before
  1816. // setting this bit.
  1817. // 9 LINREG_D_POWERUP Power up the primary VDDD linear regulator, 0 = Power down, 1 = Power up
  1818. // 8 VCOAMP_POWERUP Power up the PLL VCO amplifier, 0 = Power down, 1 = Power up
  1819. // 7 VAG_POWERUP Power up the VAG reference buffer.
  1820. // Setting this bit starts the power up ramp for the headphone
  1821. // and LINEOUT. The headphone (and/or LINEOUT) powerup should
  1822. // be set BEFORE clearing this bit. When this bit is cleared
  1823. // the power-down ramp is started. The headphone (and/or LINEOUT)
  1824. // powerup should stay set until the VAG is fully ramped down
  1825. // (200 to 400 ms after clearing this bit).
  1826. // 0x0 = Power down, 0x1 = Power up
  1827. // 6 ADC_MONO While ADC_POWERUP is set, this allows the ADC to be put into left only
  1828. // mono operation for power savings. This mode is useful when
  1829. // only using the microphone input.
  1830. // 0x0 = Mono (left only), 0x1 = Stereo
  1831. // 5 REFTOP_POWERUP Power up the reference bias currents
  1832. // 0x0 = Power down, 0x1 = Power up
  1833. // This bit can be cleared when the part is a sleep state
  1834. // to minimize analog power.
  1835. // 4 HEADPHONE_POWERUP Power up the headphone amplifiers
  1836. // 0x0 = Power down, 0x1 = Power up
  1837. // 3 DAC_POWERUP Power up the DACs
  1838. // 0x0 = Power down, 0x1 = Power up
  1839. // 2 CAPLESS_HEADPHONE_POWERUP Power up the capless headphone mode
  1840. // 0x0 = Power down, 0x1 = Power up
  1841. // 1 ADC_POWERUP Power up the ADCs
  1842. // 0x0 = Power down, 0x1 = Power up
  1843. // 0 LINEOUT_POWERUP Power up the LINEOUT amplifiers
  1844. // 0x0 = Power down, 0x1 = Power up
  1845. #define CHIP_PLL_CTRL 0x0032
  1846. // 15:11 INT_DIVISOR
  1847. // 10:0 FRAC_DIVISOR
  1848. #define CHIP_CLK_TOP_CTRL 0x0034
  1849. // 11 ENABLE_INT_OSC Setting this bit enables an internal oscillator to be used for the
  1850. // zero cross detectors, the short detect recovery, and the
  1851. // charge pump. This allows the I2S clock to be shut off while
  1852. // still operating an analog signal path. This bit can be kept
  1853. // on when the I2S clock is enabled, but the I2S clock is more
  1854. // accurate so it is preferred to clear this bit when I2S is present.
  1855. // 3 INPUT_FREQ_DIV2 SYS_MCLK divider before PLL input
  1856. // 0x0 = pass through
  1857. // 0x1 = SYS_MCLK is divided by 2 before entering PLL
  1858. // This must be set when the input clock is above 17 Mhz. This
  1859. // has no effect when the PLL is powered down.
  1860. #define CHIP_ANA_STATUS 0x0036
  1861. // 9 LRSHORT_STS This bit is high whenever a short is detected on the left or right
  1862. // channel headphone drivers.
  1863. // 8 CSHORT_STS This bit is high whenever a short is detected on the capless headphone
  1864. // common/center channel driver.
  1865. // 4 PLL_IS_LOCKED This bit goes high after the PLL is locked.
  1866. #define CHIP_ANA_TEST1 0x0038 // intended only for debug.
  1867. #define CHIP_ANA_TEST2 0x003A // intended only for debug.
  1868. #define CHIP_SHORT_CTRL 0x003C
  1869. // 14:12 LVLADJR Right channel headphone short detector in 25 mA steps.
  1870. // 0x3=25 mA
  1871. // 0x2=50 mA
  1872. // 0x1=75 mA
  1873. // 0x0=100 mA
  1874. // 0x4=125 mA
  1875. // 0x5=150 mA
  1876. // 0x6=175 mA
  1877. // 0x7=200 mA
  1878. // This trip point can vary by ~30% over process so leave plenty
  1879. // of guard band to avoid false trips. This short detect trip
  1880. // point is also effected by the bias current adjustments made
  1881. // by CHIP_REF_CTRL->BIAS_CTRL and by CHIP_ANA_TEST1->HP_IALL_ADJ.
  1882. // 10:8 LVLADJL Left channel headphone short detector in 25 mA steps.
  1883. // (same scale as LVLADJR)
  1884. // 6:4 LVLADJC Capless headphone center channel short detector in 50 mA steps.
  1885. // 0x3=50 mA
  1886. // 0x2=100 mA
  1887. // 0x1=150 mA
  1888. // 0x0=200 mA
  1889. // 0x4=250 mA
  1890. // 0x5=300 mA
  1891. // 0x6=350 mA
  1892. // 0x7=400 mA
  1893. // 3:2 MODE_LR Behavior of left/right short detection
  1894. // 0x0 = Disable short detector, reset short detect latch,
  1895. // software view non-latched short signal
  1896. // 0x1 = Enable short detector and reset the latch at timeout
  1897. // (every ~50 ms)
  1898. // 0x2 = This mode is not used/invalid
  1899. // 0x3 = Enable short detector with only manual reset (have
  1900. // to return to 0x0 to reset the latch)
  1901. // 1:0 MODE_CM Behavior of capless headphone central short detection
  1902. // (same settings as MODE_LR)
  1903. #define DAP_CONTROL 0x0100
  1904. #define DAP_PEQ 0x0102
  1905. #define DAP_BASS_ENHANCE 0x0104
  1906. #define DAP_BASS_ENHANCE_CTRL 0x0106
  1907. #define DAP_AUDIO_EQ 0x0108
  1908. #define DAP_SGTL_SURROUND 0x010A
  1909. #define DAP_FILTER_COEF_ACCESS 0x010C
  1910. #define DAP_COEF_WR_B0_MSB 0x010E
  1911. #define DAP_COEF_WR_B0_LSB 0x0110
  1912. #define DAP_AUDIO_EQ_BASS_BAND0 0x0116 // 115 Hz
  1913. #define DAP_AUDIO_EQ_BAND1 0x0118 // 330 Hz
  1914. #define DAP_AUDIO_EQ_BAND2 0x011A // 990 Hz
  1915. #define DAP_AUDIO_EQ_BAND3 0x011C // 3000 Hz
  1916. #define DAP_AUDIO_EQ_TREBLE_BAND4 0x011E // 9900 Hz
  1917. #define DAP_MAIN_CHAN 0x0120
  1918. #define DAP_MIX_CHAN 0x0122
  1919. #define DAP_AVC_CTRL 0x0124
  1920. #define DAP_AVC_THRESHOLD 0x0126
  1921. #define DAP_AVC_ATTACK 0x0128
  1922. #define DAP_AVC_DECAY 0x012A
  1923. #define DAP_COEF_WR_B1_MSB 0x012C
  1924. #define DAP_COEF_WR_B1_LSB 0x012E
  1925. #define DAP_COEF_WR_B2_MSB 0x0130
  1926. #define DAP_COEF_WR_B2_LSB 0x0132
  1927. #define DAP_COEF_WR_A1_MSB 0x0134
  1928. #define DAP_COEF_WR_A1_LSB 0x0136
  1929. #define DAP_COEF_WR_A2_MSB 0x0138
  1930. #define DAP_COEF_WR_A2_LSB 0x013A
  1931. #define SGTL5000_I2C_ADDR 0x0A // CTRL_ADR0_CS pin low (normal configuration)
  1932. //#define SGTL5000_I2C_ADDR 0x2A // CTRL_ADR0_CS pin high
  1933. bool AudioControlSGTL5000::enable(void)
  1934. {
  1935. //unsigned int n;
  1936. muted = true;
  1937. Wire.begin();
  1938. delay(5);
  1939. //Serial.print("chip ID = ");
  1940. //delay(5);
  1941. //n = read(CHIP_ID);
  1942. //Serial.println(n, HEX);
  1943. write(CHIP_ANA_POWER, 0x4060); // VDDD is externally driven with 1.8V
  1944. write(CHIP_LINREG_CTRL, 0x006C); // VDDA & VDDIO both over 3.1V
  1945. write(CHIP_REF_CTRL, 0x01F1); // VAG=1.575 slow ramp, normal bias current
  1946. write(CHIP_LINE_OUT_CTRL, 0x0322); // LO_VAGCNTRL=1.65V, OUT_CURRENT=0.36mA
  1947. write(CHIP_SHORT_CTRL, 0x4446); // allow up to 125mA
  1948. write(CHIP_ANA_CTRL, 0x0137); // enable zero cross detectors
  1949. write(CHIP_ANA_POWER, 0x40FF); // power up: lineout, hp, adc, dac
  1950. write(CHIP_DIG_POWER, 0x0073); // power up all digital stuff
  1951. delay(400);
  1952. // 40*log((1.575)/(1.65)) + 15 = 13.1391993746043 but it seems wrong, 5 is better...
  1953. write(CHIP_LINE_OUT_VOL, 0x0505); // TODO: correct value for 3.3V
  1954. write(CHIP_CLK_CTRL, 0x0004); // 44.1 kHz, 256*Fs
  1955. write(CHIP_I2S_CTRL, 0x0130); // SCLK=32*Fs, 16bit, I2S format
  1956. // default signal routing is ok?
  1957. write(CHIP_SSS_CTRL, 0x0010); // ADC->I2S, I2S->DAC
  1958. write(CHIP_ADCDAC_CTRL, 0x0000); // disable dac mute
  1959. write(CHIP_DAC_VOL, 0x3C3C); // digital gain, 0dB
  1960. write(CHIP_ANA_HP_CTRL, 0x7F7F); // set volume (lowest level)
  1961. write(CHIP_ANA_CTRL, 0x0136); // enable zero cross detectors
  1962. //mute = false;
  1963. return true;
  1964. }
  1965. unsigned int AudioControlSGTL5000::read(unsigned int reg)
  1966. {
  1967. unsigned int val;
  1968. Wire.beginTransmission(SGTL5000_I2C_ADDR);
  1969. Wire.write(reg >> 8);
  1970. Wire.write(reg);
  1971. if (Wire.endTransmission(false) != 0) return 0;
  1972. if (Wire.requestFrom(SGTL5000_I2C_ADDR, 2) < 2) return 0;
  1973. val = Wire.read() << 8;
  1974. val |= Wire.read();
  1975. return val;
  1976. }
  1977. bool AudioControlSGTL5000::write(unsigned int reg, unsigned int val)
  1978. {
  1979. if (reg == CHIP_ANA_CTRL) ana_ctrl = val;
  1980. Wire.beginTransmission(SGTL5000_I2C_ADDR);
  1981. Wire.write(reg >> 8);
  1982. Wire.write(reg);
  1983. Wire.write(val >> 8);
  1984. Wire.write(val);
  1985. if (Wire.endTransmission() == 0) return true;
  1986. return false;
  1987. }
  1988. unsigned int AudioControlSGTL5000::modify(unsigned int reg, unsigned int val, unsigned int iMask)
  1989. {
  1990. unsigned int val1 = (read(reg)&(~iMask))|val;
  1991. if(!write(reg,val1)) return 0;
  1992. return val1;
  1993. }
  1994. bool AudioControlSGTL5000::volumeInteger(unsigned int n)
  1995. {
  1996. if (n == 0) {
  1997. muted = true;
  1998. write(CHIP_ANA_HP_CTRL, 0x7F7F);
  1999. return muteHeadphone();
  2000. } else if (n > 0x80) {
  2001. n = 0;
  2002. } else {
  2003. n = 0x80 - n;
  2004. }
  2005. if (muted) {
  2006. muted = false;
  2007. unmuteHeadphone();
  2008. }
  2009. n = n | (n << 8);
  2010. return write(CHIP_ANA_HP_CTRL, n); // set volume
  2011. }
  2012. bool AudioControlSGTL5000::volume(float left, float right)
  2013. {
  2014. unsigned short m=((0x7F-calcVol(right,0x7F))<<8)|(0x7F-calcVol(left,0x7F));
  2015. return write(CHIP_ANA_HP_CTRL, m);
  2016. }
  2017. // CHIP_LINE_OUT_VOL
  2018. unsigned short AudioControlSGTL5000::lo_lvl(uint8_t n)
  2019. {
  2020. n&=31;
  2021. return modify(CHIP_LINE_OUT_VOL,(n<<8)|n,(31<<8)|31);
  2022. }
  2023. unsigned short AudioControlSGTL5000::lo_lvl(uint8_t left, uint8_t right)
  2024. {
  2025. left&=31;
  2026. right&=31;
  2027. return modify(CHIP_LINE_OUT_VOL,(right<<8)|left,(31<<8)|31);
  2028. }
  2029. unsigned short AudioControlSGTL5000::dac_vol(float n) // set both directly
  2030. {
  2031. if(read(CHIP_ADCDAC_CTRL)&(3<<2)!=((n>0 ? 0:3)<<2)) modify(CHIP_ADCDAC_CTRL,(n>0 ? 0:3)<<2,3<<2);
  2032. unsigned char m=calcVol(n,0xC0);
  2033. return modify(CHIP_DAC_VOL,((0xFC-m)<<8)|(0xFC-m),65535);
  2034. }
  2035. unsigned short AudioControlSGTL5000::dac_vol(float left, float right)
  2036. {
  2037. unsigned short adcdac=((right>0 ? 0:2)|(left>0 ? 0:1))<<2;
  2038. if(read(CHIP_ADCDAC_CTRL)&(3<<2)!=adcdac) modify(CHIP_ADCDAC_CTRL,adcdac,1<<2);
  2039. unsigned short m=(0xFC-calcVol(right,0xC0))<<8|(0xFC-calcVol(left,0xC0));
  2040. return modify(CHIP_DAC_VOL,m,65535);
  2041. }
  2042. // DAP_CONTROL
  2043. unsigned short AudioControlSGTL5000::dap_mix_enable(uint8_t n)
  2044. {
  2045. return modify(DAP_CONTROL,(n&1)<<4,1<<4);
  2046. }
  2047. unsigned short AudioControlSGTL5000::dap_enable(uint8_t n)
  2048. {
  2049. if(n) n=1;
  2050. unsigned char DAC=1+(2*n); // I2S_IN if n==0 else DAP
  2051. modify(DAP_CONTROL,n,1);
  2052. return modify(CHIP_SSS_CTRL,(0<<6)|(DAC<<4),(3<<6)|(3<<4));
  2053. }
  2054. unsigned short AudioControlSGTL5000::dap_enable(void)
  2055. {
  2056. return dap_enable(1);
  2057. }
  2058. // DAP_PEQ
  2059. unsigned short AudioControlSGTL5000::dap_peqs(uint8_t n) // valid to n&7, 0 thru 7 filters enabled.
  2060. {
  2061. return modify(DAP_PEQ,(n&7),7);
  2062. }
  2063. // DAP_AUDIO_EQ
  2064. unsigned short AudioControlSGTL5000::dap_audio_eq(uint8_t n) // 0=NONE, 1=PEQ (7 IIR Biquad filters), 2=TONE (tone), 3=GEQ (5 band EQ)
  2065. {
  2066. return modify(DAP_AUDIO_EQ,n&3,3);
  2067. }
  2068. /******************************************************************/
  2069. void AudioFilterFIR::begin(short *cp,int n_coeffs)
  2070. {
  2071. // pointer to coefficients
  2072. coeff_p = cp;
  2073. // Initialize FIR instances for the left and right channels
  2074. if(coeff_p && (coeff_p != FIR_PASSTHRU)) {
  2075. arm_fir_init_q15(&l_fir_inst, n_coeffs, coeff_p, &l_StateQ15[0], AUDIO_BLOCK_SAMPLES);
  2076. arm_fir_init_q15(&r_fir_inst, n_coeffs, coeff_p, &r_StateQ15[0], AUDIO_BLOCK_SAMPLES);
  2077. }
  2078. }
  2079. // This has the same effect as begin(NULL,0);
  2080. void AudioFilterFIR::stop(void)
  2081. {
  2082. coeff_p = NULL;
  2083. }
  2084. void AudioFilterFIR::update(void)
  2085. {
  2086. audio_block_t *block,*b_new;
  2087. // If there's no coefficient table, give up.
  2088. if(coeff_p == NULL)return;
  2089. // do passthru
  2090. if(coeff_p == FIR_PASSTHRU) {
  2091. // Just passthrough
  2092. block = receiveWritable(0);
  2093. if(block) {
  2094. transmit(block,0);
  2095. release(block);
  2096. }
  2097. block = receiveWritable(1);
  2098. if(block) {
  2099. transmit(block,1);
  2100. release(block);
  2101. }
  2102. return;
  2103. }
  2104. // Left Channel
  2105. block = receiveWritable(0);
  2106. // get a block for the FIR output
  2107. b_new = allocate();
  2108. if(block && b_new) {
  2109. arm_fir_q15(&l_fir_inst, (q15_t *)block->data, (q15_t *)b_new->data, AUDIO_BLOCK_SAMPLES);
  2110. // send the FIR output to the left channel
  2111. transmit(b_new,0);
  2112. }
  2113. if(block)release(block);
  2114. if(b_new)release(b_new);
  2115. // Right Channel
  2116. block = receiveWritable(1);
  2117. b_new = allocate();
  2118. if(block && b_new) {
  2119. arm_fir_q15(&r_fir_inst, (q15_t *)block->data, (q15_t *)b_new->data, AUDIO_BLOCK_SAMPLES);
  2120. transmit(b_new,1);
  2121. }
  2122. if(block)release(block);
  2123. if(b_new)release(b_new);
  2124. }
  2125. /******************************************************************/
  2126. // A u d i o E f f e c t F l a n g e
  2127. // Written by Pete (El Supremo) Jan 2014
  2128. // 140207 - fix calculation of delay_rate_incr which is expressed as
  2129. // a fraction of 2*PI
  2130. // 140207 - cosmetic fix to begin()
  2131. // circular addressing indices for left and right channels
  2132. short AudioEffectFlange::l_circ_idx;
  2133. short AudioEffectFlange::r_circ_idx;
  2134. short * AudioEffectFlange::l_delayline = NULL;
  2135. short * AudioEffectFlange::r_delayline = NULL;
  2136. // User-supplied offset for the delayed sample
  2137. // but start with passthru
  2138. int AudioEffectFlange::delay_offset_idx = DELAY_PASSTHRU;
  2139. int AudioEffectFlange::delay_length;
  2140. int AudioEffectFlange::delay_depth;
  2141. int AudioEffectFlange::delay_rate_incr;
  2142. unsigned int AudioEffectFlange::l_delay_rate_index;
  2143. unsigned int AudioEffectFlange::r_delay_rate_index;
  2144. // fails if the user provides unreasonable values but will
  2145. // coerce them and go ahead anyway. e.g. if the delay offset
  2146. // is >= CHORUS_DELAY_LENGTH, the code will force it to
  2147. // CHORUS_DELAY_LENGTH-1 and return false.
  2148. // delay_rate is the rate (in Hz) of the sine wave modulation
  2149. // delay_depth is the maximum variation around delay_offset
  2150. // i.e. the total offset is delay_offset + delay_depth * sin(delay_rate)
  2151. boolean AudioEffectFlange::begin(short *delayline,int d_length,int delay_offset,int d_depth,float delay_rate)
  2152. {
  2153. boolean all_ok = true;
  2154. if(0) {
  2155. Serial.print("AudioEffectFlange.begin(offset = ");
  2156. Serial.print(delay_offset);
  2157. Serial.print(", depth = ");
  2158. Serial.print(d_depth);
  2159. Serial.print(", rate = ");
  2160. Serial.print(delay_rate,3);
  2161. Serial.println(")");
  2162. Serial.print(" FLANGE_DELAY_LENGTH = ");
  2163. Serial.println(d_length);
  2164. }
  2165. delay_length = d_length/2;
  2166. l_delayline = delayline;
  2167. r_delayline = delayline + delay_length;
  2168. delay_depth = d_depth;
  2169. // initial index
  2170. l_delay_rate_index = 0;
  2171. r_delay_rate_index = 0;
  2172. l_circ_idx = 0;
  2173. r_circ_idx = 0;
  2174. delay_rate_incr = delay_rate/44100.*2147483648.;
  2175. //Serial.println(delay_rate_incr,HEX);
  2176. delay_offset_idx = delay_offset;
  2177. // Allow the passthru code to go through
  2178. if(delay_offset_idx < -1) {
  2179. delay_offset_idx = 0;
  2180. all_ok = false;
  2181. }
  2182. if(delay_offset_idx >= delay_length) {
  2183. delay_offset_idx = delay_length - 1;
  2184. all_ok = false;
  2185. }
  2186. return(all_ok);
  2187. }
  2188. boolean AudioEffectFlange::modify(int delay_offset,int d_depth,float delay_rate)
  2189. {
  2190. boolean all_ok = true;
  2191. delay_depth = d_depth;
  2192. delay_rate_incr = delay_rate/44100.*2147483648.;
  2193. delay_offset_idx = delay_offset;
  2194. // Allow the passthru code to go through
  2195. if(delay_offset_idx < -1) {
  2196. delay_offset_idx = 0;
  2197. all_ok = false;
  2198. }
  2199. if(delay_offset_idx >= delay_length) {
  2200. delay_offset_idx = delay_length - 1;
  2201. all_ok = false;
  2202. }
  2203. l_delay_rate_index = 0;
  2204. r_delay_rate_index = 0;
  2205. l_circ_idx = 0;
  2206. r_circ_idx = 0;
  2207. return(all_ok);
  2208. }
  2209. void AudioEffectFlange::update(void)
  2210. {
  2211. audio_block_t *block;
  2212. int idx;
  2213. short *bp;
  2214. short frac;
  2215. int idx1;
  2216. if(l_delayline == NULL)return;
  2217. if(r_delayline == NULL)return;
  2218. // do passthru
  2219. if(delay_offset_idx == DELAY_PASSTHRU) {
  2220. // Just passthrough
  2221. block = receiveWritable(0);
  2222. if(block) {
  2223. bp = block->data;
  2224. for(int i = 0;i < AUDIO_BLOCK_SAMPLES;i++) {
  2225. l_circ_idx++;
  2226. if(l_circ_idx >= delay_length) {
  2227. l_circ_idx = 0;
  2228. }
  2229. l_delayline[l_circ_idx] = *bp++;
  2230. }
  2231. transmit(block,0);
  2232. release(block);
  2233. }
  2234. block = receiveWritable(1);
  2235. if(block) {
  2236. bp = block->data;
  2237. for(int i = 0;i < AUDIO_BLOCK_SAMPLES;i++) {
  2238. r_circ_idx++;
  2239. if(r_circ_idx >= delay_length) {
  2240. r_circ_idx = 0;
  2241. }
  2242. r_delayline[r_circ_idx] = *bp++;
  2243. }
  2244. transmit(block,1);
  2245. release(block);
  2246. }
  2247. return;
  2248. }
  2249. // L E F T C H A N N E L
  2250. block = receiveWritable(0);
  2251. if(block) {
  2252. bp = block->data;
  2253. for(int i = 0;i < AUDIO_BLOCK_SAMPLES;i++) {
  2254. l_circ_idx++;
  2255. if(l_circ_idx >= delay_length) {
  2256. l_circ_idx = 0;
  2257. }
  2258. l_delayline[l_circ_idx] = *bp;
  2259. idx = arm_sin_q15( (q15_t)((l_delay_rate_index >> 16) & 0x7fff));
  2260. idx = (idx * delay_depth) >> 15;
  2261. //Serial.println(idx);
  2262. idx = l_circ_idx - (delay_offset_idx + idx);
  2263. if(idx < 0) {
  2264. idx += delay_length;
  2265. }
  2266. if(idx >= delay_length) {
  2267. idx -= delay_length;
  2268. }
  2269. if(frac < 0)
  2270. idx1 = idx - 1;
  2271. else
  2272. idx1 = idx + 1;
  2273. if(idx1 < 0) {
  2274. idx1 += delay_length;
  2275. }
  2276. if(idx1 >= delay_length) {
  2277. idx1 -= delay_length;
  2278. }
  2279. frac = (l_delay_rate_index >> 1) &0x7fff;
  2280. frac = (( (int)(l_delayline[idx1] - l_delayline[idx])*frac) >> 15);
  2281. *bp++ = (l_delayline[l_circ_idx]
  2282. + l_delayline[idx] + frac
  2283. )/2;
  2284. l_delay_rate_index += delay_rate_incr;
  2285. if(l_delay_rate_index & 0x80000000) {
  2286. l_delay_rate_index &= 0x7fffffff;
  2287. }
  2288. }
  2289. // send the effect output to the left channel
  2290. transmit(block,0);
  2291. release(block);
  2292. }
  2293. // R I G H T C H A N N E L
  2294. block = receiveWritable(1);
  2295. if(block) {
  2296. bp = block->data;
  2297. for(int i = 0;i < AUDIO_BLOCK_SAMPLES;i++) {
  2298. r_circ_idx++;
  2299. if(r_circ_idx >= delay_length) {
  2300. r_circ_idx = 0;
  2301. }
  2302. r_delayline[r_circ_idx] = *bp;
  2303. idx = arm_sin_q15( (q15_t)((r_delay_rate_index >> 16)&0x7fff));
  2304. idx = (idx * delay_depth) >> 15;
  2305. idx = r_circ_idx - (delay_offset_idx + idx);
  2306. if(idx < 0) {
  2307. idx += delay_length;
  2308. }
  2309. if(idx >= delay_length) {
  2310. idx -= delay_length;
  2311. }
  2312. if(frac < 0)
  2313. idx1 = idx - 1;
  2314. else
  2315. idx1 = idx + 1;
  2316. if(idx1 < 0) {
  2317. idx1 += delay_length;
  2318. }
  2319. if(idx1 >= delay_length) {
  2320. idx1 -= delay_length;
  2321. }
  2322. frac = (r_delay_rate_index >> 1) &0x7fff;
  2323. frac = (( (int)(r_delayline[idx1] - r_delayline[idx])*frac) >> 15);
  2324. *bp++ = (r_delayline[r_circ_idx]
  2325. + r_delayline[idx] + frac
  2326. )/2;
  2327. r_delay_rate_index += delay_rate_incr;
  2328. if(r_delay_rate_index & 0x80000000) {
  2329. r_delay_rate_index &= 0x7fffffff;
  2330. }
  2331. }
  2332. // send the effect output to the right channel
  2333. transmit(block,1);
  2334. release(block);
  2335. }
  2336. }
  2337. /******************************************************************/
  2338. // A u d i o E f f e c t C h o r u s
  2339. // Written by Pete (El Supremo) Jan 2014
  2340. // circular addressing indices for left and right channels
  2341. short AudioEffectChorus::l_circ_idx;
  2342. short AudioEffectChorus::r_circ_idx;
  2343. short * AudioEffectChorus::l_delayline = NULL;
  2344. short * AudioEffectChorus::r_delayline = NULL;
  2345. int AudioEffectChorus::delay_length;
  2346. // An initial value of zero indicates passthru
  2347. int AudioEffectChorus::num_chorus = 0;
  2348. // All three must be valid.
  2349. boolean AudioEffectChorus::begin(short *delayline,int d_length,int n_chorus)
  2350. {
  2351. Serial.print("AudioEffectChorus.begin(Chorus delay line length = ");
  2352. Serial.print(d_length);
  2353. Serial.print(", n_chorus = ");
  2354. Serial.print(n_chorus);
  2355. Serial.println(")");
  2356. l_delayline = NULL;
  2357. r_delayline = NULL;
  2358. delay_length = 0;
  2359. l_circ_idx = 0;
  2360. r_circ_idx = 0;
  2361. if(delayline == NULL) {
  2362. return(false);
  2363. }
  2364. if(d_length < 10) {
  2365. return(false);
  2366. }
  2367. if(n_chorus < 1) {
  2368. return(false);
  2369. }
  2370. l_delayline = delayline;
  2371. r_delayline = delayline + d_length/2;
  2372. delay_length = d_length/2;
  2373. num_chorus = n_chorus;
  2374. return(true);
  2375. }
  2376. // This has the same effect as begin(NULL,0);
  2377. void AudioEffectChorus::stop(void)
  2378. {
  2379. }
  2380. void AudioEffectChorus::modify(int n_chorus)
  2381. {
  2382. num_chorus = n_chorus;
  2383. }
  2384. int iabs(int x)
  2385. {
  2386. if(x < 0)return(-x);
  2387. return(x);
  2388. }
  2389. //static int d_count = 0;
  2390. int last_idx = 0;
  2391. void AudioEffectChorus::update(void)
  2392. {
  2393. audio_block_t *block;
  2394. short *bp;
  2395. int sum;
  2396. int c_idx;
  2397. if(l_delayline == NULL)return;
  2398. if(r_delayline == NULL)return;
  2399. // do passthru
  2400. // It stores the unmodified data in the delay line so that
  2401. // it isn't as likely to click
  2402. if(num_chorus < 1) {
  2403. // Just passthrough
  2404. block = receiveWritable(0);
  2405. if(block) {
  2406. bp = block->data;
  2407. for(int i = 0;i < AUDIO_BLOCK_SAMPLES;i++) {
  2408. l_circ_idx++;
  2409. if(l_circ_idx >= delay_length) {
  2410. l_circ_idx = 0;
  2411. }
  2412. l_delayline[l_circ_idx] = *bp++;
  2413. }
  2414. transmit(block,0);
  2415. release(block);
  2416. }
  2417. block = receiveWritable(1);
  2418. if(block) {
  2419. bp = block->data;
  2420. for(int i = 0;i < AUDIO_BLOCK_SAMPLES;i++) {
  2421. r_circ_idx++;
  2422. if(r_circ_idx >= delay_length) {
  2423. r_circ_idx = 0;
  2424. }
  2425. r_delayline[r_circ_idx] = *bp++;
  2426. }
  2427. transmit(block,1);
  2428. release(block);
  2429. }
  2430. return;
  2431. }
  2432. // L E F T C H A N N E L
  2433. block = receiveWritable(0);
  2434. if(block) {
  2435. bp = block->data;
  2436. for(int i = 0;i < AUDIO_BLOCK_SAMPLES;i++) {
  2437. l_circ_idx++;
  2438. if(l_circ_idx >= delay_length) {
  2439. l_circ_idx = 0;
  2440. }
  2441. l_delayline[l_circ_idx] = *bp;
  2442. sum = 0;
  2443. c_idx = l_circ_idx;
  2444. for(int k = 0; k < num_chorus; k++) {
  2445. sum += l_delayline[c_idx];
  2446. if(num_chorus > 1)c_idx -= delay_length/(num_chorus - 1) - 1;
  2447. if(c_idx < 0) {
  2448. c_idx += delay_length;
  2449. }
  2450. }
  2451. *bp++ = sum/num_chorus;
  2452. }
  2453. // send the effect output to the left channel
  2454. transmit(block,0);
  2455. release(block);
  2456. }
  2457. // R I G H T C H A N N E L
  2458. block = receiveWritable(1);
  2459. if(block) {
  2460. bp = block->data;
  2461. for(int i = 0;i < AUDIO_BLOCK_SAMPLES;i++) {
  2462. r_circ_idx++;
  2463. if(r_circ_idx >= delay_length) {
  2464. r_circ_idx = 0;
  2465. }
  2466. r_delayline[r_circ_idx] = *bp;
  2467. sum = 0;
  2468. c_idx = r_circ_idx;
  2469. for(int k = 0; k < num_chorus; k++) {
  2470. sum += r_delayline[c_idx];
  2471. if(num_chorus > 1)c_idx -= delay_length/(num_chorus - 1) - 1;
  2472. if(c_idx < 0) {
  2473. c_idx += delay_length;
  2474. }
  2475. }
  2476. *bp++ = sum/num_chorus;
  2477. }
  2478. // send the effect output to the left channel
  2479. transmit(block,1);
  2480. release(block);
  2481. }
  2482. }
  2483. // DAP_AUDIO_EQ_BASS_BAND0 & DAP_AUDIO_EQ_BAND1 & DAP_AUDIO_EQ_BAND2 etc etc
  2484. unsigned short AudioControlSGTL5000::dap_audio_eq_band(uint8_t bandNum, float n) // by signed percentage -100/+100; dap_audio_eq(3);
  2485. { // 0x00==-12dB, 0x2F==0dB, 0x5F==12dB
  2486. n=((n/100)*48)+0.499;
  2487. if(n<-47) n=-47;
  2488. if(n>48) n=48;
  2489. n+=47;
  2490. return modify(DAP_AUDIO_EQ_BASS_BAND0+(bandNum*2),(unsigned int)n,127);
  2491. }
  2492. void AudioControlSGTL5000::dap_audio_eq_geq(float bass, float mid_bass, float midrange, float mid_treble, float treble)
  2493. {
  2494. dap_audio_eq_band(0,bass);
  2495. dap_audio_eq_band(1,mid_bass);
  2496. dap_audio_eq_band(2,midrange);
  2497. dap_audio_eq_band(3,mid_treble);
  2498. dap_audio_eq_band(4,treble);
  2499. }
  2500. void AudioControlSGTL5000::dap_audio_eq_tone(float bass, float treble) // dap_audio_eq(2);
  2501. {
  2502. dap_audio_eq_band(0,bass);
  2503. dap_audio_eq_band(4,treble);
  2504. }
  2505. // SGTL5000 PEQ Coefficient loader
  2506. void AudioControlSGTL5000::load_peq(uint8_t filterNum, int *filterParameters)
  2507. {
  2508. // 1111 11111111 11111111
  2509. write(DAP_COEF_WR_B0_MSB,(*filterParameters>>4)&65535);
  2510. write(DAP_COEF_WR_B0_LSB,(*filterParameters++)&15);
  2511. write(DAP_COEF_WR_B1_MSB,(*filterParameters>>4)&65535);
  2512. write(DAP_COEF_WR_B1_LSB,(*filterParameters++)&15);
  2513. write(DAP_COEF_WR_B2_MSB,(*filterParameters>>4)&65535);
  2514. write(DAP_COEF_WR_B2_LSB,(*filterParameters++)&15);
  2515. write(DAP_COEF_WR_A1_MSB,(*filterParameters>>4)&65535);
  2516. write(DAP_COEF_WR_A1_LSB,(*filterParameters++)&15);
  2517. write(DAP_COEF_WR_A2_MSB,(*filterParameters>>4)&65535);
  2518. write(DAP_COEF_WR_A2_LSB,(*filterParameters++)&15);
  2519. write(DAP_FILTER_COEF_ACCESS,(uint16_t)0x100|filterNum);
  2520. delay(10); // seems necessary, didn't work for 1ms.
  2521. modify(DAP_FILTER_COEF_ACCESS,(uint16_t)filterNum,15);
  2522. }
  2523. unsigned char AudioControlSGTL5000::calcVol(float n, unsigned char range)
  2524. {
  2525. n=(n*(((float)range)/100))+0.499;
  2526. if ((unsigned char)n>range) n=range;
  2527. return (unsigned char)n;
  2528. }
  2529. // if(SGTL5000_PEQ) quantization_unit=524288; if(AudioFilterBiquad) quantization_unit=2147483648;
  2530. void calcBiquad(uint8_t filtertype, float fC, float dB_Gain, float Q, uint32_t quantization_unit, uint32_t fS, int *coef)
  2531. {
  2532. // I used resources like http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt
  2533. // to make this routine, I tested most of the filter types and they worked. Such filters have limits and
  2534. // before calling this routine with varying values the end user should check that those values are limited
  2535. // to valid results.
  2536. float A;
  2537. if(filtertype<FILTER_PARAEQ) A=pow(10,dB_Gain/20); else A=pow(10,dB_Gain/40);
  2538. float W0 = 2*3.14159265358979323846*fC/fS;
  2539. float cosw=cos(W0);
  2540. float sinw=sin(W0);
  2541. //float alpha = sinw*sinh((log(2)/2)*BW*W0/sinw);
  2542. //float beta = sqrt(2*A);
  2543. float alpha = sinw / (2 * Q);
  2544. float beta = sqrt(A)/Q;
  2545. float b0,b1,b2,a0,a1,a2;
  2546. switch(filtertype) {
  2547. case FILTER_LOPASS:
  2548. b0 = (1.0F - cosw) * 0.5F; // =(1-COS($H$2))/2
  2549. b1 = 1.0F - cosw;
  2550. b2 = (1.0F - cosw) * 0.5F;
  2551. a0 = 1.0F + alpha;
  2552. a1 = 2.0F * cosw;
  2553. a2 = alpha - 1.0F;
  2554. break;
  2555. case FILTER_HIPASS:
  2556. b0 = (1.0F + cosw) * 0.5F;
  2557. b1 = -(cosw + 1.0F);
  2558. b2 = (1.0F + cosw) * 0.5F;
  2559. a0 = 1.0F + alpha;
  2560. a1 = 2.0F * cosw;
  2561. a2 = alpha - 1.0F;
  2562. break;
  2563. case FILTER_BANDPASS:
  2564. b0 = alpha;
  2565. b1 = 0.0F;
  2566. b2 = -alpha;
  2567. a0 = 1.0F + alpha;
  2568. a1 = 2.0F * cosw;
  2569. a2 = alpha - 1.0F;
  2570. break;
  2571. case FILTER_NOTCH:
  2572. b0=1;
  2573. b1=-2*cosw;
  2574. b2=1;
  2575. a0=1+alpha;
  2576. a1=2*cosw;
  2577. a2=-(1-alpha);
  2578. break;
  2579. case FILTER_PARAEQ:
  2580. b0 = 1 + (alpha*A);
  2581. b1 =-2 * cosw;
  2582. b2 = 1 - (alpha*A);
  2583. a0 = 1 + (alpha/A);
  2584. a1 = 2 * cosw;
  2585. a2 =-(1-(alpha/A));
  2586. break;
  2587. case FILTER_LOSHELF:
  2588. b0 = A * ((A+1.0F) - ((A-1.0F)*cosw) + (beta*sinw));
  2589. b1 = 2.0F * A * ((A-1.0F) - ((A+1.0F)*cosw));
  2590. b2 = A * ((A+1.0F) - ((A-1.0F)*cosw) - (beta*sinw));
  2591. a0 = (A+1.0F) + ((A-1.0F)*cosw) + (beta*sinw);
  2592. a1 = 2.0F * ((A-1.0F) + ((A+1.0F)*cosw));
  2593. a2 = -((A+1.0F) + ((A-1.0F)*cosw) - (beta*sinw));
  2594. break;
  2595. case FILTER_HISHELF:
  2596. b0 = A * ((A+1.0F) + ((A-1.0F)*cosw) + (beta*sinw));
  2597. b1 = -2.0F * A * ((A-1.0F) + ((A+1.0F)*cosw));
  2598. b2 = A * ((A+1.0F) + ((A-1.0F)*cosw) - (beta*sinw));
  2599. a0 = (A+1.0F) - ((A-1.0F)*cosw) + (beta*sinw);
  2600. a1 = -2.0F * ((A-1.0F) - ((A+1.0F)*cosw));
  2601. a2 = -((A+1.0F) - ((A-1.0F)*cosw) - (beta*sinw));
  2602. }
  2603. a0=(a0*2)/(float)quantization_unit; // once here instead of five times there...
  2604. b0/=a0;
  2605. *coef++=(int)(b0+0.499);
  2606. b1/=a0;
  2607. *coef++=(int)(b1+0.499);
  2608. b2/=a0;
  2609. *coef++=(int)(b2+0.499);
  2610. a1/=a0;
  2611. *coef++=(int)(a1+0.499);
  2612. a2/=a0;
  2613. *coef++=(int)(a2+0.499);
  2614. }
  2615. /******************************************************************/
  2616. // A u d i o T o n e S w e e p
  2617. // Written by Pete (El Supremo) Feb 2014
  2618. boolean AudioToneSweep::begin(short t_amp,int t_lo,int t_hi,float t_time)
  2619. {
  2620. double tone_tmp;
  2621. if(0) {
  2622. Serial.print("AudioToneSweep.begin(tone_amp = ");
  2623. Serial.print(t_amp);
  2624. Serial.print(", tone_lo = ");
  2625. Serial.print(t_lo);
  2626. Serial.print(", tone_hi = ");
  2627. Serial.print(t_hi);
  2628. Serial.print(", tone_time = ");
  2629. Serial.print(t_time,1);
  2630. Serial.println(")");
  2631. }
  2632. tone_amp = 0;
  2633. if(t_amp < 0)return false;
  2634. if(t_lo < 1)return false;
  2635. if(t_hi < 1)return false;
  2636. if(t_hi >= 44100/2)return false;
  2637. if(t_lo >= 44100/2)return false;
  2638. if(t_time < 0)return false;
  2639. tone_lo = t_lo;
  2640. tone_hi = t_hi;
  2641. tone_phase = 0;
  2642. tone_amp = t_amp;
  2643. // Limit the output amplitude to prevent aliasing
  2644. // until I can figure out why this "overtops"
  2645. // above 29000.
  2646. if(tone_amp > 29000)tone_amp = 29000;
  2647. tone_tmp = tone_hi - tone_lo;
  2648. tone_sign = 1;
  2649. tone_freq = tone_lo*0x100000000LL;
  2650. if(tone_tmp < 0) {
  2651. tone_sign = -1;
  2652. tone_tmp = -tone_tmp;
  2653. }
  2654. tone_tmp = tone_tmp/t_time/44100.;
  2655. tone_incr = (tone_tmp * 0x100000000LL);
  2656. sweep_busy = 1;
  2657. return(true);
  2658. }
  2659. unsigned char AudioToneSweep::busy(void)
  2660. {
  2661. return(sweep_busy);
  2662. }
  2663. int b_count = 0;
  2664. void AudioToneSweep::update(void)
  2665. {
  2666. audio_block_t *block;
  2667. short *bp;
  2668. int i;
  2669. if(!sweep_busy)return;
  2670. // L E F T C H A N N E L O N L Y
  2671. block = allocate();
  2672. if(block) {
  2673. bp = block->data;
  2674. // Generate the sweep
  2675. for(i = 0;i < AUDIO_BLOCK_SAMPLES;i++) {
  2676. *bp++ = (short)(( (short)(arm_sin_q31((uint32_t)((tone_phase >> 15)&0x7fffffff))>>16) *tone_amp) >> 16);
  2677. uint64_t tone_tmp = (0x400000000000LL * (int)((tone_freq >> 32)&0x7fffffff))/44100;
  2678. tone_phase += tone_tmp;
  2679. if(tone_phase & 0x800000000000LL)tone_phase &= 0x7fffffffffffLL;
  2680. if(tone_sign > 0) {
  2681. if((tone_freq >> 32) > tone_hi) {
  2682. sweep_busy = 0;
  2683. break;
  2684. }
  2685. tone_freq += tone_incr;
  2686. } else {
  2687. if((tone_freq >> 32) < tone_hi) {
  2688. sweep_busy = 0;
  2689. break;
  2690. }
  2691. tone_freq -= tone_incr;
  2692. }
  2693. }
  2694. while(i < AUDIO_BLOCK_SAMPLES) {
  2695. *bp++ = 0;
  2696. i++;
  2697. }
  2698. b_count++;
  2699. // send the samples to the left channel
  2700. transmit(block,0);
  2701. release(block);
  2702. }
  2703. }