Add AudioEffectFreeverbStereo

effect_freeverb.cpp

@@ -220,3 +220,261 @@ void AudioEffectFreeverb::update()
 #endif
 }
 
+
+AudioEffectFreeverbStereo::AudioEffectFreeverbStereo() : AudioStream(1, inputQueueArray)
+{
+	memset(comb1bufL, 0, sizeof(comb1bufL));
+	memset(comb2bufL, 0, sizeof(comb2bufL));
+	memset(comb3bufL, 0, sizeof(comb3bufL));
+	memset(comb4bufL, 0, sizeof(comb4bufL));
+	memset(comb5bufL, 0, sizeof(comb5bufL));
+	memset(comb6bufL, 0, sizeof(comb6bufL));
+	memset(comb7bufL, 0, sizeof(comb7bufL));
+	memset(comb8bufL, 0, sizeof(comb8bufL));
+	comb1indexL = 0;
+	comb2indexL = 0;
+	comb3indexL = 0;
+	comb4indexL = 0;
+	comb5indexL = 0;
+	comb6indexL = 0;
+	comb7indexL = 0;
+	comb8indexL = 0;
+	comb1filterL = 0;
+	comb2filterL = 0;
+	comb3filterL = 0;
+	comb4filterL = 0;
+	comb5filterL = 0;
+	comb6filterL = 0;
+	comb7filterL = 0;
+	comb8filterL = 0;
+	memset(comb1bufR, 0, sizeof(comb1bufR));
+	memset(comb2bufR, 0, sizeof(comb2bufR));
+	memset(comb3bufR, 0, sizeof(comb3bufR));
+	memset(comb4bufR, 0, sizeof(comb4bufR));
+	memset(comb5bufR, 0, sizeof(comb5bufR));
+	memset(comb6bufR, 0, sizeof(comb6bufR));
+	memset(comb7bufR, 0, sizeof(comb7bufR));
+	memset(comb8bufR, 0, sizeof(comb8bufR));
+	comb1indexR = 0;
+	comb2indexR = 0;
+	comb3indexR = 0;
+	comb4indexR = 0;
+	comb5indexR = 0;
+	comb6indexR = 0;
+	comb7indexR = 0;
+	comb8indexR = 0;
+	comb1filterR = 0;
+	comb2filterR = 0;
+	comb3filterR = 0;
+	comb4filterR = 0;
+	comb5filterR = 0;
+	comb6filterR = 0;
+	comb7filterR = 0;
+	comb8filterR = 0;
+	combdamp1 = 6553;
+	combdamp2 = 26215;
+	combfeeback = 27524;
+	memset(allpass1bufL, 0, sizeof(allpass1bufL));
+	memset(allpass2bufL, 0, sizeof(allpass2bufL));
+	memset(allpass3bufL, 0, sizeof(allpass3bufL));
+	memset(allpass4bufL, 0, sizeof(allpass4bufL));
+	allpass1indexL = 0;
+	allpass2indexL = 0;
+	allpass3indexL = 0;
+	allpass4indexL = 0;
+	memset(allpass1bufR, 0, sizeof(allpass1bufR));
+	memset(allpass2bufR, 0, sizeof(allpass2bufR));
+	memset(allpass3bufR, 0, sizeof(allpass3bufR));
+	memset(allpass4bufR, 0, sizeof(allpass4bufR));
+	allpass1indexR = 0;
+	allpass2indexR = 0;
+	allpass3indexR = 0;
+	allpass4indexR = 0;
+}
+
+void AudioEffectFreeverbStereo::update()
+{
+#if defined(KINETISK)
+	const audio_block_t *block;
+	audio_block_t *outblockL;
+	audio_block_t *outblockR;
+	int i;
+	int16_t input, bufout, outputL, outputR;
+	int32_t sum;
+
+	block = receiveReadOnly(0);
+	outblockL = allocate();
+	outblockR = allocate();
+	if (!outblockL || !outblockR) {
+		if (outblockL) release(outblockL);
+		if (outblockR) release(outblockR);
+		if (block) release((audio_block_t *)block);
+		return;
+	}
+	if (!block) block = &zeroblock;
+
+	for (i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
+		// TODO: scale numerical range depending on roomsize & damping
+		input = sat16(block->data[i] * 8738, 17); // for numerical headroom
+		sum = 0;
+
+		bufout = comb1bufL[comb1indexL];
+		sum += bufout;
+		comb1filterL = sat16(bufout * combdamp2 + comb1filterL * combdamp1, 15);
+		comb1bufL[comb1indexL] = sat16(input + sat16(comb1filterL * combfeeback, 15), 0);
+		if (++comb1indexL >= sizeof(comb1bufL)/sizeof(int16_t)) comb1indexL = 0;
+
+		bufout = comb2bufL[comb2indexL];
+		sum += bufout;
+		comb2filterL = sat16(bufout * combdamp2 + comb2filterL * combdamp1, 15);
+		comb2bufL[comb2indexL] = sat16(input + sat16(comb2filterL * combfeeback, 15), 0);
+		if (++comb2indexL >= sizeof(comb2bufL)/sizeof(int16_t)) comb2indexL = 0;
+
+		bufout = comb3bufL[comb3indexL];
+		sum += bufout;
+		comb3filterL = sat16(bufout * combdamp2 + comb3filterL * combdamp1, 15);
+		comb3bufL[comb3indexL] = sat16(input + sat16(comb3filterL * combfeeback, 15), 0);
+		if (++comb3indexL >= sizeof(comb3bufL)/sizeof(int16_t)) comb3indexL = 0;
+
+		bufout = comb4bufL[comb4indexL];
+		sum += bufout;
+		comb4filterL = sat16(bufout * combdamp2 + comb4filterL * combdamp1, 15);
+		comb4bufL[comb4indexL] = sat16(input + sat16(comb4filterL * combfeeback, 15), 0);
+		if (++comb4indexL >= sizeof(comb4bufL)/sizeof(int16_t)) comb4indexL = 0;
+
+		bufout = comb5bufL[comb5indexL];
+		sum += bufout;
+		comb5filterL = sat16(bufout * combdamp2 + comb5filterL * combdamp1, 15);
+		comb5bufL[comb5indexL] = sat16(input + sat16(comb5filterL * combfeeback, 15), 0);
+		if (++comb5indexL >= sizeof(comb5bufL)/sizeof(int16_t)) comb5indexL = 0;
+
+		bufout = comb6bufL[comb6indexL];
+		sum += bufout;
+		comb6filterL = sat16(bufout * combdamp2 + comb6filterL * combdamp1, 15);
+		comb6bufL[comb6indexL] = sat16(input + sat16(comb6filterL * combfeeback, 15), 0);
+		if (++comb6indexL >= sizeof(comb6bufL)/sizeof(int16_t)) comb6indexL = 0;
+
+		bufout = comb7bufL[comb7indexL];
+		sum += bufout;
+		comb7filterL = sat16(bufout * combdamp2 + comb7filterL * combdamp1, 15);
+		comb7bufL[comb7indexL] = sat16(input + sat16(comb7filterL * combfeeback, 15), 0);
+		if (++comb7indexL >= sizeof(comb7bufL)/sizeof(int16_t)) comb7indexL = 0;
+
+		bufout = comb8bufL[comb8indexL];
+		sum += bufout;
+		comb8filterL = sat16(bufout * combdamp2 + comb8filterL * combdamp1, 15);
+		comb8bufL[comb8indexL] = sat16(input + sat16(comb8filterL * combfeeback, 15), 0);
+		if (++comb8indexL >= sizeof(comb8bufL)/sizeof(int16_t)) comb8indexL = 0;
+
+		outputL = sat16(sum * 31457, 17);
+		sum = 0;
+
+		bufout = comb1bufR[comb1indexR];
+		sum += bufout;
+		comb1filterR = sat16(bufout * combdamp2 + comb1filterR * combdamp1, 15);
+		comb1bufR[comb1indexR] = sat16(input + sat16(comb1filterR * combfeeback, 15), 0);
+		if (++comb1indexR >= sizeof(comb1bufR)/sizeof(int16_t)) comb1indexR = 0;
+
+		bufout = comb2bufR[comb2indexR];
+		sum += bufout;
+		comb2filterR = sat16(bufout * combdamp2 + comb2filterR * combdamp1, 15);
+		comb2bufR[comb2indexR] = sat16(input + sat16(comb2filterR * combfeeback, 15), 0);
+		if (++comb2indexR >= sizeof(comb2bufR)/sizeof(int16_t)) comb2indexR = 0;
+
+		bufout = comb3bufR[comb3indexR];
+		sum += bufout;
+		comb3filterR = sat16(bufout * combdamp2 + comb3filterR * combdamp1, 15);
+		comb3bufR[comb3indexR] = sat16(input + sat16(comb3filterR * combfeeback, 15), 0);
+		if (++comb3indexR >= sizeof(comb3bufR)/sizeof(int16_t)) comb3indexR = 0;
+
+		bufout = comb4bufR[comb4indexR];
+		sum += bufout;
+		comb4filterR = sat16(bufout * combdamp2 + comb4filterR * combdamp1, 15);
+		comb4bufR[comb4indexR] = sat16(input + sat16(comb4filterR * combfeeback, 15), 0);
+		if (++comb4indexR >= sizeof(comb4bufR)/sizeof(int16_t)) comb4indexR = 0;
+
+		bufout = comb5bufR[comb5indexR];
+		sum += bufout;
+		comb5filterR = sat16(bufout * combdamp2 + comb5filterR * combdamp1, 15);
+		comb5bufR[comb5indexR] = sat16(input + sat16(comb5filterR * combfeeback, 15), 0);
+		if (++comb5indexR >= sizeof(comb5bufR)/sizeof(int16_t)) comb5indexR = 0;
+
+		bufout = comb6bufR[comb6indexR];
+		sum += bufout;
+		comb6filterR = sat16(bufout * combdamp2 + comb6filterR * combdamp1, 15);
+		comb6bufR[comb6indexR] = sat16(input + sat16(comb6filterR * combfeeback, 15), 0);
+		if (++comb6indexR >= sizeof(comb6bufR)/sizeof(int16_t)) comb6indexR = 0;
+
+		bufout = comb7bufR[comb7indexR];
+		sum += bufout;
+		comb7filterR = sat16(bufout * combdamp2 + comb7filterR * combdamp1, 15);
+		comb7bufR[comb7indexR] = sat16(input + sat16(comb7filterR * combfeeback, 15), 0);
+		if (++comb7indexR >= sizeof(comb7bufR)/sizeof(int16_t)) comb7indexR = 0;
+
+		bufout = comb8bufR[comb8indexR];
+		sum += bufout;
+		comb8filterR = sat16(bufout * combdamp2 + comb8filterR * combdamp1, 15);
+		comb8bufR[comb8indexR] = sat16(input + sat16(comb8filterR * combfeeback, 15), 0);
+		if (++comb8indexR >= sizeof(comb8bufR)/sizeof(int16_t)) comb8indexR = 0;
+
+		outputR = sat16(sum * 31457, 17);
+
+		bufout = allpass1bufL[allpass1indexL];
+		allpass1bufL[allpass1indexL] = outputL + (bufout >> 1);
+		outputL = sat16(bufout - outputL, 1);
+		if (++allpass1indexL >= sizeof(allpass1bufL)/sizeof(int16_t)) allpass1indexL = 0;
+
+		bufout = allpass2bufL[allpass2indexL];
+		allpass2bufL[allpass2indexL] = outputL + (bufout >> 1);
+		outputL = sat16(bufout - outputL, 1);
+		if (++allpass2indexL >= sizeof(allpass2bufL)/sizeof(int16_t)) allpass2indexL = 0;
+
+		bufout = allpass3bufL[allpass3indexL];
+		allpass3bufL[allpass3indexL] = outputL + (bufout >> 1);
+		outputL = sat16(bufout - outputL, 1);
+		if (++allpass3indexL >= sizeof(allpass3bufL)/sizeof(int16_t)) allpass3indexL = 0;
+
+		bufout = allpass4bufL[allpass4indexL];
+		allpass4bufL[allpass4indexL] = outputL + (bufout >> 1);
+		outputL = sat16(bufout - outputL, 1);
+		if (++allpass4indexL >= sizeof(allpass4bufL)/sizeof(int16_t)) allpass4indexL = 0;
+
+		outblockL->data[i] = sat16(outputL * 30, 0);
+
+		bufout = allpass1bufR[allpass1indexR];
+		allpass1bufR[allpass1indexR] = outputR + (bufout >> 1);
+		outputR = sat16(bufout - outputR, 1);
+		if (++allpass1indexR >= sizeof(allpass1bufR)/sizeof(int16_t)) allpass1indexR = 0;
+
+		bufout = allpass2bufR[allpass2indexR];
+		allpass2bufR[allpass2indexR] = outputR + (bufout >> 1);
+		outputR = sat16(bufout - outputR, 1);
+		if (++allpass2indexR >= sizeof(allpass2bufR)/sizeof(int16_t)) allpass2indexR = 0;
+
+		bufout = allpass3bufR[allpass3indexR];
+		allpass3bufR[allpass3indexR] = outputR + (bufout >> 1);
+		outputR = sat16(bufout - outputR, 1);
+		if (++allpass3indexR >= sizeof(allpass3bufR)/sizeof(int16_t)) allpass3indexR = 0;
+
+		bufout = allpass4bufR[allpass4indexR];
+		allpass4bufR[allpass4indexR] = outputR + (bufout >> 1);
+		outputR = sat16(bufout - outputR, 1);
+		if (++allpass4indexR >= sizeof(allpass4bufR)/sizeof(int16_t)) allpass4indexR = 0;
+
+		outblockR->data[i] = sat16(outputL * 30, 0);
+	}
+	transmit(outblockL, 0);
+	transmit(outblockR, 1);
+	release(outblockL);
+	release(outblockR);
+	if (block != &zeroblock) release((audio_block_t *)block);
+
+#elif defined(KINETISL)
+	audio_block_t *block;
+	block = receiveReadOnly(0);
+	if (block) release(block);
+#endif
+}
+
+
+

