diff --git a/OpenAudio_ArduinoLibrary.h b/OpenAudio_ArduinoLibrary.h index dbcf340..fb4388f 100644 --- a/OpenAudio_ArduinoLibrary.h +++ b/OpenAudio_ArduinoLibrary.h @@ -9,4 +9,7 @@ #include #include #include -#include \ No newline at end of file +#include "input_i2s_f32.h" +#include +#include "synth_sine_f32.h" +#include "output_i2s_f32.h" diff --git a/input_i2s_f32.cpp b/input_i2s_f32.cpp new file mode 100644 index 0000000..7b62d67 --- /dev/null +++ b/input_i2s_f32.cpp @@ -0,0 +1,230 @@ +/* Audio Library for Teensy 3.X + * Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com + * + * Development of this audio library was funded by PJRC.COM, LLC by sales of + * Teensy and Audio Adaptor boards. Please support PJRC's efforts to develop + * open source software by purchasing Teensy or other PJRC products. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice, development funding notice, and this permission + * notice shall be included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "input_i2s_f32.h" +#include "output_i2s_f32.h" +#include + +DMAMEM static uint32_t i2s_rx_buffer[AUDIO_BLOCK_SAMPLES]; +audio_block_t * AudioInputI2S_F32::block_left = NULL; +audio_block_t * AudioInputI2S_F32::block_right = NULL; +uint16_t AudioInputI2S_F32::block_offset = 0; +bool AudioInputI2S_F32::update_responsibility = false; +DMAChannel AudioInputI2S_F32::dma(false); + + +void AudioInputI2S_F32::begin(void) +{ + dma.begin(true); // Allocate the DMA channel first + + //block_left_1st = NULL; + //block_right_1st = NULL; + + // TODO: should we set & clear the I2S_RCSR_SR bit here? + AudioOutputI2S_F32::config_i2s(); + + CORE_PIN13_CONFIG = PORT_PCR_MUX(4); // pin 13, PTC5, I2S0_RXD0 +#if defined(KINETISK) + dma.TCD->SADDR = &I2S0_RDR0; + dma.TCD->SOFF = 0; + dma.TCD->ATTR = DMA_TCD_ATTR_SSIZE(1) | DMA_TCD_ATTR_DSIZE(1); + dma.TCD->NBYTES_MLNO = 2; + dma.TCD->SLAST = 0; + dma.TCD->DADDR = i2s_rx_buffer; + dma.TCD->DOFF = 2; + dma.TCD->CITER_ELINKNO = sizeof(i2s_rx_buffer) / 2; + dma.TCD->DLASTSGA = -sizeof(i2s_rx_buffer); + dma.TCD->BITER_ELINKNO = sizeof(i2s_rx_buffer) / 2; + dma.TCD->CSR = DMA_TCD_CSR_INTHALF | DMA_TCD_CSR_INTMAJOR; +#endif + dma.triggerAtHardwareEvent(DMAMUX_SOURCE_I2S0_RX); + update_responsibility = update_setup(); + dma.enable(); + + I2S0_RCSR |= I2S_RCSR_RE | I2S_RCSR_BCE | I2S_RCSR_FRDE | I2S_RCSR_FR; + I2S0_TCSR |= I2S_TCSR_TE | I2S_TCSR_BCE; // TX clock enable, because sync'd to TX + dma.attachInterrupt(isr); +} + +void AudioInputI2S_F32::isr(void) +{ + uint32_t daddr, offset; + const int16_t *src, *end; + int16_t *dest_left, *dest_right; + audio_block_t *left, *right; + + //digitalWriteFast(3, HIGH); +#if defined(KINETISK) + daddr = (uint32_t)(dma.TCD->DADDR); +#endif + dma.clearInterrupt(); + + if (daddr < (uint32_t)i2s_rx_buffer + sizeof(i2s_rx_buffer) / 2) { + // DMA is receiving to the first half of the buffer + // need to remove data from the second half + src = (int16_t *)&i2s_rx_buffer[AUDIO_BLOCK_SAMPLES/2]; + end = (int16_t *)&i2s_rx_buffer[AUDIO_BLOCK_SAMPLES]; + if (AudioInputI2S_F32::update_responsibility) AudioStream_F32::update_all(); + } else { + // DMA is receiving to the second half of the buffer + // need to remove data from the first half + src = (int16_t *)&i2s_rx_buffer[0]; + end = (int16_t *)&i2s_rx_buffer[AUDIO_BLOCK_SAMPLES/2]; + } + left = AudioInputI2S_F32::block_left; + right = AudioInputI2S_F32::block_right; + if (left != NULL && right != NULL) { + offset = AudioInputI2S_F32::block_offset; + if (offset <= AUDIO_BLOCK_SAMPLES/2) { + dest_left = &(left->data[offset]); + dest_right = &(right->data[offset]); + AudioInputI2S_F32::block_offset = offset + AUDIO_BLOCK_SAMPLES/2; + do { + //n = *src++; + //*dest_left++ = (int16_t)n; + //*dest_right++ = (int16_t)(n >> 16); + *dest_left++ = *src++; + *dest_right++ = *src++; + } while (src < end); + } + } + //digitalWriteFast(3, LOW); +} + + + +void AudioInputI2S_F32::update(void) +{ + audio_block_t *new_left=NULL, *new_right=NULL, *out_left=NULL, *out_right=NULL; + + // allocate 2 new blocks, but if one fails, allocate neither + new_left = AudioStream::allocate(); + if (new_left != NULL) { + new_right = AudioStream::allocate(); + if (new_right == NULL) { + AudioStream::release(new_left); + new_left = NULL; + } + } + __disable_irq(); + if (block_offset >= AUDIO_BLOCK_SAMPLES) { + // the DMA filled 2 blocks, so grab them and get the + // 2 new blocks to the DMA, as quickly as possible + out_left = block_left; + block_left = new_left; + out_right = block_right; + block_right = new_right; + block_offset = 0; + __enable_irq(); + + // then transmit the DMA's former blocks + + // but, first, convert them to F32 + audio_block_f32_t *out_left_f32=NULL, *out_right_f32=NULL; + out_left_f32 = AudioStream_F32::allocate_f32(); + if (out_left_f32 != NULL) { + out_right_f32 = AudioStream_F32::allocate_f32(); + if (out_right_f32 == NULL) { + AudioStream_F32::release(out_left_f32); + out_left_f32 = NULL; + } + } + if (out_left_f32 != NULL) { + //convert to f32 + arm_q15_to_float((q15_t *)out_left->data, (float32_t *)out_left_f32->data, AUDIO_BLOCK_SAMPLES); + arm_q15_to_float((q15_t *)out_right->data, (float32_t *)out_right_f32->data, AUDIO_BLOCK_SAMPLES); + + //transmit the f32 data! + AudioStream_F32::transmit(out_left_f32,0); + AudioStream_F32::transmit(out_right_f32,1); + AudioStream_F32::release(out_left_f32); + AudioStream_F32::release(out_right_f32); + } + AudioStream::release(out_left); + AudioStream::release(out_right); + //Serial.print("."); + } else if (new_left != NULL) { + // the DMA didn't fill blocks, but we allocated blocks + if (block_left == NULL) { + // the DMA doesn't have any blocks to fill, so + // give it the ones we just allocated + block_left = new_left; + block_right = new_right; + block_offset = 0; + __enable_irq(); + } else { + // the DMA already has blocks, doesn't need these + __enable_irq(); + AudioStream::release(new_left); + AudioStream::release(new_right); + } + } else { + // The DMA didn't fill blocks, and we could not allocate + // memory... the system is likely starving for memory! + // Sadly, there's nothing we can do. + __enable_irq(); + } +} + + +/******************************************************************/ + +/* +void AudioInputI2Sslave::begin(void) +{ + dma.begin(true); // Allocate the DMA channel first + + //block_left_1st = NULL; + //block_right_1st = NULL; + + AudioOutputI2Sslave::config_i2s(); + + CORE_PIN13_CONFIG = PORT_PCR_MUX(4); // pin 13, PTC5, I2S0_RXD0 +#if defined(KINETISK) + dma.TCD->SADDR = &I2S0_RDR0; + dma.TCD->SOFF = 0; + dma.TCD->ATTR = DMA_TCD_ATTR_SSIZE(1) | DMA_TCD_ATTR_DSIZE(1); + dma.TCD->NBYTES_MLNO = 2; + dma.TCD->SLAST = 0; + dma.TCD->DADDR = i2s_rx_buffer; + dma.TCD->DOFF = 2; + dma.TCD->CITER_ELINKNO = sizeof(i2s_rx_buffer) / 2; + dma.TCD->DLASTSGA = -sizeof(i2s_rx_buffer); + dma.TCD->BITER_ELINKNO = sizeof(i2s_rx_buffer) / 2; + dma.TCD->CSR = DMA_TCD_CSR_INTHALF | DMA_TCD_CSR_INTMAJOR; +#endif + dma.triggerAtHardwareEvent(DMAMUX_SOURCE_I2S0_RX); + update_responsibility = update_setup(); + dma.enable(); + + I2S0_RCSR |= I2S_RCSR_RE | I2S_RCSR_BCE | I2S_RCSR_FRDE | I2S_RCSR_FR; + I2S0_TCSR |= I2S_TCSR_TE | I2S_TCSR_BCE; // TX clock enable, because sync'd to TX + dma.attachInterrupt(isr); +} +*/ + + + diff --git a/input_i2s_f32.h b/input_i2s_f32.h new file mode 100644 index 0000000..25e022e --- /dev/null +++ b/input_i2s_f32.h @@ -0,0 +1,62 @@ +/* Audio Library for Teensy 3.X + * Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com + * + * Development of this audio library was funded by PJRC.COM, LLC by sales of + * Teensy and Audio Adaptor boards. Please support PJRC's efforts to develop + * open source software by purchasing Teensy or other PJRC products. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice, development funding notice, and this permission + * notice shall be included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#ifndef _input_i2s_f32_h_ +#define _input_i2s_f32_h_ + +#include "Arduino.h" +#include "AudioStream_F32.h" +#include "AudioStream.h" +#include "DMAChannel.h" + +class AudioInputI2S_F32 : public AudioStream_F32 +{ +public: + AudioInputI2S_F32(void) : AudioStream_F32(0, NULL) { begin(); } + virtual void update(void); + void begin(void); +protected: + AudioInputI2S_F32(int dummy): AudioStream_F32(0, NULL) {} // to be used only inside AudioInputI2Sslave !! + static bool update_responsibility; + static DMAChannel dma; + static void isr(void); +private: + static audio_block_t *block_left; + static audio_block_t *block_right; + static uint16_t block_offset; +}; + +/* +class AudioInputI2Sslave : public AudioInputI2S +{ +public: + AudioInputI2Sslave(void) : AudioInputI2S(0) { begin(); } + void begin(void); + friend void dma_ch1_isr(void); +}; +*/ + +#endif diff --git a/keywords.txt b/keywords.txt index 65adfda..bae2f52 100644 --- a/keywords.txt +++ b/keywords.txt @@ -26,10 +26,18 @@ setGain_dB KEYWORD2 AudioFilterFIR_F32 KEYWORD1 AudioEffectIIR_F32 KEYWORD1 AudioEffectCompressor_F32 KEYWORD1 +AudioInputI2S_F32 KEYWORD1 AudioMemory_F32 KEYWORD1 AudioMemoryUsage_F32 KEYWORD1 AudioMemoryUsageMax_F32 KEYWORD1 AudioMemoryUsageMaxReset_F32 KEYWORD1 AudioMixer4_F32 KEYWORD1 AudioMultiply_F32 KEYWORD1 +AudioOutputI2S_F32 KEYWORD1 AudioSynthWaveform_F32 KEYWORD1 + +AudioSynthWaveformSine_F32 KEYWORD1 +frequency KEYWORD2 +phase KEYWORD2 +amplitude KEYWORD2 + diff --git a/output_i2s_f32.cpp b/output_i2s_f32.cpp new file mode 100644 index 0000000..97fa342 --- /dev/null +++ b/output_i2s_f32.cpp @@ -0,0 +1,435 @@ +/* Audio Library for Teensy 3.X + * Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com + * + * Development of this audio library was funded by PJRC.COM, LLC by sales of + * Teensy and Audio Adaptor boards. Please support PJRC's efforts to develop + * open source software by purchasing Teensy or other PJRC products. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice, development funding notice, and this permission + * notice shall be included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "output_i2s_f32.h" +#include "memcpy_audio.h" +#include + +audio_block_t * AudioOutputI2S_F32::block_left_1st = NULL; +audio_block_t * AudioOutputI2S_F32::block_right_1st = NULL; +audio_block_t * AudioOutputI2S_F32::block_left_2nd = NULL; +audio_block_t * AudioOutputI2S_F32::block_right_2nd = NULL; +uint16_t AudioOutputI2S_F32::block_left_offset = 0; +uint16_t AudioOutputI2S_F32::block_right_offset = 0; +bool AudioOutputI2S_F32::update_responsibility = false; +DMAMEM static uint32_t i2s_tx_buffer[AUDIO_BLOCK_SAMPLES]; +DMAChannel AudioOutputI2S_F32::dma(false); + +void AudioOutputI2S_F32::begin(void) +{ + dma.begin(true); // Allocate the DMA channel first + + block_left_1st = NULL; + block_right_1st = NULL; + + // TODO: should we set & clear the I2S_TCSR_SR bit here? + config_i2s(); + CORE_PIN22_CONFIG = PORT_PCR_MUX(6); // pin 22, PTC1, I2S0_TXD0 + +#if defined(KINETISK) + dma.TCD->SADDR = i2s_tx_buffer; + dma.TCD->SOFF = 2; + dma.TCD->ATTR = DMA_TCD_ATTR_SSIZE(1) | DMA_TCD_ATTR_DSIZE(1); + dma.TCD->NBYTES_MLNO = 2; + dma.TCD->SLAST = -sizeof(i2s_tx_buffer); + dma.TCD->DADDR = &I2S0_TDR0; + dma.TCD->DOFF = 0; + dma.TCD->CITER_ELINKNO = sizeof(i2s_tx_buffer) / 2; + dma.TCD->DLASTSGA = 0; + dma.TCD->BITER_ELINKNO = sizeof(i2s_tx_buffer) / 2; + dma.TCD->CSR = DMA_TCD_CSR_INTHALF | DMA_TCD_CSR_INTMAJOR; +#endif + dma.triggerAtHardwareEvent(DMAMUX_SOURCE_I2S0_TX); + update_responsibility = update_setup(); + dma.enable(); + + I2S0_TCSR = I2S_TCSR_SR; + I2S0_TCSR = I2S_TCSR_TE | I2S_TCSR_BCE | I2S_TCSR_FRDE; + dma.attachInterrupt(isr); +} + + +void AudioOutputI2S_F32::isr(void) +{ +#if defined(KINETISK) + int16_t *dest; + audio_block_t *blockL, *blockR; + uint32_t saddr, offsetL, offsetR; + + saddr = (uint32_t)(dma.TCD->SADDR); + dma.clearInterrupt(); + if (saddr < (uint32_t)i2s_tx_buffer + sizeof(i2s_tx_buffer) / 2) { + // DMA is transmitting the first half of the buffer + // so we must fill the second half + dest = (int16_t *)&i2s_tx_buffer[AUDIO_BLOCK_SAMPLES/2]; + if (AudioOutputI2S_F32::update_responsibility) AudioStream_F32::update_all(); + } else { + // DMA is transmitting the second half of the buffer + // so we must fill the first half + dest = (int16_t *)i2s_tx_buffer; + } + + blockL = AudioOutputI2S_F32::block_left_1st; + blockR = AudioOutputI2S_F32::block_right_1st; + offsetL = AudioOutputI2S_F32::block_left_offset; + offsetR = AudioOutputI2S_F32::block_right_offset; + + if (blockL && blockR) { + memcpy_tointerleaveLR(dest, blockL->data + offsetL, blockR->data + offsetR); + offsetL += AUDIO_BLOCK_SAMPLES / 2; + offsetR += AUDIO_BLOCK_SAMPLES / 2; + } else if (blockL) { + memcpy_tointerleaveL(dest, blockL->data + offsetL); + offsetL += AUDIO_BLOCK_SAMPLES / 2; + } else if (blockR) { + memcpy_tointerleaveR(dest, blockR->data + offsetR); + offsetR += AUDIO_BLOCK_SAMPLES / 2; + } else { + memset(dest,0,AUDIO_BLOCK_SAMPLES * 2); + return; + } + + if (offsetL < AUDIO_BLOCK_SAMPLES) { + AudioOutputI2S_F32::block_left_offset = offsetL; + } else { + AudioOutputI2S_F32::block_left_offset = 0; + AudioStream::release(blockL); + AudioOutputI2S_F32::block_left_1st = AudioOutputI2S_F32::block_left_2nd; + AudioOutputI2S_F32::block_left_2nd = NULL; + } + if (offsetR < AUDIO_BLOCK_SAMPLES) { + AudioOutputI2S_F32::block_right_offset = offsetR; + } else { + AudioOutputI2S_F32::block_right_offset = 0; + AudioStream::release(blockR); + AudioOutputI2S_F32::block_right_1st = AudioOutputI2S_F32::block_right_2nd; + AudioOutputI2S_F32::block_right_2nd = NULL; + } +#else + const int16_t *src, *end; + int16_t *dest; + audio_block_t *block; + uint32_t saddr, offset; + + saddr = (uint32_t)(dma.CFG->SAR); + dma.clearInterrupt(); + if (saddr < (uint32_t)i2s_tx_buffer + sizeof(i2s_tx_buffer) / 2) { + // DMA is transmitting the first half of the buffer + // so we must fill the second half + dest = (int16_t *)&i2s_tx_buffer[AUDIO_BLOCK_SAMPLES/2]; + end = (int16_t *)&i2s_tx_buffer[AUDIO_BLOCK_SAMPLES]; + if (AudioOutputI2S_F32::update_responsibility) AudioStream_F32::update_all(); + } else { + // DMA is transmitting the second half of the buffer + // so we must fill the first half + dest = (int16_t *)i2s_tx_buffer; + end = (int16_t *)&i2s_tx_buffer[AUDIO_BLOCK_SAMPLES/2]; + } + + block = AudioOutputI2S_F32::block_left_1st; + if (block) { + offset = AudioOutputI2S_F32::block_left_offset; + src = &block->data[offset]; + do { + *dest = *src++; + dest += 2; + } while (dest < end); + offset += AUDIO_BLOCK_SAMPLES/2; + if (offset < AUDIO_BLOCK_SAMPLES) { + AudioOutputI2S_F32::block_left_offset = offset; + } else { + AudioOutputI2S_F32::block_left_offset = 0; + AudioStream::release(block); + AudioOutputI2S_F32::block_left_1st = AudioOutputI2S_F32::block_left_2nd; + AudioOutputI2S_F32::block_left_2nd = NULL; + } + } else { + do { + *dest = 0; + dest += 2; + } while (dest < end); + } + dest -= AUDIO_BLOCK_SAMPLES - 1; + block = AudioOutputI2S_F32::block_right_1st; + if (block) { + offset = AudioOutputI2S_F32::block_right_offset; + src = &block->data[offset]; + do { + *dest = *src++; + dest += 2; + } while (dest < end); + offset += AUDIO_BLOCK_SAMPLES/2; + if (offset < AUDIO_BLOCK_SAMPLES) { + AudioOutputI2S_F32::block_right_offset = offset; + } else { + AudioOutputI2S_F32::block_right_offset = 0; + AudioStream::release(block); + AudioOutputI2S_F32::block_right_1st = AudioOutputI2S_F32::block_right_2nd; + AudioOutputI2S_F32::block_right_2nd = NULL; + } + } else { + do { + *dest = 0; + dest += 2; + } while (dest < end); + } +#endif +} + + + + +void AudioOutputI2S_F32::update(void) +{ + // null audio device: discard all incoming data + //if (!active) return; + //audio_block_t *block = receiveReadOnly(); + //if (block) release(block); + + audio_block_t *block; + audio_block_f32_t *block_f32; + block_f32 = receiveReadOnly_f32(0); // input 0 = left channel + if (block_f32) { + + //convert F32 to Int16 + block = AudioStream::allocate(); + arm_float_to_q15((float32_t *)(block_f32->data),(q15_t *)(block->data), AUDIO_BLOCK_SAMPLES); + AudioStream_F32::release(block_f32); + + //now process the data blocks + __disable_irq(); + if (block_left_1st == NULL) { + block_left_1st = block; + block_left_offset = 0; + __enable_irq(); + } else if (block_left_2nd == NULL) { + block_left_2nd = block; + __enable_irq(); + } else { + audio_block_t *tmp = block_left_1st; + block_left_1st = block_left_2nd; + block_left_2nd = block; + block_left_offset = 0; + __enable_irq(); + AudioStream::release(tmp); + } + } + + block_f32 = receiveReadOnly_f32(1); // input 1 = right channel + if (block_f32) { + //convert F32 to Int16 + block = AudioStream::allocate(); + arm_float_to_q15((float32_t *)(block_f32->data),(q15_t *)(block->data), AUDIO_BLOCK_SAMPLES); + AudioStream_F32::release(block_f32); + + __disable_irq(); + if (block_right_1st == NULL) { + block_right_1st = block; + block_right_offset = 0; + __enable_irq(); + } else if (block_right_2nd == NULL) { + block_right_2nd = block; + __enable_irq(); + } else { + audio_block_t *tmp = block_right_1st; + block_right_1st = block_right_2nd; + block_right_2nd = block; + block_right_offset = 0; + __enable_irq(); + AudioStream::release(tmp); + } + } +} + + +// MCLK needs to be 48e6 / 1088 * 256 = 11.29411765 MHz -> 44.117647 kHz sample rate +// +#if F_CPU == 96000000 || F_CPU == 48000000 || F_CPU == 24000000 + // PLL is at 96 MHz in these modes + #define MCLK_MULT 2 + #define MCLK_DIV 17 +#elif F_CPU == 72000000 + #define MCLK_MULT 8 + #define MCLK_DIV 51 +#elif F_CPU == 120000000 + #define MCLK_MULT 8 + #define MCLK_DIV 85 +#elif F_CPU == 144000000 + #define MCLK_MULT 4 + #define MCLK_DIV 51 +#elif F_CPU == 168000000 + #define MCLK_MULT 8 + #define MCLK_DIV 119 +#elif F_CPU == 180000000 + #define MCLK_MULT 16 + #define MCLK_DIV 255 + #define MCLK_SRC 0 +#elif F_CPU == 192000000 + #define MCLK_MULT 1 + #define MCLK_DIV 17 +#elif F_CPU == 216000000 + #define MCLK_MULT 8 + #define MCLK_DIV 153 + #define MCLK_SRC 0 +#elif F_CPU == 240000000 + #define MCLK_MULT 4 + #define MCLK_DIV 85 +#elif F_CPU == 16000000 + #define MCLK_MULT 12 + #define MCLK_DIV 17 +#else + #error "This CPU Clock Speed is not supported by the Audio library"; +#endif + +#ifndef MCLK_SRC +#if F_CPU >= 20000000 + #define MCLK_SRC 3 // the PLL +#else + #define MCLK_SRC 0 // system clock +#endif +#endif + +void AudioOutputI2S_F32::config_i2s(void) +{ + SIM_SCGC6 |= SIM_SCGC6_I2S; + SIM_SCGC7 |= SIM_SCGC7_DMA; + SIM_SCGC6 |= SIM_SCGC6_DMAMUX; + + // if either transmitter or receiver is enabled, do nothing + if (I2S0_TCSR & I2S_TCSR_TE) return; + if (I2S0_RCSR & I2S_RCSR_RE) return; + + // enable MCLK output + I2S0_MCR = I2S_MCR_MICS(MCLK_SRC) | I2S_MCR_MOE; + while (I2S0_MCR & I2S_MCR_DUF) ; + I2S0_MDR = I2S_MDR_FRACT((MCLK_MULT-1)) | I2S_MDR_DIVIDE((MCLK_DIV-1)); + + // configure transmitter + I2S0_TMR = 0; + I2S0_TCR1 = I2S_TCR1_TFW(1); // watermark at half fifo size + I2S0_TCR2 = I2S_TCR2_SYNC(0) | I2S_TCR2_BCP | I2S_TCR2_MSEL(1) + | I2S_TCR2_BCD | I2S_TCR2_DIV(3); + I2S0_TCR3 = I2S_TCR3_TCE; + I2S0_TCR4 = I2S_TCR4_FRSZ(1) | I2S_TCR4_SYWD(15) | I2S_TCR4_MF + | I2S_TCR4_FSE | I2S_TCR4_FSP | I2S_TCR4_FSD; + I2S0_TCR5 = I2S_TCR5_WNW(15) | I2S_TCR5_W0W(15) | I2S_TCR5_FBT(15); + + // configure receiver (sync'd to transmitter clocks) + I2S0_RMR = 0; + I2S0_RCR1 = I2S_RCR1_RFW(1); + I2S0_RCR2 = I2S_RCR2_SYNC(1) | I2S_TCR2_BCP | I2S_RCR2_MSEL(1) + | I2S_RCR2_BCD | I2S_RCR2_DIV(3); + I2S0_RCR3 = I2S_RCR3_RCE; + I2S0_RCR4 = I2S_RCR4_FRSZ(1) | I2S_RCR4_SYWD(15) | I2S_RCR4_MF + | I2S_RCR4_FSE | I2S_RCR4_FSP | I2S_RCR4_FSD; + I2S0_RCR5 = I2S_RCR5_WNW(15) | I2S_RCR5_W0W(15) | I2S_RCR5_FBT(15); + + // configure pin mux for 3 clock signals + CORE_PIN23_CONFIG = PORT_PCR_MUX(6); // pin 23, PTC2, I2S0_TX_FS (LRCLK) + CORE_PIN9_CONFIG = PORT_PCR_MUX(6); // pin 9, PTC3, I2S0_TX_BCLK + CORE_PIN11_CONFIG = PORT_PCR_MUX(6); // pin 11, PTC6, I2S0_MCLK +} + + + + +/******************************************************************/ + +/* +void AudioOutputI2Sslave::begin(void) +{ + dma.begin(true); // Allocate the DMA channel first + + //pinMode(2, OUTPUT); + block_left_1st = NULL; + block_right_1st = NULL; + + AudioOutputI2Sslave::config_i2s(); + CORE_PIN22_CONFIG = PORT_PCR_MUX(6); // pin 22, PTC1, I2S0_TXD0 +#if defined(KINETISK) + dma.TCD->SADDR = i2s_tx_buffer; + dma.TCD->SOFF = 2; + dma.TCD->ATTR = DMA_TCD_ATTR_SSIZE(1) | DMA_TCD_ATTR_DSIZE(1); + dma.TCD->NBYTES_MLNO = 2; + dma.TCD->SLAST = -sizeof(i2s_tx_buffer); + dma.TCD->DADDR = &I2S0_TDR0; + dma.TCD->DOFF = 0; + dma.TCD->CITER_ELINKNO = sizeof(i2s_tx_buffer) / 2; + dma.TCD->DLASTSGA = 0; + dma.TCD->BITER_ELINKNO = sizeof(i2s_tx_buffer) / 2; + dma.TCD->CSR = DMA_TCD_CSR_INTHALF | DMA_TCD_CSR_INTMAJOR; +#endif + dma.triggerAtHardwareEvent(DMAMUX_SOURCE_I2S0_TX); + update_responsibility = update_setup(); + dma.enable(); + + I2S0_TCSR |= I2S_TCSR_TE | I2S_TCSR_BCE | I2S_TCSR_FRDE | I2S_TCSR_FR; + dma.attachInterrupt(isr); +} + +void AudioOutputI2Sslave::config_i2s(void) +{ + SIM_SCGC6 |= SIM_SCGC6_I2S; + SIM_SCGC7 |= SIM_SCGC7_DMA; + SIM_SCGC6 |= SIM_SCGC6_DMAMUX; + + // if either transmitter or receiver is enabled, do nothing + if (I2S0_TCSR & I2S_TCSR_TE) return; + if (I2S0_RCSR & I2S_RCSR_RE) return; + + // Select input clock 0 + // Configure to input the bit-clock from pin, bypasses the MCLK divider + I2S0_MCR = I2S_MCR_MICS(0); + I2S0_MDR = 0; + + // configure transmitter + I2S0_TMR = 0; + I2S0_TCR1 = I2S_TCR1_TFW(1); // watermark at half fifo size + I2S0_TCR2 = I2S_TCR2_SYNC(0) | I2S_TCR2_BCP; + + I2S0_TCR3 = I2S_TCR3_TCE; + I2S0_TCR4 = I2S_TCR4_FRSZ(1) | I2S_TCR4_SYWD(15) | I2S_TCR4_MF + | I2S_TCR4_FSE | I2S_TCR4_FSP; + + I2S0_TCR5 = I2S_TCR5_WNW(15) | I2S_TCR5_W0W(15) | I2S_TCR5_FBT(15); + + // configure receiver (sync'd to transmitter clocks) + I2S0_RMR = 0; + I2S0_RCR1 = I2S_RCR1_RFW(1); + I2S0_RCR2 = I2S_RCR2_SYNC(1) | I2S_TCR2_BCP; + + I2S0_RCR3 = I2S_RCR3_RCE; + I2S0_RCR4 = I2S_RCR4_FRSZ(1) | I2S_RCR4_SYWD(15) | I2S_RCR4_MF + | I2S_RCR4_FSE | I2S_RCR4_FSP | I2S_RCR4_FSD; + + I2S0_RCR5 = I2S_RCR5_WNW(15) | I2S_RCR5_W0W(15) | I2S_RCR5_FBT(15); + + // configure pin mux for 3 clock signals + CORE_PIN23_CONFIG = PORT_PCR_MUX(6); // pin 23, PTC2, I2S0_TX_FS (LRCLK) + CORE_PIN9_CONFIG = PORT_PCR_MUX(6); // pin 9, PTC3, I2S0_TX_BCLK + CORE_PIN11_CONFIG = PORT_PCR_MUX(6); // pin 11, PTC6, I2S0_MCLK +} +*/ \ No newline at end of file diff --git a/output_i2s_f32.h b/output_i2s_f32.h new file mode 100644 index 0000000..18b2eb0 --- /dev/null +++ b/output_i2s_f32.h @@ -0,0 +1,72 @@ +/* Audio Library for Teensy 3.X + * Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com + * + * Development of this audio library was funded by PJRC.COM, LLC by sales of + * Teensy and Audio Adaptor boards. Please support PJRC's efforts to develop + * open source software by purchasing Teensy or other PJRC products. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice, development funding notice, and this permission + * notice shall be included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#ifndef output_i2s_f32_h_ +#define output_i2s_f32_h_ + +#include "Arduino.h" +#include "AudioStream_F32.h" +#include "AudioStream.h" +#include "DMAChannel.h" + +class AudioOutputI2S_F32 : public AudioStream_F32 +{ +public: + AudioOutputI2S_F32(void) : AudioStream_F32(2, inputQueueArray) { begin(); } + virtual void update(void); + void begin(void); + friend class AudioInputI2S_F32; +protected: + AudioOutputI2S_F32(int dummy): AudioStream_F32(2, inputQueueArray) {} // to be used only inside AudioOutputI2Sslave !! + static void config_i2s(void); + static audio_block_t *block_left_1st; + static audio_block_t *block_right_1st; + static bool update_responsibility; + static DMAChannel dma; + static void isr(void); +private: + static audio_block_t *block_left_2nd; + static audio_block_t *block_right_2nd; + static uint16_t block_left_offset; + static uint16_t block_right_offset; + audio_block_f32_t *inputQueueArray[2]; +}; + + +/* +class AudioOutputI2Sslave : public AudioOutputI2S +{ +public: + AudioOutputI2Sslave(void) : AudioOutputI2S(0) { begin(); } ; + void begin(void); + friend class AudioInputI2Sslave; + friend void dma_ch0_isr(void); +protected: + static void config_i2s(void); +}; +*/ + +#endif diff --git a/synth_sine_f32.cpp b/synth_sine_f32.cpp new file mode 100644 index 0000000..d4af818 --- /dev/null +++ b/synth_sine_f32.cpp @@ -0,0 +1,65 @@ +/* + * AudioSynthWaveformSine_F32 + * + * Created: Chip Audette (OpenAudio) Feb 2017 + * Modeled on: AudioSynthWaveformSine from Teensy Audio Library + * + * Purpose: Create sine wave of given amplitude and frequency + * + * License: MIT License. Use at your own risk. + * + */ + +#include "synth_sine_f32.h" +#include "utility/dspinst.h" + +// data_waveforms.c +extern "C" { + extern const int16_t AudioWaveformSine[257]; +} + + +void AudioSynthWaveformSine_F32::update(void) +{ + audio_block_f32_t *block; + uint32_t i, ph, inc, index, scale; + int32_t val1, val2; + + if (magnitude) { + block = allocate_f32(); + if (block) { + ph = phase_accumulator; + inc = phase_increment; + for (i=0; i < AUDIO_BLOCK_SAMPLES; i++) { + index = ph >> 24; + val1 = AudioWaveformSine[index]; + val2 = AudioWaveformSine[index+1]; + scale = (ph >> 8) & 0xFFFF; + val2 *= scale; + val1 *= 0x10000 - scale; +#if defined(KINETISK) + block->data[i] = (float) multiply_32x32_rshift32(val1 + val2, magnitude); +#elif defined(KINETISL) + block->data[i] = (float) ((((val1 + val2) >> 16) * magnitude) >> 16); +#endif + ph += inc; + + block->data[i] = block->data[i] / 32768.0f; // scale to float + } + phase_accumulator = ph; + + + + AudioStream_F32::transmit(block); + AudioStream_F32::release(block); + return; + } + } + phase_accumulator += phase_increment * AUDIO_BLOCK_SAMPLES; +} + + + + + + diff --git a/synth_sine_f32.h b/synth_sine_f32.h new file mode 100644 index 0000000..3cdcd3f --- /dev/null +++ b/synth_sine_f32.h @@ -0,0 +1,52 @@ +/* + * AdioSynthWaveformSine_F32 + * + * Created: Chip Audette (OpenAudio) Feb 2017 + * Modeled on: AudioSynthWaveformSine from Teensy Audio Library + * + * Purpose: Create sine wave of given amplitude and frequency + * + * License: MIT License. Use at your own risk. + * + */ + +#ifndef synth_sine_f32_h_ +#define synth_sine_f32_h_ + +#include "Arduino.h" +#include "AudioStream_F32.h" +#include "arm_math.h" + + +class AudioSynthWaveformSine_F32 : public AudioStream_F32 +{ +public: + AudioSynthWaveformSine_F32() : AudioStream_F32(0, NULL), magnitude(16384) {} + void frequency(float freq) { + if (freq < 0.0) freq = 0.0; + else if (freq > AUDIO_SAMPLE_RATE_EXACT/2) freq = AUDIO_SAMPLE_RATE_EXACT/2; + phase_increment = freq * (4294967296.0 / AUDIO_SAMPLE_RATE_EXACT); + } + void phase(float angle) { + if (angle < 0.0) angle = 0.0; + else if (angle > 360.0) { + angle = angle - 360.0; + if (angle >= 360.0) return; + } + phase_accumulator = angle * (4294967296.0 / 360.0); + } + void amplitude(float n) { + if (n < 0) n = 0; + else if (n > 1.0) n = 1.0; + magnitude = n * 65536.0; + } + virtual void update(void); +private: + uint32_t phase_accumulator; + uint32_t phase_increment; + int32_t magnitude; +}; + + + +#endif diff --git a/utility/dma_chan.h b/utility/dma_chan.h new file mode 100644 index 0000000..842a177 --- /dev/null +++ b/utility/dma_chan.h @@ -0,0 +1,76 @@ +/* Audio Library for Teensy 3.X + * Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com + * + * Development of this audio library was funded by PJRC.COM, LLC by sales of + * Teensy and Audio Adaptor boards. Please support PJRC's efforts to develop + * open source software by purchasing Teensy or other PJRC products. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice, development funding notice, and this permission + * notice shall be included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#ifndef dma_chan_h_ +#define dma_chan_h_ + +/* +TODO: remove this file - made obsolete by DMAChannel.h + +#define DMA_ISR(ch) DMA_ISR_EXP(ch) +#define DMA_ISR_EXP(ch) dma_ch ## ch ## _isr + +#define IRQ_DMA_CH(ch) IRQ_DMA_CH_EXP(ch) +#define IRQ_DMA_CH_EXP(ch) IRQ_DMA_CH ## ch + +#define DMAMUX0_CHCFG(ch) DMAMUX0_CHCFG_EXP(ch) +#define DMAMUX0_CHCFG_EXP(ch) DMAMUX0_CHCFG ## ch + +#define DMA_TCD_SADDR(ch) DMA_TCD_SADDR_EXP(ch) +#define DMA_TCD_SADDR_EXP(ch) DMA_TCD ## ch ## _SADDR + +#define DMA_TCD_SOFF(ch) DMA_TCD_SOFF_EXP(ch) +#define DMA_TCD_SOFF_EXP(ch) DMA_TCD ## ch ## _SOFF + +#define DMA_TCD_ATTR(ch) DMA_TCD_ATTR_EXP(ch) +#define DMA_TCD_ATTR_EXP(ch) DMA_TCD ## ch ## _ATTR + +#define DMA_TCD_NBYTES_MLNO(ch) DMA_TCD_NBYTES_MLNO_EXP(ch) +#define DMA_TCD_NBYTES_MLNO_EXP(ch) DMA_TCD ## ch ## _NBYTES_MLNO + +#define DMA_TCD_SLAST(ch) DMA_TCD_SLAST_EXP(ch) +#define DMA_TCD_SLAST_EXP(ch) DMA_TCD ## ch ## _SLAST + +#define DMA_TCD_DADDR(ch) DMA_TCD_DADDR_EXP(ch) +#define DMA_TCD_DADDR_EXP(ch) DMA_TCD ## ch ## _DADDR + +#define DMA_TCD_DOFF(ch) DMA_TCD_DOFF_EXP(ch) +#define DMA_TCD_DOFF_EXP(ch) DMA_TCD ## ch ## _DOFF + +#define DMA_TCD_CITER_ELINKNO(ch) DMA_TCD_CITER_ELINKNO_EXP(ch) +#define DMA_TCD_CITER_ELINKNO_EXP(ch) DMA_TCD ## ch ## _CITER_ELINKNO + +#define DMA_TCD_DLASTSGA(ch) DMA_TCD_DLASTSGA_EXP(ch) +#define DMA_TCD_DLASTSGA_EXP(ch) DMA_TCD ## ch ## _DLASTSGA + +#define DMA_TCD_BITER_ELINKNO(ch) DMA_TCD_BITER_ELINKNO_EXP(ch) +#define DMA_TCD_BITER_ELINKNO_EXP(ch) DMA_TCD ## ch ## _BITER_ELINKNO + +#define DMA_TCD_CSR(ch) DMA_TCD_CSR_EXP(ch) +#define DMA_TCD_CSR_EXP(ch) DMA_TCD ## ch ## _CSR +*/ + +#endif diff --git a/utility/dspinst.h b/utility/dspinst.h new file mode 100644 index 0000000..d66e246 --- /dev/null +++ b/utility/dspinst.h @@ -0,0 +1,354 @@ +/* Audio Library for Teensy 3.X + * Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com + * + * Development of this audio library was funded by PJRC.COM, LLC by sales of + * Teensy and Audio Adaptor boards. Please support PJRC's efforts to develop + * open source software by purchasing Teensy or other PJRC products. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice, development funding notice, and this permission + * notice shall be included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#ifndef dspinst_h_ +#define dspinst_h_ + +#include + +// computes limit((val >> rshift), 2**bits) +static inline int32_t signed_saturate_rshift(int32_t val, int bits, int rshift) __attribute__((always_inline, unused)); +static inline int32_t signed_saturate_rshift(int32_t val, int bits, int rshift) +{ +#if defined(KINETISK) + int32_t out; + asm volatile("ssat %0, %1, %2, asr %3" : "=r" (out) : "I" (bits), "r" (val), "I" (rshift)); + return out; +#elif defined(KINETISL) + int32_t out, max; + out = val >> rshift; + max = 1 << (bits - 1); + if (out >= 0) { + if (out > max - 1) out = max - 1; + } else { + if (out < -max) out = -max; + } + return out; +#endif +} + +// computes limit(val, 2**bits) +static inline int16_t saturate16(int32_t val) __attribute__((always_inline, unused)); +static inline int16_t saturate16(int32_t val) +{ +#if defined(KINETISK) + int16_t out; + int32_t tmp; + asm volatile("ssat %0, %1, %2" : "=r" (tmp) : "I" (16), "r" (val) ); + out = (int16_t) (tmp & 0xffff); // not sure if the & 0xffff is necessary. test. + return out; +#elif defined(KINETISL) + return 0; // TODO.... +#endif +} + +// computes ((a[31:0] * b[15:0]) >> 16) +static inline int32_t signed_multiply_32x16b(int32_t a, uint32_t b) __attribute__((always_inline, unused)); +static inline int32_t signed_multiply_32x16b(int32_t a, uint32_t b) +{ +#if defined(KINETISK) + int32_t out; + asm volatile("smulwb %0, %1, %2" : "=r" (out) : "r" (a), "r" (b)); + return out; +#elif defined(KINETISL) + return ((int64_t)a * (int16_t)(b & 0xFFFF)) >> 16; +#endif +} + +// computes ((a[31:0] * b[31:16]) >> 16) +static inline int32_t signed_multiply_32x16t(int32_t a, uint32_t b) __attribute__((always_inline, unused)); +static inline int32_t signed_multiply_32x16t(int32_t a, uint32_t b) +{ +#if defined(KINETISK) + int32_t out; + asm volatile("smulwt %0, %1, %2" : "=r" (out) : "r" (a), "r" (b)); + return out; +#elif defined(KINETISL) + return ((int64_t)a * (int16_t)(b >> 16)) >> 16; +#endif +} + +// computes (((int64_t)a[31:0] * (int64_t)b[31:0]) >> 32) +static inline int32_t multiply_32x32_rshift32(int32_t a, int32_t b) __attribute__((always_inline, unused)); +static inline int32_t multiply_32x32_rshift32(int32_t a, int32_t b) +{ +#if defined(KINETISK) + int32_t out; + asm volatile("smmul %0, %1, %2" : "=r" (out) : "r" (a), "r" (b)); + return out; +#elif defined(KINETISL) + return 0; // TODO.... +#endif +} + +// computes (((int64_t)a[31:0] * (int64_t)b[31:0] + 0x8000000) >> 32) +static inline int32_t multiply_32x32_rshift32_rounded(int32_t a, int32_t b) __attribute__((always_inline, unused)); +static inline int32_t multiply_32x32_rshift32_rounded(int32_t a, int32_t b) +{ +#if defined(KINETISK) + int32_t out; + asm volatile("smmulr %0, %1, %2" : "=r" (out) : "r" (a), "r" (b)); + return out; +#elif defined(KINETISL) + return 0; // TODO.... +#endif +} + +// computes sum + (((int64_t)a[31:0] * (int64_t)b[31:0] + 0x8000000) >> 32) +static inline int32_t multiply_accumulate_32x32_rshift32_rounded(int32_t sum, int32_t a, int32_t b) __attribute__((always_inline, unused)); +static inline int32_t multiply_accumulate_32x32_rshift32_rounded(int32_t sum, int32_t a, int32_t b) +{ +#if defined(KINETISK) + int32_t out; + asm volatile("smmlar %0, %2, %3, %1" : "=r" (out) : "r" (sum), "r" (a), "r" (b)); + return out; +#elif defined(KINETISL) + return 0; // TODO.... +#endif +} + +// computes sum - (((int64_t)a[31:0] * (int64_t)b[31:0] + 0x8000000) >> 32) +static inline int32_t multiply_subtract_32x32_rshift32_rounded(int32_t sum, int32_t a, int32_t b) __attribute__((always_inline, unused)); +static inline int32_t multiply_subtract_32x32_rshift32_rounded(int32_t sum, int32_t a, int32_t b) +{ +#if defined(KINETISK) + int32_t out; + asm volatile("smmlsr %0, %2, %3, %1" : "=r" (out) : "r" (sum), "r" (a), "r" (b)); + return out; +#elif defined(KINETISL) + return 0; // TODO.... +#endif +} + + +// computes (a[31:16] | (b[31:16] >> 16)) +static inline uint32_t pack_16t_16t(int32_t a, int32_t b) __attribute__((always_inline, unused)); +static inline uint32_t pack_16t_16t(int32_t a, int32_t b) +{ +#if defined(KINETISK) + int32_t out; + asm volatile("pkhtb %0, %1, %2, asr #16" : "=r" (out) : "r" (a), "r" (b)); + return out; +#elif defined(KINETISL) + return (a & 0xFFFF0000) | ((uint32_t)b >> 16); +#endif +} + +// computes (a[31:16] | b[15:0]) +static inline uint32_t pack_16t_16b(int32_t a, int32_t b) __attribute__((always_inline, unused)); +static inline uint32_t pack_16t_16b(int32_t a, int32_t b) +{ +#if defined(KINETISK) + int32_t out; + asm volatile("pkhtb %0, %1, %2" : "=r" (out) : "r" (a), "r" (b)); + return out; +#elif defined(KINETISL) + return (a & 0xFFFF0000) | (b & 0x0000FFFF); +#endif +} + +// computes ((a[15:0] << 16) | b[15:0]) +static inline uint32_t pack_16b_16b(int32_t a, int32_t b) __attribute__((always_inline, unused)); +static inline uint32_t pack_16b_16b(int32_t a, int32_t b) +{ +#if defined(KINETISK) + int32_t out; + asm volatile("pkhbt %0, %1, %2, lsl #16" : "=r" (out) : "r" (b), "r" (a)); + return out; +#elif defined(KINETISL) + return (a << 16) | (b & 0x0000FFFF); +#endif +} + +// computes ((a[15:0] << 16) | b[15:0]) +/* +static inline uint32_t pack_16x16(int32_t a, int32_t b) __attribute__((always_inline, unused)); +static inline uint32_t pack_16x16(int32_t a, int32_t b) +{ + int32_t out; + asm volatile("pkhbt %0, %1, %2, lsl #16" : "=r" (out) : "r" (b), "r" (a)); + return out; +} +*/ + +// computes (((a[31:16] + b[31:16]) << 16) | (a[15:0 + b[15:0])) (saturates) +static inline uint32_t signed_add_16_and_16(uint32_t a, uint32_t b) __attribute__((always_inline, unused)); +static inline uint32_t signed_add_16_and_16(uint32_t a, uint32_t b) +{ + int32_t out; + asm volatile("qadd16 %0, %1, %2" : "=r" (out) : "r" (a), "r" (b)); + return out; +} + +// computes (((a[31:16] - b[31:16]) << 16) | (a[15:0 - b[15:0])) (saturates) +static inline int32_t signed_subtract_16_and_16(int32_t a, int32_t b) __attribute__((always_inline, unused)); +static inline int32_t signed_subtract_16_and_16(int32_t a, int32_t b) +{ + int32_t out; + asm volatile("qsub16 %0, %1, %2" : "=r" (out) : "r" (a), "r" (b)); + return out; +} + +// computes out = (((a[31:16]+b[31:16])/2) <<16) | ((a[15:0]+b[15:0])/2) +static inline int32_t signed_halving_add_16_and_16(int32_t a, int32_t b) __attribute__((always_inline, unused)); +static inline int32_t signed_halving_add_16_and_16(int32_t a, int32_t b) +{ + int32_t out; + asm volatile("shadd16 %0, %1, %2" : "=r" (out) : "r" (a), "r" (b)); + return out; +} + +// computes out = (((a[31:16]-b[31:16])/2) <<16) | ((a[15:0]-b[15:0])/2) +static inline int32_t signed_halving_subtract_16_and_16(int32_t a, int32_t b) __attribute__((always_inline, unused)); +static inline int32_t signed_halving_subtract_16_and_16(int32_t a, int32_t b) +{ + int32_t out; + asm volatile("shsub16 %0, %1, %2" : "=r" (out) : "r" (a), "r" (b)); + return out; +} + +// computes (sum + ((a[31:0] * b[15:0]) >> 16)) +static inline int32_t signed_multiply_accumulate_32x16b(int32_t sum, int32_t a, uint32_t b) __attribute__((always_inline, unused)); +static inline int32_t signed_multiply_accumulate_32x16b(int32_t sum, int32_t a, uint32_t b) +{ + int32_t out; + asm volatile("smlawb %0, %2, %3, %1" : "=r" (out) : "r" (sum), "r" (a), "r" (b)); + return out; +} + +// computes (sum + ((a[31:0] * b[31:16]) >> 16)) +static inline int32_t signed_multiply_accumulate_32x16t(int32_t sum, int32_t a, uint32_t b) __attribute__((always_inline, unused)); +static inline int32_t signed_multiply_accumulate_32x16t(int32_t sum, int32_t a, uint32_t b) +{ + int32_t out; + asm volatile("smlawt %0, %2, %3, %1" : "=r" (out) : "r" (sum), "r" (a), "r" (b)); + return out; +} + +// computes logical and, forces compiler to allocate register and use single cycle instruction +static inline uint32_t logical_and(uint32_t a, uint32_t b) __attribute__((always_inline, unused)); +static inline uint32_t logical_and(uint32_t a, uint32_t b) +{ + asm volatile("and %0, %1" : "+r" (a) : "r" (b)); + return a; +} + +// computes ((a[15:0] * b[15:0]) + (a[31:16] * b[31:16])) +static inline int32_t multiply_16tx16t_add_16bx16b(uint32_t a, uint32_t b) __attribute__((always_inline, unused)); +static inline int32_t multiply_16tx16t_add_16bx16b(uint32_t a, uint32_t b) +{ + int32_t out; + asm volatile("smuad %0, %1, %2" : "=r" (out) : "r" (a), "r" (b)); + return out; +} + +// computes ((a[15:0] * b[31:16]) + (a[31:16] * b[15:0])) +static inline int32_t multiply_16tx16b_add_16bx16t(uint32_t a, uint32_t b) __attribute__((always_inline, unused)); +static inline int32_t multiply_16tx16b_add_16bx16t(uint32_t a, uint32_t b) +{ + int32_t out; + asm volatile("smuadx %0, %1, %2" : "=r" (out) : "r" (a), "r" (b)); + return out; +} + +// // computes sum += ((a[15:0] * b[15:0]) + (a[31:16] * b[31:16])) +static inline int64_t multiply_accumulate_16tx16t_add_16bx16b(int64_t sum, uint32_t a, uint32_t b) +{ + asm volatile("smlald %Q0, %R0, %1, %2" : "+r" (sum) : "r" (a), "r" (b)); + return sum; +} + +// // computes sum += ((a[15:0] * b[31:16]) + (a[31:16] * b[15:0])) +static inline int64_t multiply_accumulate_16tx16b_add_16bx16t(int64_t sum, uint32_t a, uint32_t b) +{ + asm volatile("smlaldx %Q0, %R0, %1, %2" : "+r" (sum) : "r" (a), "r" (b)); + return sum; +} + +// computes ((a[15:0] * b[15:0]) +static inline int32_t multiply_16bx16b(uint32_t a, uint32_t b) __attribute__((always_inline, unused)); +static inline int32_t multiply_16bx16b(uint32_t a, uint32_t b) +{ + int32_t out; + asm volatile("smulbb %0, %1, %2" : "=r" (out) : "r" (a), "r" (b)); + return out; +} + +// computes ((a[15:0] * b[31:16]) +static inline int32_t multiply_16bx16t(uint32_t a, uint32_t b) __attribute__((always_inline, unused)); +static inline int32_t multiply_16bx16t(uint32_t a, uint32_t b) +{ + int32_t out; + asm volatile("smulbt %0, %1, %2" : "=r" (out) : "r" (a), "r" (b)); + return out; +} + +// computes ((a[31:16] * b[15:0]) +static inline int32_t multiply_16tx16b(uint32_t a, uint32_t b) __attribute__((always_inline, unused)); +static inline int32_t multiply_16tx16b(uint32_t a, uint32_t b) +{ + int32_t out; + asm volatile("smultb %0, %1, %2" : "=r" (out) : "r" (a), "r" (b)); + return out; +} + +// computes ((a[31:16] * b[31:16]) +static inline int32_t multiply_16tx16t(uint32_t a, uint32_t b) __attribute__((always_inline, unused)); +static inline int32_t multiply_16tx16t(uint32_t a, uint32_t b) +{ + int32_t out; + asm volatile("smultt %0, %1, %2" : "=r" (out) : "r" (a), "r" (b)); + return out; +} + +// computes (a - b), result saturated to 32 bit integer range +static inline int32_t substract_32_saturate(uint32_t a, uint32_t b) __attribute__((always_inline, unused)); +static inline int32_t substract_32_saturate(uint32_t a, uint32_t b) +{ + int32_t out; + asm volatile("qsub %0, %1, %2" : "=r" (out) : "r" (a), "r" (b)); + return out; +} + +//get Q from PSR +static inline uint32_t get_q_psr(void) __attribute__((always_inline, unused)); +static inline uint32_t get_q_psr(void) +{ + uint32_t out; + asm ("mrs %0, APSR" : "=r" (out)); + return (out & 0x8000000)>>27; +} + +//clear Q BIT in PSR +static inline void clr_q_psr(void) __attribute__((always_inline, unused)); +static inline void clr_q_psr(void) +{ + uint32_t t; + asm ("mov %[t],#0\n" + "msr APSR_nzcvq,%0\n" : [t] "=&r" (t)::"cc"); +} + +#endif