parent
ce0032dd78
commit
d6ac1238a9
@ -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 <arm_math.h> |
||||||
|
|
||||||
|
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); |
||||||
|
} |
||||||
|
*/ |
||||||
|
|
||||||
|
|
||||||
|
|
@ -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 |
@ -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 <arm_math.h> |
||||||
|
|
||||||
|
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
|
||||||
|
} |
||||||
|
*/ |
@ -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 |
@ -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; |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -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 |
@ -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 |
@ -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 <stdint.h> |
||||||
|
|
||||||
|
// 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 |
Loading…
Reference in new issue