effect_freeverb.h

@@ -50,7 +50,7 @@ public:
 		__enable_irq();
 	}
 private:
-	audio_block_t *inputQueueArray[2];
+	audio_block_t *inputQueueArray[1];
 	int16_t comb1buf[1116];
 	int16_t comb2buf[1188];
 	int16_t comb3buf[1277];
@@ -88,5 +88,98 @@ private:
 	uint16_t allpass4index;
 };
 
+
+class AudioEffectFreeverbStereo : public AudioStream
+{
+public:
+	AudioEffectFreeverbStereo();
+	virtual void update();
+	void roomsize(float n) {
+		if (n > 1.0f) n = 1.0f;
+		else if (n < 0.0) n = 0.0f;
+		combfeeback = (int)(n * 9175.04f) + 22937;
+	}
+	void damping(float n) {
+		if (n > 1.0f) n = 1.0f;
+		else if (n < 0.0) n = 0.0f;
+		int x1 = (int)(n * 13107.2f);
+		int x2 = 32768 - x1;
+		__disable_irq();
+		combdamp1 = x1;
+		combdamp2 = x2;
+		__enable_irq();
+	}
+private:
+	audio_block_t *inputQueueArray[1];
+	int16_t comb1bufL[1116];
+	int16_t comb2bufL[1188];
+	int16_t comb3bufL[1277];
+	int16_t comb4bufL[1356];
+	int16_t comb5bufL[1422];
+	int16_t comb6bufL[1491];
+	int16_t comb7bufL[1557];
+	int16_t comb8bufL[1617];
+	uint16_t comb1indexL;
+	uint16_t comb2indexL;
+	uint16_t comb3indexL;
+	uint16_t comb4indexL;
+	uint16_t comb5indexL;
+	uint16_t comb6indexL;
+	uint16_t comb7indexL;
+	uint16_t comb8indexL;
+	int16_t comb1filterL;
+	int16_t comb2filterL;
+	int16_t comb3filterL;
+	int16_t comb4filterL;
+	int16_t comb5filterL;
+	int16_t comb6filterL;
+	int16_t comb7filterL;
+	int16_t comb8filterL;
+	int16_t comb1bufR[1139];
+	int16_t comb2bufR[1211];
+	int16_t comb3bufR[1300];
+	int16_t comb4bufR[1379];
+	int16_t comb5bufR[1445];
+	int16_t comb6bufR[1514];
+	int16_t comb7bufR[1580];
+	int16_t comb8bufR[1640];
+	uint16_t comb1indexR;
+	uint16_t comb2indexR;
+	uint16_t comb3indexR;
+	uint16_t comb4indexR;
+	uint16_t comb5indexR;
+	uint16_t comb6indexR;
+	uint16_t comb7indexR;
+	uint16_t comb8indexR;
+	int16_t comb1filterR;
+	int16_t comb2filterR;
+	int16_t comb3filterR;
+	int16_t comb4filterR;
+	int16_t comb5filterR;
+	int16_t comb6filterR;
+	int16_t comb7filterR;
+	int16_t comb8filterR;
+	int16_t combdamp1;
+	int16_t combdamp2;
+	int16_t combfeeback;
+	int16_t allpass1bufL[556];
+	int16_t allpass2bufL[441];
+	int16_t allpass3bufL[341];
+	int16_t allpass4bufL[225];
+	uint16_t allpass1indexL;
+	uint16_t allpass2indexL;
+	uint16_t allpass3indexL;
+	uint16_t allpass4indexL;
+	int16_t allpass1bufR[579];
+	int16_t allpass2bufR[464];
+	int16_t allpass3bufR[364];
+	int16_t allpass4bufR[248];
+	uint16_t allpass1indexR;
+	uint16_t allpass2indexR;
+	uint16_t allpass3indexR;
+	uint16_t allpass4indexR;
+};
+
+
 #endif
 

keywords.txt

@@ -39,6 +39,7 @@ AudioEffectDelayExternal	KEYWORD2
 AudioEffectBitcrusher	KEYWORD2
 AudioEffectReverb	KEYWORD2
 AudioEffectFreeverb	KEYWORD2
+AudioEffectFreeverbStereo	KEYWORD2
 AudioEffectMidSide	KEYWORD2
 AudioEffectWaveshaper	KEYWORD2
 AudioFilterBiquad	KEYWORD2