Add AudioStream for T4

teensy4/AudioStream.cpp

@@ -0,0 +1,339 @@
+/* Teensyduino Core Library
+ * http://www.pjrc.com/teensy/
+ * Copyright (c) 2017 PJRC.COM, LLC.
+ *
+ * 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:
+ *
+ * 1. The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * 2. If the Software is incorporated into a build system that allows
+ * selection among a list of target devices, then similar target
+ * devices manufactured by PJRC.COM must be included in the list of
+ * target devices and selectable in the same manner.
+ *
+ * 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 <Arduino.h>
+#include "AudioStream.h"
+
+#if defined(__IMXRT1052__)
+  #define MAX_AUDIO_MEMORY 229376
+#elif defined(__IMXRT1062__)
+  #define MAX_AUDIO_MEMORY 229376
+#endif
+
+#define NUM_MASKS  (((MAX_AUDIO_MEMORY / AUDIO_BLOCK_SAMPLES / 2) + 31) / 32)
+
+audio_block_t * AudioStream::memory_pool;
+uint32_t AudioStream::memory_pool_available_mask[NUM_MASKS];
+uint16_t AudioStream::memory_pool_first_mask;
+
+uint16_t AudioStream::cpu_cycles_total = 0;
+uint16_t AudioStream::cpu_cycles_total_max = 0;
+uint16_t AudioStream::memory_used = 0;
+uint16_t AudioStream::memory_used_max = 0;
+
+void software_isr(void);
+
+
+// Set up the pool of audio data blocks
+// placing them all onto the free list
+void AudioStream::initialize_memory(audio_block_t *data, unsigned int num)
+{
+	unsigned int i;
+	unsigned int maxnum = MAX_AUDIO_MEMORY / AUDIO_BLOCK_SAMPLES / 2;
+
+	//Serial.println("AudioStream initialize_memory");
+	//delay(10);
+	if (num > maxnum) num = maxnum;
+	__disable_irq();
+	memory_pool = data;
+	memory_pool_first_mask = 0;
+	for (i=0; i < NUM_MASKS; i++) {
+		memory_pool_available_mask[i] = 0;
+	}
+	for (i=0; i < num; i++) {
+		memory_pool_available_mask[i >> 5] |= (1 << (i & 0x1F));
+	}
+	for (i=0; i < num; i++) {
+		data[i].memory_pool_index = i;
+	}
+	__enable_irq();
+
+}
+
+// Allocate 1 audio data block.  If successful
+// the caller is the only owner of this new block
+audio_block_t * AudioStream::allocate(void)
+{
+	uint32_t n, index, avail;
+	uint32_t *p, *end;
+	audio_block_t *block;
+	uint32_t used;
+
+	p = memory_pool_available_mask;
+	end = p + NUM_MASKS;
+	__disable_irq();
+	index = memory_pool_first_mask;
+	p += index;
+	while (1) {
+		if (p >= end) {
+			__enable_irq();
+			//Serial.println("alloc:null");
+			return NULL;
+		}
+		avail = *p;
+		if (avail) break;
+		index++;
+		p++;
+	}
+	n = __builtin_clz(avail);
+	avail &= ~(0x80000000 >> n);
+	*p = avail;
+	if (!avail) index++;
+	memory_pool_first_mask = index;
+	used = memory_used + 1;
+	memory_used = used;
+	__enable_irq();
+	index = p - memory_pool_available_mask;
+	block = memory_pool + ((index << 5) + (31 - n));
+	block->ref_count = 1;
+	if (used > memory_used_max) memory_used_max = used;
+	//Serial.print("alloc:");
+	//Serial.println((uint32_t)block, HEX);
+	return block;
+}
+
+// Release ownership of a data block.  If no
+// other streams have ownership, the block is
+// returned to the free pool
+void AudioStream::release(audio_block_t *block)
+{
+	//if (block == NULL) return;
+	uint32_t mask = (0x80000000 >> (31 - (block->memory_pool_index & 0x1F)));
+	uint32_t index = block->memory_pool_index >> 5;
+
+	__disable_irq();
+	if (block->ref_count > 1) {
+		block->ref_count--;
+	} else {
+		//Serial.print("reles:");
+		//Serial.println((uint32_t)block, HEX);
+		memory_pool_available_mask[index] |= mask;
+		if (index < memory_pool_first_mask) memory_pool_first_mask = index;
+		memory_used--;
+	}
+	__enable_irq();
+}
+
+// Transmit an audio data block
+// to all streams that connect to an output.  The block
+// becomes owned by all the recepients, but also is still
+// owned by this object.  Normally, a block must be released
+// by the caller after it's transmitted.  This allows the
+// caller to transmit to same block to more than 1 output,
+// and then release it once after all transmit calls.
+void AudioStream::transmit(audio_block_t *block, unsigned char index)
+{
+	for (AudioConnection *c = destination_list; c != NULL; c = c->next_dest) {
+		if (c->src_index == index) {
+			if (c->dst.inputQueue[c->dest_index] == NULL) {
+				c->dst.inputQueue[c->dest_index] = block;
+				block->ref_count++;
+			}
+		}
+	}
+}
+
+
+// Receive block from an input.  The block's data
+// may be shared with other streams, so it must not be written
+audio_block_t * AudioStream::receiveReadOnly(unsigned int index)
+{
+	audio_block_t *in;
+
+	if (index >= num_inputs) return NULL;
+	in = inputQueue[index];
+	inputQueue[index] = NULL;
+	return in;
+}
+
+// Receive block from an input.  The block will not
+// be shared, so its contents may be changed.
+audio_block_t * AudioStream::receiveWritable(unsigned int index)
+{
+	audio_block_t *in, *p;
+
+	if (index >= num_inputs) return NULL;
+	in = inputQueue[index];
+	inputQueue[index] = NULL;
+	if (in && in->ref_count > 1) {
+		p = allocate();
+		if (p) memcpy(p->data, in->data, sizeof(p->data));
+		in->ref_count--;
+		in = p;
+	}
+	return in;
+}
+
+
+void AudioConnection::connect(void)
+{
+	AudioConnection *p;
+
+	if (isConnected) return;
+	if (dest_index > dst.num_inputs) return;
+	__disable_irq();
+	p = src.destination_list;
+	if (p == NULL) {
+		src.destination_list = this;
+	} else {
+		while (p->next_dest) {
+			if (&p->src == &this->src && &p->dst == &this->dst
+				&& p->src_index == this->src_index && p->dest_index == this->dest_index) {
+				//Source and destination already connected through another connection, abort
+				__enable_irq();
+				return;
+			}
+			p = p->next_dest;
+		}
+		p->next_dest = this;
+	}
+	this->next_dest = NULL;
+	src.numConnections++;
+	src.active = true;
+
+	dst.numConnections++;
+	dst.active = true;
+
+	isConnected = true;
+
+	__enable_irq();
+}
+
+void AudioConnection::disconnect(void)
+{
+	AudioConnection *p;
+
+	if (!isConnected) return;
+	if (dest_index > dst.num_inputs) return;
+	__disable_irq();
+	// Remove destination from source list
+	p = src.destination_list;
+	if (p == NULL) {
+//>>> PAH re-enable the IRQ
+		__enable_irq();
+		return;
+	} else if (p == this) {
+		if (p->next_dest) {
+			src.destination_list = next_dest;
+		} else {
+			src.destination_list = NULL;
+		}
+	} else {
+		while (p) {
+			if (p == this) {
+				if (p->next_dest) {
+					p = next_dest;
+					break;
+				} else {
+					p = NULL;
+					break;
+				}
+			}
+			p = p->next_dest;
+		}
+	}
+//>>> PAH release the audio buffer properly
+	//Remove possible pending src block from destination
+	if(dst.inputQueue[dest_index] != NULL) {
+		AudioStream::release(dst.inputQueue[dest_index]);
+		// release() re-enables the IRQ. Need it to be disabled a little longer
+		__disable_irq();
+		dst.inputQueue[dest_index] = NULL;
+	}
+
+	//Check if the disconnected AudioStream objects should still be active
+	src.numConnections--;
+	if (src.numConnections == 0) {
+		src.active = false;
+	}
+
+	dst.numConnections--;
+	if (dst.numConnections == 0) {
+		dst.active = false;
+	}
+
+	isConnected = false;
+
+	__enable_irq();
+}
+
+
+// When an object has taken responsibility for calling update_all()
+// at each block interval (approx 2.9ms), this variable is set to
+// true.  Objects that are capable of calling update_all(), typically
+// input and output based on interrupts, must check this variable in
+// their constructors.
+bool AudioStream::update_scheduled = false;
+
+bool AudioStream::update_setup(void)
+{
+	if (update_scheduled) return false;
+	attachInterruptVector(IRQ_SOFTWARE, software_isr);
+	NVIC_SET_PRIORITY(IRQ_SOFTWARE, 208); // 255 = lowest priority
+	NVIC_ENABLE_IRQ(IRQ_SOFTWARE);
+	update_scheduled = true;
+	return true;
+}
+
+void AudioStream::update_stop(void)
+{
+	NVIC_DISABLE_IRQ(IRQ_SOFTWARE);
+	update_scheduled = false;
+}
+
+AudioStream * AudioStream::first_update = NULL;
+
+void software_isr(void) // AudioStream::update_all()
+{
+	AudioStream *p;
+
+	uint32_t totalcycles = ARM_DWT_CYCCNT;
+	//digitalWriteFast(2, HIGH);
+	for (p = AudioStream::first_update; p; p = p->next_update) {
+		if (p->active) {
+			uint32_t cycles = ARM_DWT_CYCCNT;
+			p->update();
+			// TODO: traverse inputQueueArray and release
+			// any input blocks that weren't consumed?
+			cycles = (ARM_DWT_CYCCNT - cycles) >> 4;
+			p->cpu_cycles = cycles;
+			if (cycles > p->cpu_cycles_max) p->cpu_cycles_max = cycles;
+		}
+	}
+	//digitalWriteFast(2, LOW);
+	totalcycles = (ARM_DWT_CYCCNT - totalcycles) >> 4;;
+	AudioStream::cpu_cycles_total = totalcycles;
+	if (totalcycles > AudioStream::cpu_cycles_total_max)
+		AudioStream::cpu_cycles_total_max = totalcycles;
+
+	asm("DSB");
+}
+

teensy4/AudioStream.h

@@ -0,0 +1,180 @@
+/* Teensyduino Core Library
+ * http://www.pjrc.com/teensy/
+ * Copyright (c) 2017 PJRC.COM, LLC.
+ *
+ * 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:
+ *
+ * 1. The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * 2. If the Software is incorporated into a build system that allows
+ * selection among a list of target devices, then similar target
+ * devices manufactured by PJRC.COM must be included in the list of
+ * target devices and selectable in the same manner.
+ *
+ * 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 AudioStream_h
+#define AudioStream_h
+
+#ifndef __ASSEMBLER__
+#include <stdio.h>  // for NULL
+#include <string.h> // for memcpy
+
+#endif
+
+// AUDIO_BLOCK_SAMPLES determines how many samples the audio library processes
+// per update.  It may be reduced to achieve lower latency response to events,
+// at the expense of higher interrupt and DMA setup overhead.
+//
+// Less than 32 may not work with some input & output objects.  Multiples of 16
+// should be used, since some synthesis objects generate 16 samples per loop.
+//
+// Some parts of the audio library may have hard-coded dependency on 128 samples.
+// Please report these on the forum with reproducible test cases.
+
+#ifndef AUDIO_BLOCK_SAMPLES
+#define AUDIO_BLOCK_SAMPLES  128
+#endif
+
+#ifndef AUDIO_SAMPLE_RATE_EXACT
+#define AUDIO_SAMPLE_RATE_EXACT 44100.0f
+#endif
+
+#define AUDIO_SAMPLE_RATE AUDIO_SAMPLE_RATE_EXACT
+
+#ifndef __ASSEMBLER__
+class AudioStream;
+class AudioConnection;
+
+typedef struct audio_block_struct {
+	uint8_t  ref_count;
+	uint8_t  reserved1;
+	uint16_t memory_pool_index;
+	int16_t  data[AUDIO_BLOCK_SAMPLES];
+	int16_t x;/////////////////////////////
+} audio_block_t;
+
+
+class AudioConnection
+{
+public:
+	AudioConnection(AudioStream &source, AudioStream &destination) :
+		src(source), dst(destination), src_index(0), dest_index(0),
+		next_dest(NULL)
+		{ isConnected = false;
+		  connect(); }
+	AudioConnection(AudioStream &source, unsigned char sourceOutput,
+		AudioStream &destination, unsigned char destinationInput) :
+		src(source), dst(destination),
+		src_index(sourceOutput), dest_index(destinationInput),
+		next_dest(NULL)
+		{ isConnected = false;
+		  connect(); }
+	friend class AudioStream;
+	~AudioConnection() {
+		disconnect();
+	}
+	void disconnect(void);
+	void connect(void);
+protected:
+	AudioStream &src;
+	AudioStream &dst;
+	unsigned char src_index;
+	unsigned char dest_index;
+	AudioConnection *next_dest;
+	bool isConnected;
+};
+
+
+#define AudioMemory(num) ({ \
+	static DMAMEM audio_block_t data[num]; \
+	AudioStream::initialize_memory(data, num); \
+})
+
+#define CYCLE_COUNTER_APPROX_PERCENT(n) (((n) + (F_CPU_ACTUAL / 32 / AUDIO_SAMPLE_RATE * AUDIO_BLOCK_SAMPLES / 100)) / (F_CPU_ACTUAL / 16 / AUDIO_SAMPLE_RATE * AUDIO_BLOCK_SAMPLES / 100))
+
+#define AudioProcessorUsage() (CYCLE_COUNTER_APPROX_PERCENT(AudioStream::cpu_cycles_total))
+#define AudioProcessorUsageMax() (CYCLE_COUNTER_APPROX_PERCENT(AudioStream::cpu_cycles_total_max))
+#define AudioProcessorUsageMaxReset() (AudioStream::cpu_cycles_total_max = AudioStream::cpu_cycles_total)
+#define AudioMemoryUsage() (AudioStream::memory_used)
+#define AudioMemoryUsageMax() (AudioStream::memory_used_max)
+#define AudioMemoryUsageMaxReset() (AudioStream::memory_used_max = AudioStream::memory_used)
+
+class AudioStream
+{
+public:
+	AudioStream(unsigned char ninput, audio_block_t **iqueue) :
+		num_inputs(ninput), inputQueue(iqueue) {
+			active = false;
+			destination_list = NULL;
+			for (int i=0; i < num_inputs; i++) {
+				inputQueue[i] = NULL;
+			}
+			// add to a simple list, for update_all
+			// TODO: replace with a proper data flow analysis in update_all
+			if (first_update == NULL) {
+				first_update = this;
+			} else {
+				AudioStream *p;
+				for (p=first_update; p->next_update; p = p->next_update) ;
+				p->next_update = this;
+			}
+			next_update = NULL;
+			cpu_cycles = 0;
+			cpu_cycles_max = 0;
+			numConnections = 0;
+		}
+	static void initialize_memory(audio_block_t *data, unsigned int num);
+	int processorUsage(void) { return CYCLE_COUNTER_APPROX_PERCENT(cpu_cycles); }
+	int processorUsageMax(void) { return CYCLE_COUNTER_APPROX_PERCENT(cpu_cycles_max); }
+	void processorUsageMaxReset(void) { cpu_cycles_max = cpu_cycles; }
+	bool isActive(void) { return active; }
+	uint16_t cpu_cycles;
+	uint16_t cpu_cycles_max;
+	static uint16_t cpu_cycles_total;
+	static uint16_t cpu_cycles_total_max;
+	static uint16_t memory_used;
+	static uint16_t memory_used_max;
+protected:
+	bool active;
+	unsigned char num_inputs;
+	static audio_block_t * allocate(void);
+	static void release(audio_block_t * block);
+	void transmit(audio_block_t *block, unsigned char index = 0);
+	audio_block_t * receiveReadOnly(unsigned int index = 0);
+	audio_block_t * receiveWritable(unsigned int index = 0);
+	static bool update_setup(void);
+	static void update_stop(void);
+	static void update_all(void) { NVIC_SET_PENDING(IRQ_SOFTWARE); }
+	friend void software_isr(void);
+	friend class AudioConnection;
+	uint8_t numConnections;
+private:
+	AudioConnection *destination_list;
+	audio_block_t **inputQueue;
+	static bool update_scheduled;
+	virtual void update(void) = 0;
+	static AudioStream *first_update; // for update_all
+	AudioStream *next_update; // for update_all
+	static audio_block_t *memory_pool;
+	static uint32_t memory_pool_available_mask[];
+	static uint16_t memory_pool_first_mask;
+};
+
+#endif
+#endif

teensy4/math_helper.h

@@ -0,0 +1,63 @@
+/* ----------------------------------------------------------------------
+* Copyright (C) 2010 ARM Limited. All rights reserved.
+*
+* $Date:        29. November 2010
+* $Revision: 	V1.0.3
+*
+* Project: 	    CMSIS DSP Library
+*
+* Title:	    math_helper.h
+*
+*
+* Description:	Prototypes of all helper functions required.
+*
+* Target Processor: Cortex-M4/Cortex-M3
+*
+* Version 1.0.3 2010/11/29
+*    Re-organized the CMSIS folders and updated documentation.
+*
+* Version 1.0.2 2010/11/11
+*    Documentation updated.
+*
+* Version 1.0.1 2010/10/05
+*    Production release and review comments incorporated.
+*
+* Version 1.0.0 2010/09/20
+*    Production release and review comments incorporated.
+*
+* Version 0.0.7  2010/06/10
+*    Misra-C changes done
+* -------------------------------------------------------------------- */
+
+
+#include "arm_math.h"
+
+#ifndef MATH_HELPER_H
+#define MATH_HELPER_H
+
+#ifdef  __cplusplus
+extern "C"
+{
+#endif
+
+float arm_snr_f32(float *pRef, float *pTest,  uint32_t buffSize);
+void arm_float_to_q12_20(float *pIn, q31_t * pOut, uint32_t numSamples);
+void arm_provide_guard_bits_q15(q15_t *input_buf, uint32_t blockSize, uint32_t guard_bits);
+void arm_provide_guard_bits_q31(q31_t *input_buf, uint32_t blockSize, uint32_t guard_bits);
+void arm_float_to_q14(float *pIn, q15_t *pOut, uint32_t numSamples);
+void arm_float_to_q29(float *pIn, q31_t *pOut, uint32_t numSamples);
+void arm_float_to_q28(float *pIn, q31_t *pOut, uint32_t numSamples);
+void arm_float_to_q30(float *pIn, q31_t *pOut, uint32_t numSamples);
+void arm_clip_f32(float *pIn, uint32_t numSamples);
+uint32_t arm_calc_guard_bits(uint32_t num_adds);
+void arm_apply_guard_bits (float32_t * pIn, uint32_t numSamples, uint32_t guard_bits);
+uint32_t arm_compare_fixed_q15(q15_t *pIn, q15_t * pOut, uint32_t numSamples);
+uint32_t arm_compare_fixed_q31(q31_t *pIn, q31_t *pOut, uint32_t numSamples);
+uint32_t arm_calc_2pow(uint32_t guard_bits);
+
+#ifdef  __cplusplus
+}
+#endif
+
+#endif
+