Fixed module loading (again), only audio tx works for now

firmware/baseband-tx/Makefile

@@ -1,3 +1,4 @@
+
 #
 # Copyright (C) 2014 Jared Boone, ShareBrained Technology, Inc.
 #
@@ -126,6 +127,7 @@ CPPSRC = main.cpp \
          message_queue.cpp \
          event.cpp \
          event_m4.cpp \
+         thread_wait.cpp \
          gpdma.cpp \
          baseband_dma.cpp \
          baseband_sgpio.cpp \
@@ -136,11 +138,6 @@ CPPSRC = main.cpp \
          dsp_decimate.cpp \
          dsp_demodulate.cpp \
          matched_filter.cpp \
-         spectrum_collector.cpp \
-         proc_rds.cpp \
-         proc_jammer.cpp \
-         proc_fsk_lcr.cpp \
-         proc_xylos.cpp \
          proc_audiotx.cpp \
          proc_playaudio.cpp \
          dsp_squelch.cpp \
@@ -153,7 +150,7 @@ CPPSRC = main.cpp \
          rssi.cpp \
          rssi_dma.cpp \
          rssi_thread.cpp \
-         audio.cpp \
+         audio_compressor.cpp \
          audio_output.cpp \
          audio_dma.cpp \
          audio_stats_collector.cpp \

firmware/baseband-tx/audio_dma.hpp

@@ -24,9 +24,22 @@
 
 #include <cstdint>
 
-#include "audio.hpp"
+#include "buffer.hpp"
 
 namespace audio {
+
+struct sample_t {
+	union {
+		struct {
+			int16_t left;
+			int16_t right;
+		};
+		uint32_t raw;
+	};
+};
+
+using buffer_t = buffer_t<sample_t>;
+
 namespace dma {
 
 void init();

firmware/baseband-tx/audio_output.cpp

@@ -46,7 +46,7 @@ void AudioOutput::write(
 ) {
 	std::array<float, 32> audio_f;
 	for(size_t i=0; i<audio.count; i++) {
-		audio_f[i] = audio.p[i];
+		audio_f[i] = audio.p[i] * ki;
 	}
 	write(buffer_f32_t {
 		audio_f.data(),
@@ -57,6 +57,17 @@ void AudioOutput::write(
 
 void AudioOutput::write(
 	const buffer_f32_t& audio
+) {
+	block_buffer.feed(
+		audio,
+		[this](const buffer_f32_t& buffer) {
+			this->on_block(buffer);
+		}
+	);
+}
+
+void AudioOutput::on_block(
+	const buffer_f32_t& audio
 ) {
 	const auto audio_present_now = squelch.execute(audio);
 
@@ -66,24 +77,27 @@ void AudioOutput::write(
 	audio_present_history = (audio_present_history << 1) | (audio_present_now ? 1 : 0);
 	const bool audio_present = (audio_present_history != 0);
 	
-	if( audio_present ) {
-		i2s::i2s0::tx_unmute();
-	} else {
-		i2s::i2s0::tx_mute();
+	if( !audio_present ) {
 		for(size_t i=0; i<audio.count; i++) {
 			audio.p[i] = 0;
 		}
 	}
 
-	fill_audio_buffer(audio);
+	fill_audio_buffer(audio, audio_present);
 }
 
-void AudioOutput::fill_audio_buffer(const buffer_f32_t& audio) {
+void AudioOutput::fill_audio_buffer(const buffer_f32_t& audio, const bool send_to_fifo) {
+	std::array<int16_t, 32> audio_int;
+
 	auto audio_buffer = audio::dma::tx_empty_buffer();
 	for(size_t i=0; i<audio_buffer.count; i++) {
-		const int32_t sample_int = audio.p[i];
+		const int32_t sample_int = audio.p[i] * k;
 		const int32_t sample_saturated = __SSAT(sample_int, 16);
 		audio_buffer.p[i].left = audio_buffer.p[i].right = sample_saturated;
+		audio_int[i] = sample_saturated;
+	}
+	if( send_to_fifo ) {
+		stream.write(audio_int.data(), audio_buffer.count * sizeof(audio_int[0]));
 	}
 
 	feed_audio_stats(audio);

firmware/baseband-tx/audio_output.hpp

@@ -27,6 +27,8 @@
 #include "dsp_iir.hpp"
 #include "dsp_squelch.hpp"
 
+#include "stream_input.hpp"
+#include "block_decimator.hpp"
 #include "audio_stats_collector.hpp"
 
 #include <cstdint>
@@ -43,15 +45,23 @@ public:
 	void write(const buffer_f32_t& audio);
 
 private:
+	static constexpr float k = 32768.0f;
+	static constexpr float ki = 1.0f / k;
+
+	BlockDecimator<float, 32> block_buffer { 1 };	
+
 	IIRBiquadFilter hpf;
 	IIRBiquadFilter deemph;
 	FMSquelch squelch;
 
+	StreamInput stream { 14 };
+
 	AudioStatsCollector audio_stats;
 
 	uint64_t audio_present_history = 0;
 
-	void fill_audio_buffer(const buffer_f32_t& audio);
+	void on_block(const buffer_f32_t& audio);
+	void fill_audio_buffer(const buffer_f32_t& audio, const bool send_to_fifo);
 	void feed_audio_stats(const buffer_f32_t& audio);
 };
 

firmware/baseband-tx/audio_stats_collector.cpp

@@ -42,8 +42,8 @@ bool AudioStatsCollector::update_stats(const size_t sample_count, const size_t s
 	const size_t samples_per_update = sampling_rate * update_interval;
 
 	if( count >= samples_per_update ) {
-		statistics.rms_db = complex16_mag_squared_to_dbv_norm(squared_sum / count);
-		statistics.max_db = complex16_mag_squared_to_dbv_norm(max_squared);
+		statistics.rms_db = mag2_to_dbv_norm(squared_sum / count);
+		statistics.max_db = mag2_to_dbv_norm(max_squared);
 		statistics.count = count;
 
 		squared_sum = 0;

firmware/baseband-tx/baseband_dma.cpp

@@ -32,6 +32,8 @@ using namespace lpc43xx;
 
 #include "portapack_dma.hpp"
 
+#include "thread_wait.hpp"
+
 namespace baseband {
 namespace dma {
 
@@ -99,21 +101,19 @@ constexpr size_t msg_count = transfers_per_buffer - 1;
 static std::array<gpdma::channel::LLI, transfers_per_buffer> lli_loop;
 static constexpr auto& gpdma_channel_sgpio = gpdma::channels[portapack::sgpio_gpdma_channel_number];
 
-static Semaphore semaphore;
-
-static volatile const gpdma::channel::LLI* next_lli = nullptr;
+static ThreadWait thread_wait;
 
 static void transfer_complete() {
-	next_lli = gpdma_channel_sgpio.next_lli();
-	chSemSignalI(&semaphore);
+	const auto next_lli_index = gpdma_channel_sgpio.next_lli() - &lli_loop[0];
+	thread_wait.wake_from_interrupt(next_lli_index);
 }
 
 static void dma_error() {
+	thread_wait.wake_from_interrupt(-1);
 	disable();
 }
 
 void init() {
-	chSemInit(&semaphore, 0);
 	gpdma_channel_sgpio.set_handlers(transfer_complete, dma_error);
 
 	// LPC_GPDMA->SYNC |= (1 << gpdma_src_peripheral);
@@ -138,9 +138,6 @@ void configure(
 void enable(const baseband::Direction direction) {
 	const auto gpdma_config = config(direction);
 	gpdma_channel_sgpio.configure(lli_loop[0], gpdma_config);
-
-	chSemReset(&semaphore, 0);
-
 	gpdma_channel_sgpio.enable();
 }
 
@@ -153,16 +150,22 @@ void disable() {
 }
 
 baseband::buffer_t wait_for_rx_buffer() {
-	const auto status = chSemWait(&semaphore);
-	if( status == RDY_OK ) {
-		const auto next = next_lli;
-		if( next ) {
-			const size_t next_index = next - &lli_loop[0];
-			const size_t free_index = (next_index + transfers_per_buffer - 2) & transfers_mask;
-			return { reinterpret_cast<sample_t*>(lli_loop[free_index].destaddr), transfer_samples };
-		} else {
-			return { };
-		}
+	const auto next_index = thread_wait.sleep();
+	
+	if( next_index >= 0 ) {
+		const size_t free_index = (next_index + transfers_per_buffer - 2) & transfers_mask;
+		return { reinterpret_cast<sample_t*>(lli_loop[free_index].destaddr), transfer_samples };
+	} else {
+		return { };
+	}
+}
+
+baseband::buffer_t wait_for_tx_buffer() {
+	const auto next_index = thread_wait.sleep();
+	
+	if( next_index >= 0 ) {
+		const size_t free_index = (next_index + transfers_per_buffer - 2) & transfers_mask;
+		return { reinterpret_cast<sample_t*>(lli_loop[free_index].srcaddr), transfer_samples };
 	} else {
 		return { };
 	}

firmware/baseband-tx/baseband_thread.cpp

@@ -31,11 +31,8 @@
 #include "rssi.hpp"
 #include "i2s.hpp"
 
-#include "proc_xylos.hpp"
-#include "proc_fsk_lcr.hpp"
-#include "proc_jammer.hpp"
-#include "proc_rds.hpp"
 #include "proc_playaudio.hpp"
+#include "proc_audiotx.hpp"
 
 #include "portapack_shared_memory.hpp"
 
@@ -83,7 +80,7 @@ void BasebandThread::run() {
 	baseband_sgpio.init();
 	baseband::dma::init();
 
-	const auto baseband_buffer = new std::array<baseband::sample_t, 8192>();
+	const auto baseband_buffer = std::make_unique<std::array<baseband::sample_t, 8192>>();
 	baseband::dma::configure(
 		baseband_buffer->data(),
 		direction()
@@ -99,7 +96,7 @@ void BasebandThread::run() {
 
 	while(true) {
 		// TODO: Place correct sampling rate into buffer returned here:
-		const auto buffer_tmp = baseband::dma::wait_for_rx_buffer();
+		const auto buffer_tmp = baseband::dma::wait_for_tx_buffer();
 		if( buffer_tmp ) {
 			buffer_c8_t buffer {
 				buffer_tmp.p, buffer_tmp.count, baseband_configuration.sampling_rate
@@ -117,19 +114,12 @@ void BasebandThread::run() {
 			);
 		}
 	}
-
-	delete baseband_buffer;
 }
 
 BasebandProcessor* BasebandThread::create_processor(const int32_t mode) {
 	switch(mode) {
-	case 0:		return new RDSProcessor();
-	case 1:		return new LCRFSKProcessor();
-	case 2:		return nullptr; //new ToneProcessor();
-	case 3:		return new JammerProcessor();
-	case 4:		return new XylosProcessor();
-	case 5:		return new PlayAudioProcessor();
-	case 6:		return nullptr; //new AFSKRXProcessor();
+	case 0:		return new PlayAudioProcessor();
+	case 1:		return new AudioTXProcessor();
 	default:	return nullptr;
 	}
 }
@@ -145,9 +135,6 @@ void BasebandThread::disable() {
 
 void BasebandThread::enable() {
 	if( baseband_processor ) {
-		if( direction() == baseband::Direction::Receive ) {
-			rf::rssi::start();
-		}
 		baseband_sgpio.configure(direction());
 		baseband::dma::enable(direction());
 		baseband_sgpio.streaming_enable();

firmware/baseband-tx/baseband_thread.hpp

@@ -30,11 +30,6 @@
 
 class BasebandThread : public ThreadBase {
 public:
-	BasebandThread(
-	) : ThreadBase { "baseband" }
-	{
-	}
-
 	Thread* start(const tprio_t priority);
 
 	void on_message(const Message* const message);
@@ -42,14 +37,17 @@ public:
 	// This getter should die, it's just here to leak information to code that
 	// isn't in the right place to begin with.
 	baseband::Direction direction() const {
-		return baseband::Direction::Receive;
+		return baseband::Direction::Transmit;
 	}
 
+	void wait_for_switch(void);
+
 	Thread* thread_main { nullptr };
 	Thread* thread_rssi { nullptr };
-	BasebandProcessor* baseband_processor { nullptr };
 
 private:
+	BasebandProcessor* baseband_processor { nullptr };
+
 	BasebandConfiguration baseband_configuration;
 
 	void run() override;

firmware/baseband-tx/block_decimator.hpp

@@ -29,7 +29,7 @@
 #include "dsp_types.hpp"
 #include "complex.hpp"
 
-template<size_t N>
+template<typename T, size_t N>
 class BlockDecimator {
 public:
 	constexpr BlockDecimator(
@@ -65,7 +65,7 @@ public:
 	}
 
 	template<typename BlockCallback>
-	void feed(const buffer_c16_t src, BlockCallback callback) {
+	void feed(const buffer_t<T>& src, BlockCallback callback) {
 		/* NOTE: Input block size must be >= factor */
 
 		set_input_sampling_rate(src.sampling_rate);
@@ -85,7 +85,7 @@ public:
 	}
 
 private:
-	std::array<complex16_t, N> buffer;
+	std::array<T, N> buffer;
 	uint32_t input_sampling_rate_ { 0 };
 	size_t factor_ { 1 };
 	size_t src_i { 0 };

firmware/baseband-tx/channel_decimator.cpp

@@ -21,7 +21,7 @@
 
 #include "channel_decimator.hpp"
 
-buffer_c16_t ChannelDecimator::execute_decimation(buffer_c8_t buffer) {
+buffer_c16_t ChannelDecimator::execute_decimation(const buffer_c8_t& buffer) {
 	const buffer_c16_t work_baseband_buffer {
 		work_baseband.data(),
 		work_baseband.size()
@@ -39,19 +39,15 @@ buffer_c16_t ChannelDecimator::execute_decimation(buffer_c8_t buffer) {
 	 * -> gain of 256
 	 * -> decimation by 2
 	 * -> 1.544MHz complex<int16_t>[1024], [-32768, 32512] */
-	const auto stage_0_out = translate.execute(buffer, work_baseband_buffer);
-
-	//if( fs_over_4_downconvert ) {
-	//	// TODO:
-	//} else {
-	// Won't work until cic_0 will accept input type of buffer_c8_t.
-	//	stage_0_out = cic_0.execute(buffer, work_baseband_buffer);
-	//}
+	auto stage_0_out = execute_stage_0(buffer, work_baseband_buffer);
+	if( decimation_factor == DecimationFactor::By2 ) {
+		return stage_0_out;
+	}
 
 	/* 1.536MHz complex<int16_t>[1024], [-32768, 32512]
 	 * -> 3rd order CIC: -0.1dB @ 0.028fs, -1dB @ 0.088fs, -60dB @ 0.468fs
 	 *                   -0.1dB @ 43kHz,   -1dB @ 136kHz,  -60dB @ 723kHz
-	 * -> gain of 8
+	 * -> gain of 1
 	 * -> decimation by 2
 	 * -> 768kHz complex<int16_t>[512], [-8192, 8128] */
 	auto cic_1_out = cic_1.execute(stage_0_out, work_baseband_buffer);
@@ -82,3 +78,14 @@ buffer_c16_t ChannelDecimator::execute_decimation(buffer_c8_t buffer) {
 
 	return cic_4_out;
 }
+
+buffer_c16_t ChannelDecimator::execute_stage_0(
+	const buffer_c8_t& buffer,
+	const buffer_c16_t& work_baseband_buffer
+) {
+	if( fs_over_4_downconvert ) {
+		return translate.execute(buffer, work_baseband_buffer);
+	} else {
+		return cic_0.execute(buffer, work_baseband_buffer);
+	}
+}

firmware/baseband-tx/channel_decimator.hpp

@@ -32,6 +32,7 @@
 class ChannelDecimator {
 public:
 	enum class DecimationFactor {
+		By2,
 		By4,
 		By8,
 		By16,
@@ -39,13 +40,16 @@ public:
 	};
 
 	constexpr ChannelDecimator(
-	) : decimation_factor { DecimationFactor::By32 }
+	) : decimation_factor { DecimationFactor::By32 },
+		fs_over_4_downconvert { true }
 	{
 	}
 	
 	constexpr ChannelDecimator(
-		const DecimationFactor decimation_factor
-	) : decimation_factor { decimation_factor }
+		const DecimationFactor decimation_factor,
+		const bool fs_over_4_downconvert = true
+	) : decimation_factor { decimation_factor },
+		fs_over_4_downconvert { fs_over_4_downconvert }
 	{
 	}
 
@@ -53,7 +57,7 @@ public:
 		decimation_factor = f;
 	}
 
-	buffer_c16_t execute(buffer_c8_t buffer) {
+	buffer_c16_t execute(const buffer_c8_t& buffer) {
 		auto decimated = execute_decimation(buffer);
 
 		return decimated;
@@ -62,18 +66,22 @@ public:
 private:
 	std::array<complex16_t, 1024> work_baseband;
 
-	//const bool fs_over_4_downconvert = true;
-
 	dsp::decimate::TranslateByFSOver4AndDecimateBy2CIC3 translate;
-	//dsp::decimate::DecimateBy2CIC3 cic_0;
+	dsp::decimate::Complex8DecimateBy2CIC3 cic_0;
 	dsp::decimate::DecimateBy2CIC3 cic_1;
 	dsp::decimate::DecimateBy2CIC3 cic_2;
 	dsp::decimate::DecimateBy2CIC3 cic_3;
 	dsp::decimate::DecimateBy2CIC3 cic_4;
 
 	DecimationFactor decimation_factor;
+	const bool fs_over_4_downconvert;
 
-	buffer_c16_t execute_decimation(buffer_c8_t buffer);
+	buffer_c16_t execute_decimation(const buffer_c8_t& buffer);
+
+	buffer_c16_t execute_stage_0(
+		const buffer_c8_t& buffer,
+		const buffer_c16_t& work_baseband_buffer
+	);
 };
 
 #endif/*__CHANNEL_DECIMATOR_H__*/

firmware/baseband-tx/channel_stats_collector.hpp

@@ -34,7 +34,7 @@
 class ChannelStatsCollector {
 public:
 	template<typename Callback>
-	void feed(buffer_c16_t src, Callback callback) {
+	void feed(const buffer_c16_t& src, Callback callback) {
 		auto src_p = src.p;
 		while(src_p < &src.p[src.count]) {
 			const uint32_t sample = *__SIMD32(src_p)++;
@@ -49,7 +49,7 @@ public:
 
 		if( count >= samples_per_update ) {
 			const float max_squared_f = max_squared;
-			const int32_t max_db = complex16_mag_squared_to_dbv_norm(max_squared_f);
+			const int32_t max_db = mag2_to_dbv_norm(max_squared_f * (1.0f / (32768.0f * 32768.0f)));
 			callback({ max_db, count });
 
 			max_squared = 0;

firmware/baseband-tx/chconf.h

@@ -129,7 +129,7 @@
  * @note    The default is @p TRUE.
  */
 #if !defined(CH_USE_REGISTRY) || defined(__DOXYGEN__)
-#define CH_USE_REGISTRY                 TRUE
+#define CH_USE_REGISTRY                 FALSE
 #endif
 
 /**

firmware/baseband-tx/clock_recovery.hpp

@@ -116,19 +116,21 @@ private:
 template<typename ErrorFilter>
 class ClockRecovery {
 public:
+	using SymbolHandler = std::function<void(const float)>;
+
 	ClockRecovery(
 		const float sampling_rate,
 		const float symbol_rate,
 		ErrorFilter error_filter,
-		std::function<void(const float)> symbol_handler
-	) : symbol_handler { symbol_handler }
+		SymbolHandler symbol_handler
+	) : symbol_handler { std::move(symbol_handler) }
 	{
 		configure(sampling_rate, symbol_rate, error_filter);
 	}
 
 	ClockRecovery(
-		std::function<void(const float)> symbol_handler
-	) : symbol_handler { symbol_handler }
+		SymbolHandler symbol_handler
+	) : symbol_handler { std::move(symbol_handler) }
 	{
 	}
 
@@ -155,7 +157,7 @@ private:
 	dsp::interpolation::LinearResampler resampler;
 	GardnerTimingErrorDetector timing_error_detector;
 	ErrorFilter error_filter;
-	std::function<void(const float)> symbol_handler;
+	const SymbolHandler symbol_handler;
 
 	void resampler_callback(const float interpolated_sample) {
 		timing_error_detector(interpolated_sample,
@@ -166,7 +168,12 @@ private:
 	}
 
 	void symbol_callback(const float symbol, const float lateness) {
-		symbol_handler(symbol);
+		// NOTE: This check is to avoid std::function nullptr check, which
+		// brings in "_ZSt25__throw_bad_function_callv" and a lot of extra code.
+		// TODO: Make symbol_handler known at compile time.
+		if( symbol_handler) {
+			symbol_handler(symbol);
+		}
 
 		const float adjustment = error_filter(lateness);
 		resampler.advance(adjustment);

firmware/baseband-tx/dsp_decimate.cpp

@@ -26,7 +26,451 @@
 namespace dsp {
 namespace decimate {
 
-buffer_c16_t TranslateByFSOver4AndDecimateBy2CIC3::execute(buffer_c8_t src, buffer_c16_t dst) {
+static inline complex32_t mac_fs4_shift(
+	const vec2_s16* const z,
+	const vec2_s16* const t,
+	const size_t index,
+	const complex32_t accum
+) {
+	/* Accumulate sample * tap results for samples already in z buffer.
+	 * Multiply using swap/negation to achieve Fs/4 shift.
+	 * For iterations where samples are shifting out of z buffer (being discarded).
+	 * Expect negated tap t[2] to accomodate instruction set limitations.
+	 */
+	const bool negated_t2 = index & 1;
+	const auto q1_i0 = z[index*2 + 0];
+	const auto i1_q0 = z[index*2 + 1];
+	const auto t1_t0 = t[index];
+	const auto real = negated_t2 ? smlsd(q1_i0, t1_t0, accum.real()) : smlad(q1_i0, t1_t0, accum.real());
+	const auto imag = negated_t2 ? smlad(i1_q0, t1_t0, accum.imag()) : smlsd(i1_q0, t1_t0, accum.imag());
+	return { real, imag };
+}
+
+static inline complex32_t mac_shift(
+	const vec2_s16* const z,
+	const vec2_s16* const t,
+	const size_t index,
+	const complex32_t accum
+) {
+	/* Accumulate sample * tap results for samples already in z buffer.
+	 * For iterations where samples are shifting out of z buffer (being discarded).
+	 * real += i1 * t1 + i0 * t0
+	 * imag += q1 * t1 + q0 * t0
+	 */
+	const auto i1_i0 = z[index*2 + 0];
+	const auto q1_q0 = z[index*2 + 1];
+	const auto t1_t0 = t[index];
+	const auto real = smlad(i1_i0, t1_t0, accum.real());
+	const auto imag = smlad(q1_q0, t1_t0, accum.imag());
+	return { real, imag };
+}
+
+static inline complex32_t mac_fs4_shift_and_store(
+	vec2_s16* const z,
+	const vec2_s16* const t,
+	const size_t decimation_factor,
+	const size_t index,
+	const complex32_t accum
+) {
+	/* Accumulate sample * tap results for samples already in z buffer.
+	 * Place new samples into z buffer.
+	 * Expect negated tap t[2] to accomodate instruction set limitations.
+	 */
+	const bool negated_t2 = index & 1;
+	const auto q1_i0 = z[decimation_factor + index*2 + 0];
+	const auto i1_q0 = z[decimation_factor + index*2 + 1];
+	const auto t1_t0 = t[decimation_factor / 2 + index];
+	z[index*2 + 0] = q1_i0;
+	const auto real = negated_t2 ? smlsd(q1_i0, t1_t0, accum.real()) : smlad(q1_i0, t1_t0, accum.real());
+	z[index*2 + 1] = i1_q0;
+	const auto imag = negated_t2 ? smlad(i1_q0, t1_t0, accum.imag()) : smlsd(i1_q0, t1_t0, accum.imag());
+	return { real, imag };
+}
+
+static inline complex32_t mac_shift_and_store(
+	vec2_s16* const z,
+	const vec2_s16* const t,
+	const size_t decimation_factor,
+	const size_t index,
+	const complex32_t accum
+) {
+	/* Accumulate sample * tap results for samples already in z buffer.
+	 * Place new samples into z buffer.
+	 * Expect negated tap t[2] to accomodate instruction set limitations.
+	 */
+	const auto i1_i0 = z[decimation_factor + index*2 + 0];
+	const auto q1_q0 = z[decimation_factor + index*2 + 1];
+	const auto t1_t0 = t[decimation_factor / 2 + index];
+	z[index*2 + 0] = i1_i0;
+	const auto real = smlad(i1_i0, t1_t0, accum.real());
+	z[index*2 + 1] = q1_q0;
+	const auto imag = smlad(q1_q0, t1_t0, accum.imag());
+	return { real, imag };
+}
+
+static inline complex32_t mac_fs4_shift_and_store_new_c8_samples(
+	vec2_s16* const z,
+	const vec2_s16* const t,
+	const vec4_s8* const in,
+	const size_t decimation_factor,
+	const size_t index,
+	const size_t length,
+	const complex32_t accum
+) {
+	/* Accumulate sample * tap results for new samples.
+	 * Place new samples into z buffer.
+	 * Expect negated tap t[2] to accomodate instruction set limitations.
+	 */
+	const bool negated_t2 = index & 1;
+	const auto q1_i1_q0_i0 = in[index];
+	const auto t1_t0 = t[(length - decimation_factor) / 2 + index];
+	const auto i1_q1_i0_q0 = rev16(q1_i1_q0_i0);
+	const auto i1_q1_q0_i0 = pkhbt(q1_i1_q0_i0, i1_q1_i0_q0);
+	const auto q1_i0 = sxtb16(i1_q1_q0_i0);
+	const auto i1_q0 = sxtb16(i1_q1_q0_i0, 8);
+	z[length - decimation_factor * 2 + index*2 + 0] = q1_i0;
+	const auto real = negated_t2 ? smlsd(q1_i0, t1_t0, accum.real()) : smlad(q1_i0, t1_t0, accum.real());
+	z[length - decimation_factor * 2 + index*2 + 1] = i1_q0;
+	const auto imag = negated_t2 ? smlad(i1_q0, t1_t0, accum.imag()) : smlsd(i1_q0, t1_t0, accum.imag());
+	return { real, imag };
+}
+
+static inline complex32_t mac_shift_and_store_new_c16_samples(
+	vec2_s16* const z,
+	const vec2_s16* const t,
+	const vec2_s16* const in,
+	const size_t decimation_factor,
+	const size_t index,
+	const size_t length,
+	const complex32_t accum
+) {
+	/* Accumulate sample * tap results for new samples.
+	 * Place new samples into z buffer.
+	 * Expect negated tap t[2] to accomodate instruction set limitations.
+	 */
+	const auto q0_i0 = in[index*2+0];
+	const auto q1_i1 = in[index*2+1];
+	const auto i1_i0 = pkhbt(q0_i0, q1_i1, 16);
+	const auto q1_q0 = pkhtb(q1_i1, q0_i0, 16);
+	const auto t1_t0 = t[(length - decimation_factor) / 2 + index];
+	z[length - decimation_factor * 2 + index*2 + 0] = i1_i0;
+	const auto real = smlad(i1_i0, t1_t0, accum.real());
+	z[length - decimation_factor * 2 + index*2 + 1] = q1_q0;
+	const auto imag = smlad(q1_q0, t1_t0, accum.imag());
+	return { real, imag };
+}
+
+static inline uint32_t scale_round_and_pack(
+	const complex32_t value,
+	const int32_t scale_factor
+) {
+	/* Multiply 32-bit components of the complex<int32_t> by a scale factor,
+	 * into int64_ts, then round to nearest LSB (1 << 32), saturate to 16 bits,
+	 * and pack into a complex<int16_t>.
+	 */
+	const auto scaled_real = __SMMULR(value.real(), scale_factor);
+	const auto saturated_real = __SSAT(scaled_real, 16);
+
+	const auto scaled_imag = __SMMULR(value.imag(), scale_factor);
+	const auto saturated_imag = __SSAT(scaled_imag, 16);
+
+	return __PKHBT(saturated_real, saturated_imag, 16);
+}
+
+template<typename Tap>
+static void taps_copy(
+	const Tap* const source,
+	Tap* const target,
+	const size_t count,
+	const bool shift_up
+) {
+	const uint32_t negate_pattern = shift_up ? 0b1110 : 0b0100;
+	for(size_t i=0; i<count; i++) {
+		const bool negate = (negate_pattern >> (i & 3)) & 1;
+		target[i] = negate ? -source[i] : source[i];
+	}
+}
+
+// FIRC8xR16x24FS4Decim4 //////////////////////////////////////////////////
+
+void FIRC8xR16x24FS4Decim4::configure(
+	const std::array<tap_t, taps_count>& taps,
+	const int32_t scale,
+	const Shift shift
+) {
+	taps_copy(taps.data(), taps_.data(), taps_.size(), shift == Shift::Up);
+	output_scale = scale;
+	z_.fill({});
+}
+
+buffer_c16_t FIRC8xR16x24FS4Decim4::execute(
+	const buffer_c8_t& src,
+	const buffer_c16_t& dst
+) {
+	vec2_s16* const z = static_cast<vec2_s16*>(__builtin_assume_aligned(z_.data(), 4));
+	const vec2_s16* const t = static_cast<vec2_s16*>(__builtin_assume_aligned(taps_.data(), 4));
+	uint32_t* const d = static_cast<uint32_t*>(__builtin_assume_aligned(dst.p, 4));
+
+	const auto k = output_scale;
+
+	const size_t count = src.count / decimation_factor;
+	for(size_t i=0; i<count; i++) {
+		const vec4_s8* const in = static_cast<const vec4_s8*>(__builtin_assume_aligned(&src.p[i * decimation_factor], 4));
+
+		complex32_t accum;
+
+		// Oldest samples are discarded.
+		accum = mac_fs4_shift(z, t, 0, accum);
+		accum = mac_fs4_shift(z, t, 1, accum);
+
+		// Middle samples are shifted earlier in the "z" delay buffer.
+		accum = mac_fs4_shift_and_store(z, t, decimation_factor, 0, accum);
+		accum = mac_fs4_shift_and_store(z, t, decimation_factor, 1, accum);
+		accum = mac_fs4_shift_and_store(z, t, decimation_factor, 2, accum);
+		accum = mac_fs4_shift_and_store(z, t, decimation_factor, 3, accum);
+		accum = mac_fs4_shift_and_store(z, t, decimation_factor, 4, accum);
+		accum = mac_fs4_shift_and_store(z, t, decimation_factor, 5, accum);
+		accum = mac_fs4_shift_and_store(z, t, decimation_factor, 6, accum);
+		accum = mac_fs4_shift_and_store(z, t, decimation_factor, 7, accum);
+
+		// Newest samples come from "in" buffer, are copied to "z" delay buffer.
+		accum = mac_fs4_shift_and_store_new_c8_samples(z, t, in, decimation_factor, 0, taps_count, accum);
+		accum = mac_fs4_shift_and_store_new_c8_samples(z, t, in, decimation_factor, 1, taps_count, accum);
+
+		d[i] = scale_round_and_pack(accum, k);
+	}
+
+	return {
+		dst.p,
+		count,
+		src.sampling_rate / decimation_factor
+	};
+}
+
+// FIRC8xR16x24FS4Decim8 //////////////////////////////////////////////////
+
+void FIRC8xR16x24FS4Decim8::configure(
+	const std::array<tap_t, taps_count>& taps,
+	const int32_t scale,
+	const Shift shift
+) {
+	taps_copy(taps.data(), taps_.data(), taps_.size(), shift == Shift::Up);
+	output_scale = scale;
+	z_.fill({});
+}
+
+buffer_c16_t FIRC8xR16x24FS4Decim8::execute(
+	const buffer_c8_t& src,
+	const buffer_c16_t& dst
+) {
+	vec2_s16* const z = static_cast<vec2_s16*>(__builtin_assume_aligned(z_.data(), 4));
+	const vec2_s16* const t = static_cast<vec2_s16*>(__builtin_assume_aligned(taps_.data(), 4));
+	uint32_t* const d = static_cast<uint32_t*>(__builtin_assume_aligned(dst.p, 4));
+
+	const auto k = output_scale;
+
+	const size_t count = src.count / decimation_factor;
+	for(size_t i=0; i<count; i++) {
+		const vec4_s8* const in = static_cast<const vec4_s8*>(__builtin_assume_aligned(&src.p[i * decimation_factor], 4));
+
+		complex32_t accum;
+
+		// Oldest samples are discarded.
+		accum = mac_fs4_shift(z, t, 0, accum);
+		accum = mac_fs4_shift(z, t, 1, accum);
+		accum = mac_fs4_shift(z, t, 2, accum);
+		accum = mac_fs4_shift(z, t, 3, accum);
+
+		// Middle samples are shifted earlier in the "z" delay buffer.
+		accum = mac_fs4_shift_and_store(z, t, decimation_factor, 0, accum);
+		accum = mac_fs4_shift_and_store(z, t, decimation_factor, 1, accum);
+		accum = mac_fs4_shift_and_store(z, t, decimation_factor, 2, accum);
+		accum = mac_fs4_shift_and_store(z, t, decimation_factor, 3, accum);
+
+		// Newest samples come from "in" buffer, are copied to "z" delay buffer.
+		accum = mac_fs4_shift_and_store_new_c8_samples(z, t, in, decimation_factor, 0, taps_count, accum);
+		accum = mac_fs4_shift_and_store_new_c8_samples(z, t, in, decimation_factor, 1, taps_count, accum);
+		accum = mac_fs4_shift_and_store_new_c8_samples(z, t, in, decimation_factor, 2, taps_count, accum);
+		accum = mac_fs4_shift_and_store_new_c8_samples(z, t, in, decimation_factor, 3, taps_count, accum);
+
+		d[i] = scale_round_and_pack(accum, k);
+	}
+
+	return {
+		dst.p,
+		count,
+		src.sampling_rate / decimation_factor
+	};
+}
+
+// FIRC16xR16x16Decim2 ////////////////////////////////////////////////////
+
+void FIRC16xR16x16Decim2::configure(
+	const std::array<tap_t, taps_count>& taps,
+	const int32_t scale
+) {
+	std::copy(taps.cbegin(), taps.cend(), taps_.begin());
+	output_scale = scale;
+	z_.fill({});
+}
+
+buffer_c16_t FIRC16xR16x16Decim2::execute(
+	const buffer_c16_t& src,
+	const buffer_c16_t& dst
+) {
+	vec2_s16* const z = static_cast<vec2_s16*>(__builtin_assume_aligned(z_.data(), 4));
+	const vec2_s16* const t = static_cast<vec2_s16*>(__builtin_assume_aligned(taps_.data(), 4));
+	uint32_t* const d = static_cast<uint32_t*>(__builtin_assume_aligned(dst.p, 4));
+
+	const auto k = output_scale;
+
+	const size_t count = src.count / decimation_factor;
+	for(size_t i=0; i<count; i++) {
+		const vec2_s16* const in = static_cast<const vec2_s16*>(__builtin_assume_aligned(&src.p[i * decimation_factor], 4));
+
+		complex32_t accum;
+
+		// Oldest samples are discarded.
+		accum = mac_shift(z, t, 0, accum);
+
+		// Middle samples are shifted earlier in the "z" delay buffer.
+		accum = mac_shift_and_store(z, t, decimation_factor, 0, accum);
+		accum = mac_shift_and_store(z, t, decimation_factor, 1, accum);
+		accum = mac_shift_and_store(z, t, decimation_factor, 2, accum);
+		accum = mac_shift_and_store(z, t, decimation_factor, 3, accum);
+		accum = mac_shift_and_store(z, t, decimation_factor, 4, accum);
+		accum = mac_shift_and_store(z, t, decimation_factor, 5, accum);
+
+		// Newest samples come from "in" buffer, are copied to "z" delay buffer.
+		accum = mac_shift_and_store_new_c16_samples(z, t, in, decimation_factor, 0, taps_count, accum);
+
+		d[i] = scale_round_and_pack(accum, k);
+	}
+
+	return {
+		dst.p,
+		count,
+		src.sampling_rate / decimation_factor
+	};
+}
+
+// FIRC16xR16x32Decim8 ////////////////////////////////////////////////////
+
+void FIRC16xR16x32Decim8::configure(
+	const std::array<tap_t, taps_count>& taps,
+	const int32_t scale
+) {
+	std::copy(taps.cbegin(), taps.cend(), taps_.begin());
+	output_scale = scale;
+	z_.fill({});
+}
+
+buffer_c16_t FIRC16xR16x32Decim8::execute(
+	const buffer_c16_t& src,
+	const buffer_c16_t& dst
+) {
+	vec2_s16* const z = static_cast<vec2_s16*>(__builtin_assume_aligned(z_.data(), 4));
+	const vec2_s16* const t = static_cast<vec2_s16*>(__builtin_assume_aligned(taps_.data(), 4));
+	uint32_t* const d = static_cast<uint32_t*>(__builtin_assume_aligned(dst.p, 4));
+
+	const auto k = output_scale;
+
+	const size_t count = src.count / decimation_factor;
+	for(size_t i=0; i<count; i++) {
+		const vec2_s16* const in = static_cast<const vec2_s16*>(__builtin_assume_aligned(&src.p[i * decimation_factor], 4));
+
+		complex32_t accum;
+
+		// Oldest samples are discarded.
+		accum = mac_shift(z, t, 0, accum);
+		accum = mac_shift(z, t, 1, accum);
+		accum = mac_shift(z, t, 2, accum);
+		accum = mac_shift(z, t, 3, accum);
+
+		// Middle samples are shifted earlier in the "z" delay buffer.
+		accum = mac_shift_and_store(z, t, decimation_factor, 0, accum);
+		accum = mac_shift_and_store(z, t, decimation_factor, 1, accum);
+		accum = mac_shift_and_store(z, t, decimation_factor, 2, accum);
+		accum = mac_shift_and_store(z, t, decimation_factor, 3, accum);
+		accum = mac_shift_and_store(z, t, decimation_factor, 4, accum);
+		accum = mac_shift_and_store(z, t, decimation_factor, 5, accum);
+		accum = mac_shift_and_store(z, t, decimation_factor, 6, accum);
+		accum = mac_shift_and_store(z, t, decimation_factor, 7, accum);
+
+		// Newest samples come from "in" buffer, are copied to "z" delay buffer.
+		accum = mac_shift_and_store_new_c16_samples(z, t, in, decimation_factor, 0, taps_count, accum);
+		accum = mac_shift_and_store_new_c16_samples(z, t, in, decimation_factor, 1, taps_count, accum);
+		accum = mac_shift_and_store_new_c16_samples(z, t, in, decimation_factor, 2, taps_count, accum);
+		accum = mac_shift_and_store_new_c16_samples(z, t, in, decimation_factor, 3, taps_count, accum);
+
+		d[i] = scale_round_and_pack(accum, k);
+	}
+
+	return {
+		dst.p,
+		count,
+		src.sampling_rate / decimation_factor
+	};
+}
+
+buffer_c16_t Complex8DecimateBy2CIC3::execute(const buffer_c8_t& src, const buffer_c16_t& dst) {
+	/* Decimates by two using a non-recursive third-order CIC filter.
+	 */
+
+	/* CIC filter (decimating by two):
+	 * 	D_I0 = i3 * 1 + i2 * 3 + i1 * 3 + i0 * 1
+	 * 	D_Q0 = q3 * 1 + q2 * 3 + q1 * 3 + q0 * 1
+	 *
+	 * 	D_I1 = i5 * 1 + i4 * 3 + i3 * 3 + i2 * 1
+	 * 	D_Q1 = q5 * 1 + q4 * 3 + q3 * 3 + q2 * 1
+	 */
+
+	uint32_t i1_i0 = _i1_i0;
+	uint32_t q1_q0 = _q1_q0;
+
+	/* 3:1 Scaled by 32 to normalize output to +/-32768-ish. */
+	constexpr uint32_t scale_factor = 32;
+	constexpr uint32_t k_3_1 = 0x00030001 * scale_factor;
+	uint32_t* src_p = reinterpret_cast<uint32_t*>(&src.p[0]);
+	uint32_t* const src_end = reinterpret_cast<uint32_t*>(&src.p[src.count]);
+	uint32_t* dst_p = reinterpret_cast<uint32_t*>(&dst.p[0]);
+	while(src_p < src_end) {
+		const uint32_t q3_i3_q2_i2 = *(src_p++);						// 3
+		const uint32_t q5_i5_q4_i4 = *(src_p++);
+
+		const uint32_t d_i0_partial = __SMUAD(k_3_1, i1_i0);			// 1: = 3 * i1 + 1 * i0
+		const uint32_t i3_i2 = __SXTB16(q3_i3_q2_i2,  0);				// 1: (q3_i3_q2_i2 ror  0)[23:16]:(q3_i3_q2_i2 ror  0)[7:0]
+		const uint32_t d_i0 = __SMLADX(k_3_1, i3_i2, d_i0_partial);		// 1: + 3 * i2 + 1 * i3
+
+		const uint32_t d_q0_partial = __SMUAD(k_3_1, q1_q0);			// 1: = 3 * q1 * 1 * q0
+		const uint32_t q3_q2 = __SXTB16(q3_i3_q2_i2,  8);				// 1: (q3_i3_q2_i2 ror  8)[23:16]:(q3_i3_q2_i2 ror  8)[7:0]
+		const uint32_t d_q0 = __SMLADX(k_3_1, q3_q2, d_q0_partial);		// 1: + 3 * q2 + 1 * q3 
+
+		const uint32_t d_q0_i0 = __PKHBT(d_i0, d_q0, 16);				// 1: (Rm<<16)[31:16]:Rn[15:0]
+
+		const uint32_t d_i1_partial = __SMUAD(k_3_1, i3_i2);			// 1: = 3 * i3 + 1 * i2
+		const uint32_t i5_i4 = __SXTB16(q5_i5_q4_i4,  0);				// 1: (q5_i5_q4_i4 ror  0)[23:16]:(q5_i5_q4_i4 ror  0)[7:0]
+		const uint32_t d_i1 = __SMLADX(k_3_1, i5_i4, d_i1_partial);		// 1: + 1 * i5 + 3 * i4
+
+		const uint32_t d_q1_partial = __SMUAD(k_3_1, q3_q2);			// 1: = 3 * q3 * 1 * q2
+		const uint32_t q5_q4 = __SXTB16(q5_i5_q4_i4,  8);				// 1: (q5_i5_q4_i4 ror  8)[23:16]:(q5_i5_q4_i4 ror  8)[7:0]
+		const uint32_t d_q1 = __SMLADX(k_3_1, q5_q4, d_q1_partial);		// 1: + 1 * q5 + 3 * q4 
+
+		const uint32_t d_q1_i1 = __PKHBT(d_i1, d_q1, 16);				// 1: (Rm<<16)[31:16]:Rn[15:0]
+
+		*(dst_p++) = d_q0_i0;											// 3
+		*(dst_p++) = d_q1_i1;
+
+		i1_i0 = i5_i4;
+		q1_q0 = q5_q4;
+	}
+	_i1_i0 = i1_i0;
+	_q1_q0 = q1_q0;
+
+	return { dst.p, src.count / 2, src.sampling_rate / 2 };
+}
+
+buffer_c16_t TranslateByFSOver4AndDecimateBy2CIC3::execute(const buffer_c8_t& src, const buffer_c16_t& dst) {
 	/* Translates incoming complex<int8_t> samples by -fs/4,
 	 * decimates by two using a non-recursive third-order CIC filter.
 	 */
@@ -111,8 +555,8 @@ buffer_c16_t TranslateByFSOver4AndDecimateBy2CIC3::execute(buffer_c8_t src, buff
 }
 
 buffer_c16_t DecimateBy2CIC3::execute(
-	buffer_c16_t src,
-	buffer_c16_t dst
+	const buffer_c16_t& src,
+	const buffer_c16_t& dst
 ) {
 	/* Complex non-recursive 3rd-order CIC filter (taps 1,3,3,1).
 	 * Gain of 8.
@@ -121,20 +565,18 @@ buffer_c16_t DecimateBy2CIC3::execute(
 	 */
 	uint32_t t1 = _iq0;
 	uint32_t t2 = _iq1;
-	uint32_t t3, t4;
 	const uint32_t taps = 0x00000003;
 	auto s = src.p;
 	auto d = dst.p;
 	const auto d_end = &dst.p[src.count / 2];
-	uint32_t i, q;
 	while(d < d_end) {
-		i = __SXTH(t1, 0);			/* 1: I0 */
-		q = __SXTH(t1, 16);			/* 1: Q0 */
+		uint32_t i = __SXTH(t1, 0);			/* 1: I0 */
+		uint32_t q = __SXTH(t1, 16);			/* 1: Q0 */
 		i = __SMLABB(t2, taps, i);	/* 1: I1*3 + I0 */
 		q = __SMLATB(t2, taps, q);	/* 1: Q1*3 + Q0 */
 
-		t3 = *__SIMD32(s)++;		/* 3: Q2:I2 */
-		t4 = *__SIMD32(s)++;		/*    Q3:I3 */
+		const uint32_t t3 = *__SIMD32(s)++;		/* 3: Q2:I2 */
+		const uint32_t t4 = *__SIMD32(s)++;		/*    Q3:I3 */
 
 		i = __SMLABB(t3, taps, i);	/* 1: I2*3 + I1*3 + I0 */
 		q = __SMLATB(t3, taps, q);	/* 1: Q2*3 + Q1*3 + Q0 */
@@ -164,9 +606,15 @@ buffer_c16_t DecimateBy2CIC3::execute(
 	return { dst.p, src.count / 2, src.sampling_rate / 2 };
 }
 
+void FIR64AndDecimateBy2Real::configure(
+	const std::array<int16_t, taps_count>& new_taps
+) {
+	std::copy(new_taps.cbegin(), new_taps.cend(), taps.begin());
+}
+
 buffer_s16_t FIR64AndDecimateBy2Real::execute(
-	buffer_s16_t src,
-	buffer_s16_t dst
+	const buffer_s16_t& src,
+	const buffer_s16_t& dst
 ) {
 	/* int16_t input (sample count "n" must be multiple of 4)
 	 * -> int16_t output, decimated by 2.
@@ -197,9 +645,18 @@ buffer_s16_t FIR64AndDecimateBy2Real::execute(
 	return { dst.p, src.count / 2, src.sampling_rate / 2 };
 }
 
+void FIRAndDecimateComplex::configure_common(
+	const size_t taps_count, const size_t decimation_factor
+) {
+	samples_ = std::make_unique<samples_t>(taps_count);
+	taps_reversed_ = std::make_unique<taps_t>(taps_count);
+	taps_count_ = taps_count;
+	decimation_factor_ = decimation_factor;
+}
+
 buffer_c16_t FIRAndDecimateComplex::execute(
-	buffer_c16_t src,
-	buffer_c16_t dst
+	const buffer_c16_t& src,
+	const buffer_c16_t& dst
 ) {
 	/* int16_t input (sample count "n" must be multiple of decimation_factor)
 	 * -> int16_t output, decimated by decimation_factor.
@@ -308,8 +765,8 @@ buffer_c16_t FIRAndDecimateComplex::execute(
 }
 
 buffer_s16_t DecimateBy2CIC4Real::execute(
-	buffer_s16_t src,
-	buffer_s16_t dst
+	const buffer_s16_t& src,
+	const buffer_s16_t& dst
 ) {
 	auto src_p = src.p;
 	auto dst_p = dst.p;
@@ -328,76 +785,6 @@ buffer_s16_t DecimateBy2CIC4Real::execute(
 
 	return { dst.p, src.count / 2, src.sampling_rate / 2 };
 }
-#if 0
-buffer_c16_t DecimateBy2HBF5Complex::execute(
-	buffer_c16_t const src,
-	buffer_c16_t const dst
-) {
-	auto src_p = src.p;
-	auto dst_p = dst.p;
-	int32_t n = src.count;
-	for(; n>0; n-=2) {
-		/* TODO: Probably a lot of room to optimize... */
-		z[0] = z[2];
-		//z[1] = z[3];
-		z[2] = z[4];
-		//z[3] = z[5];
-		z[4] = z[6];
-		z[5] = z[7];
-		z[6] = z[8];
-		z[7] = z[9];
-		z[8] = z[10];
-		z[9] = *(src_p++);
-		z[10] = *(src_p++);
 
-		int32_t t_real { z[5].real * 256 };
-		int32_t t_imag { z[5].imag * 256 };
-		t_real += (z[ 0].real + z[10].real) * 3;
-		t_imag += (z[ 0].imag + z[10].imag) * 3;
-		t_real -= (z[ 2].real + z[ 8].real) * 25;
-		t_imag -= (z[ 2].imag + z[ 8].imag) * 25;
-		t_real += (z[ 4].real + z[ 6].real) * 150;
-		t_imag += (z[ 4].imag + z[ 6].imag) * 150;
-		*(dst_p++) = { t_real / 256, t_imag / 256 };
-	}
-
-	return { dst.p, src.count / 2, src.sampling_rate / 2 };
-}
-
-buffer_c16_t DecimateBy2HBF7Complex::execute(
-	buffer_c16_t const src,
-	buffer_c16_t const dst
-) {
-	auto src_p = src.p;
-	auto dst_p = dst.p;
-	int32_t n = src.count;
-	for(; n>0; n-=2) {
-		/* TODO: Probably a lot of room to optimize... */
-		z[0] = z[2];
-		//z[1] = z[3];
-		z[2] = z[4];
-		//z[3] = z[5];
-		z[4] = z[6];
-		z[5] = z[7];
-		z[6] = z[8];
-		z[7] = z[9];
-		z[8] = z[10];
-		z[9] = *(src_p++);
-		z[10] = *(src_p++);
-
-		int32_t t_real { z[5].real * 512 };
-		int32_t t_imag { z[5].imag * 512 };
-		t_real += (z[ 0].real + z[10].real) * 7;
-		t_imag += (z[ 0].imag + z[10].imag) * 7;
-		t_real -= (z[ 2].real + z[ 8].real) * 53;
-		t_imag -= (z[ 2].imag + z[ 8].imag) * 53;
-		t_real += (z[ 4].real + z[ 6].real) * 302;
-		t_imag += (z[ 4].imag + z[ 6].imag) * 302;
-		*(dst_p++) = { t_real / 512, t_imag / 512 };
-	}
-
-	return { dst.p, src.count / 2, src.sampling_rate / 2 };
-}
-#endif
 } /* namespace decimate */
 } /* namespace dsp */

firmware/baseband-tx/dsp_decimate.hpp

@@ -31,14 +31,28 @@
 
 #include "dsp_types.hpp"
 
+#include "simd.hpp"
+
 namespace dsp {
 namespace decimate {
 
+class Complex8DecimateBy2CIC3 {
+public:
+	buffer_c16_t execute(
+		const buffer_c8_t& src,
+		const buffer_c16_t& dst
+	);
+
+private:
+	uint32_t _i1_i0 { 0 };
+	uint32_t _q1_q0 { 0 };
+};
+
 class TranslateByFSOver4AndDecimateBy2CIC3 {
 public:
 	buffer_c16_t execute(
-		buffer_c8_t src,
-		buffer_c16_t dst
+		const buffer_c8_t& src,
+		const buffer_c16_t& dst
 	);
 
 private:
@@ -49,8 +63,8 @@ private:
 class DecimateBy2CIC3 {
 public:
 	buffer_c16_t execute(
-		buffer_c16_t src,
-		buffer_c16_t dst
+		const buffer_c16_t& src,
+		const buffer_c16_t& dst
 	);
 
 private:
@@ -62,20 +76,126 @@ class FIR64AndDecimateBy2Real {
 public:
 	static constexpr size_t taps_count = 64;
 
-	FIR64AndDecimateBy2Real(
+	void configure(
 		const std::array<int16_t, taps_count>& taps
-	) : taps(taps)
-	{
-	}
+	);
 
 	buffer_s16_t execute(
-		buffer_s16_t src,
-		buffer_s16_t dst
+		const buffer_s16_t& src,
+		const buffer_s16_t& dst
 	);
 
 private:
 	std::array<int16_t, taps_count + 2> z;
-	const std::array<int16_t, taps_count>& taps;
+	std::array<int16_t, taps_count> taps;
+};
+
+class FIRC8xR16x24FS4Decim4 {
+public:
+	static constexpr size_t taps_count = 24;
+	static constexpr size_t decimation_factor = 4;
+
+	using sample_t = complex8_t;
+	using tap_t = int16_t;
+
+	enum class Shift : bool {
+		Down = true,
+		Up = false
+	};
+
+	void configure(
+		const std::array<tap_t, taps_count>& taps,
+		const int32_t scale,
+		const Shift shift = Shift::Down
+	);
+
+	buffer_c16_t execute(
+		const buffer_c8_t& src,
+		const buffer_c16_t& dst
+	);
+	
+private:
+	std::array<vec2_s16, taps_count - decimation_factor> z_;
+	std::array<tap_t, taps_count> taps_;
+	int32_t output_scale = 0;
+};
+
+class FIRC8xR16x24FS4Decim8 {
+public:
+	static constexpr size_t taps_count = 24;
+	static constexpr size_t decimation_factor = 8;
+
+	using sample_t = complex8_t;
+	using tap_t = int16_t;
+
+	enum class Shift : bool {
+		Down = true,
+		Up = false
+	};
+
+	void configure(
+		const std::array<tap_t, taps_count>& taps,
+		const int32_t scale,
+		const Shift shift = Shift::Down
+	);
+
+	buffer_c16_t execute(
+		const buffer_c8_t& src,
+		const buffer_c16_t& dst
+	);
+	
+private:
+	std::array<vec2_s16, taps_count - decimation_factor> z_;
+	std::array<tap_t, taps_count> taps_;
+	int32_t output_scale = 0;
+};
+
+class FIRC16xR16x16Decim2 {
+public:
+	static constexpr size_t taps_count = 16;
+	static constexpr size_t decimation_factor = 2;
+
+	using sample_t = complex16_t;
+	using tap_t = int16_t;
+
+	void configure(
+		const std::array<tap_t, taps_count>& taps,
+		const int32_t scale
+	);
+
+	buffer_c16_t execute(
+		const buffer_c16_t& src,
+		const buffer_c16_t& dst
+	);
+	
+private:
+	std::array<vec2_s16, taps_count - decimation_factor> z_;
+	std::array<tap_t, taps_count> taps_;
+	int32_t output_scale = 0;
+};
+
+class FIRC16xR16x32Decim8 {
+public:
+	static constexpr size_t taps_count = 32;
+	static constexpr size_t decimation_factor = 8;
+
+	using sample_t = complex16_t;
+	using tap_t = int16_t;
+
+	void configure(
+		const std::array<tap_t, taps_count>& taps,
+		const int32_t scale
+	);
+
+	buffer_c16_t execute(
+		const buffer_c16_t& src,
+		const buffer_c16_t& dst
+	);
+	
+private:
+	std::array<vec2_s16, taps_count - decimation_factor> z_;
+	std::array<tap_t, taps_count> taps_;
+	int32_t output_scale = 0;
 };
 
 class FIRAndDecimateComplex {
@@ -99,16 +219,12 @@ public:
 		const T& taps,
 		const size_t decimation_factor
 	) {
-		samples_ = std::make_unique<samples_t>(taps.size());
-		taps_reversed_ = std::make_unique<taps_t>(taps.size());
-		taps_count_ = taps.size();
-		decimation_factor_ = decimation_factor;
-		std::reverse_copy(taps.cbegin(), taps.cend(), &taps_reversed_[0]);
+		configure(taps.data(), taps.size(), decimation_factor);
 	}
 
 	buffer_c16_t execute(
-		buffer_c16_t src,
-		buffer_c16_t dst
+		const buffer_c16_t& src,
+		const buffer_c16_t& dst
 	);
 	
 private:
@@ -118,124 +234,34 @@ private:
 	std::unique_ptr<taps_t> taps_reversed_;
 	size_t taps_count_;
 	size_t decimation_factor_;
+
+	template<typename T>
+	void configure(
+		const T* const taps,
+		const size_t taps_count,
+		const size_t decimation_factor
+	) {
+		configure_common(taps_count, decimation_factor);
+		std::reverse_copy(&taps[0], &taps[taps_count], &taps_reversed_[0]);
+	}
+
+	void configure_common(
+		const size_t taps_count,
+		const size_t decimation_factor
+	);
 };
 
 class DecimateBy2CIC4Real {
 public:
 	buffer_s16_t execute(
-		buffer_s16_t src,
-		buffer_s16_t dst
+		const buffer_s16_t& src,
+		const buffer_s16_t& dst
 	);
 
 private:
 	int16_t z[5];
 };
-#if 0
-class DecimateBy2HBF5Complex {
-public:
-	buffer_c16_t execute(
-		buffer_c16_t const src,
-		buffer_c16_t const dst
-	);
 
-private:
-	complex16_t z[11];
-};
-
-class DecimateBy2HBF7Complex {
-public:
-	buffer_c16_t execute(
-		buffer_c16_t const src,
-		buffer_c16_t const dst
-	);
-
-private:
-	complex16_t z[11];
-};
-#endif
-/* From http://www.dspguru.com/book/export/html/3
-
-Here are several basic techniques to fake circular buffers:
-
-Split the calculation: You can split any FIR calculation into its "pre-wrap"
-and "post-wrap" parts. By splitting the calculation into these two parts, you
-essentially can do the circular logic only once, rather than once per tap.
-(See fir_double_z in FirAlgs.c above.)
-
-Duplicate the delay line: For a FIR with N taps, use a delay line of size 2N.
-Copy each sample to its proper location, as well as at location-plus-N.
-Therefore, the FIR calculation's MAC loop can be done on a flat buffer of N
-points, starting anywhere within the first set of N points. The second set of
-N delayed samples provides the "wrap around" comparable to a true circular
-buffer. (See fir_double_z in FirAlgs.c above.)
-
-Duplicate the coefficients: This is similar to the above, except that the
-duplication occurs in terms of the coefficients, not the delay line.
-Compared to the previous method, this has a calculation advantage of not
-having to store each incoming sample twice, and it also has a memory
-advantage when the same coefficient set will be used on multiple delay lines.
-(See fir_double_h in FirAlgs.c above.)
-
-Use block processing: In block processing, you use a delay line which is a
-multiple of the number of taps. You therefore only have to move the data
-once per block to implement the delay-line mechanism. When the block size
-becomes "large", the overhead of a moving the delay line once per block
-becomes negligible.
-*/
-
-#if 0
-template<size_t N>
-class FIRAndDecimateBy2Complex {
-public:
-	FIR64AndDecimateBy2Complex(
-		const std::array<int16_t, N>& taps
-	) : taps { taps }
-	{
-	}
-
-	buffer_c16_t execute(
-		buffer_c16_t const src,
-		buffer_c16_t const dst
-	) {
-		/* int16_t input (sample count "n" must be multiple of 4)
-		 * -> int16_t output, decimated by 2.
-		 * taps are normalized to 1 << 16 == 1.0.
-		 */
-
-		return { dst.p, src.count / 2 };
-	}
-
-private:
-	std::array<complex16_t, N> z;
-	const std::array<int16_t, N>& taps;
-
-	complex<int16_t> process_one(const size_t start_offset) {
-		const auto split = &z[start_offset];
-		const auto end = &z[z.size()];
-		auto tap = &taps[0];
-
-		complex<int32_t> t { 0, 0 };
-
-		auto p = split;
-		while(p < end) {
-			const auto t = *(tap++);
-			const auto c = *(p++);
-			t.real += c.real * t;
-			t.imag += c.imag * t;
-		}
-
-		p = &z[0];
-		while(p < split) {
-			const auto t = *(tap++);
-			const auto c = *(p++);
-			t.real += c.real * t;
-			t.imag += c.imag * t;
-		}
-
-		return { t.real / 65536, t.imag / 65536 };
-	}
-};
-#endif
 } /* namespace decimate */
 } /* namespace dsp */
 

firmware/baseband-tx/dsp_demodulate.cpp

@@ -30,34 +30,37 @@
 namespace dsp {
 namespace demodulate {
 
-buffer_s16_t AM::execute(
-	buffer_c16_t src,
-	buffer_s16_t dst
+buffer_f32_t AM::execute(
+	const buffer_c16_t& src,
+	const buffer_f32_t& dst
 ) {
-	/* Intermediate maximum value: 46341 (when input is -32768,-32768). */
-	/* Normalized to maximum 32767 for int16_t representation. */
-
 	const auto src_p = src.p;
 	const auto src_end = &src.p[src.count];
 	auto dst_p = dst.p;
 	while(src_p < src_end) {
-		// const auto s = *(src_p++);
-		// const uint32_t r_sq = s.real() * s.real();
-		// const uint32_t i_sq = s.imag() * s.imag();
-		// const uint32_t mag_sq = r_sq + i_sq;
 		const uint32_t sample0 = *__SIMD32(src_p)++;
 		const uint32_t sample1 = *__SIMD32(src_p)++;
 		const uint32_t mag_sq0 = __SMUAD(sample0, sample0);
 		const uint32_t mag_sq1 = __SMUAD(sample1, sample1);
-		const int32_t mag0_int = __builtin_sqrtf(mag_sq0);
-		const int32_t mag0_sat = __SSAT(mag0_int, 16);
-		const int32_t mag1_int = __builtin_sqrtf(mag_sq1);
-		const int32_t mag1_sat = __SSAT(mag1_int, 16);
-		*__SIMD32(dst_p)++ = __PKHBT(
-			mag0_sat,
-			mag1_sat,
-			16
-		);
+		*(dst_p++) = __builtin_sqrtf(mag_sq0) * k;
+		*(dst_p++) = __builtin_sqrtf(mag_sq1) * k;
+	}
+
+	return { dst.p, src.count, src.sampling_rate };
+}
+
+buffer_f32_t SSB::execute(
+	const buffer_c16_t& src,
+	const buffer_f32_t& dst
+) {
+	const complex16_t* src_p = src.p;
+	const auto src_end = &src.p[src.count];
+	auto dst_p = dst.p;
+	while(src_p < src_end) {
+		*(dst_p++) = (src_p++)->real() * k;
+		*(dst_p++) = (src_p++)->real() * k;
+		*(dst_p++) = (src_p++)->real() * k;
+		*(dst_p++) = (src_p++)->real() * k;
 	}
 
 	return { dst.p, src.count, src.sampling_rate };
@@ -69,17 +72,21 @@ static inline float angle_approx_4deg0(const complex32_t t) {
 }
 */
 static inline float angle_approx_0deg27(const complex32_t t) {
-	const auto x = static_cast<float>(t.imag()) / static_cast<float>(t.real());
-	return x / (1.0f + 0.28086f * x * x);
+	if( t.real() ) {
+		const auto x = static_cast<float>(t.imag()) / static_cast<float>(t.real());
+		return x / (1.0f + 0.28086f * x * x);
+	} else {
+		return (t.imag() < 0) ? -1.5707963268f : 1.5707963268f;
+	}
 }
-/*
+
 static inline float angle_precise(const complex32_t t) {
 	return atan2f(t.imag(), t.real());
 }
-*/
-buffer_s16_t FM::execute(
-	buffer_c16_t src,
-	buffer_s16_t dst
+
+buffer_f32_t FM::execute(
+	const buffer_c16_t& src,
+	const buffer_f32_t& dst
 ) {
 	auto z = z_;
 
@@ -92,9 +99,32 @@ buffer_s16_t FM::execute(
 		const auto t0 = multiply_conjugate_s16_s32(s0, z);
 		const auto t1 = multiply_conjugate_s16_s32(s1, s0);
 		z = s1;
-		const int32_t theta0_int = angle_approx_0deg27(t0) * k;
+		*(dst_p++) = angle_precise(t0) * kf;
+		*(dst_p++) = angle_precise(t1) * kf;
+	}
+	z_ = z;
+
+	return { dst.p, src.count, src.sampling_rate };
+}
+
+buffer_s16_t FM::execute(
+	const buffer_c16_t& src,
+	const buffer_s16_t& dst
+) {
+	auto z = z_;
+
+	const auto src_p = src.p;
+	const auto src_end = &src.p[src.count];
+	auto dst_p = dst.p;
+	while(src_p < src_end) {
+		const auto s0 = *__SIMD32(src_p)++;
+		const auto s1 = *__SIMD32(src_p)++;
+		const auto t0 = multiply_conjugate_s16_s32(s0, z);
+		const auto t1 = multiply_conjugate_s16_s32(s1, s0);
+		z = s1;
+		const int32_t theta0_int = angle_approx_0deg27(t0) * ks16;
 		const int32_t theta0_sat = __SSAT(theta0_int, 16);
-		const int32_t theta1_int = angle_approx_0deg27(t1) * k;
+		const int32_t theta1_int = angle_approx_0deg27(t1) * ks16;
 		const int32_t theta1_sat = __SSAT(theta1_int, 16);
 		*__SIMD32(dst_p)++ = __PKHBT(
 			theta0_sat,
@@ -107,5 +137,15 @@ buffer_s16_t FM::execute(
 	return { dst.p, src.count, src.sampling_rate };
 }
 
+void FM::configure(const float sampling_rate, const float deviation_hz) {
+	/*
+	 * angle: -pi to pi. output range: -32768 to 32767.
+	 * Maximum delta-theta (output of atan2) at maximum deviation frequency:
+	 * delta_theta_max = 2 * pi * deviation / sampling_rate
+	 */
+	kf = static_cast<float>(1.0f / (2.0 * pi * deviation_hz / sampling_rate));
+	ks16 = 32767.0f * kf;
+}
+
 }
 }

firmware/baseband-tx/dsp_demodulate.hpp

@@ -29,39 +29,44 @@ namespace demodulate {
 
 class AM {
 public:
-	buffer_s16_t execute(
-		buffer_c16_t src,
-		buffer_s16_t dst
+	buffer_f32_t execute(
+		const buffer_c16_t& src,
+		const buffer_f32_t& dst
 	);
+
+private:
+	static constexpr float k = 1.0f / 32768.0f;
+};
+
+class SSB {
+public:
+	buffer_f32_t execute(
+		const buffer_c16_t& src,
+		const buffer_f32_t& dst
+	);
+
+private:
+	static constexpr float k = 1.0f / 32768.0f;
 };
 
 class FM {
 public:
-	/*
-	 * angle: -pi to pi. output range: -32768 to 32767.
-	 * Maximum delta-theta (output of atan2) at maximum deviation frequency:
-	 * delta_theta_max = 2 * pi * deviation / sampling_rate
-	 */
-	constexpr FM(
-		const float sampling_rate,
-		const float deviation_hz
-	) : z_ { 0 },
-		k { static_cast<float>(32767.0f / (2.0 * pi * deviation_hz / sampling_rate)) }
-	{
-	}
-
-	buffer_s16_t execute(
-		buffer_c16_t src,
-		buffer_s16_t dst
+	buffer_f32_t execute(
+		const buffer_c16_t& src,
+		const buffer_f32_t& dst
 	);
 
-	void configure(const float sampling_rate, const float deviation_hz) {
-		k = static_cast<float>(32767.0f / (2.0 * pi * deviation_hz / sampling_rate));
-	}
+	buffer_s16_t execute(
+		const buffer_c16_t& src,
+		const buffer_s16_t& dst
+	);
+
+	void configure(const float sampling_rate, const float deviation_hz);
 
 private:
-	complex16_t::rep_type z_;
-	float k;
+	complex16_t::rep_type z_ { 0 };
+	float kf { 0 };
+	float ks16 { 0 };
 };
 
 } /* namespace demodulate */

firmware/baseband-tx/dsp_iir.cpp

@@ -1,57 +0,0 @@
-/*
- * Copyright (C) 2015 Jared Boone, ShareBrained Technology, Inc.
- *
- * This file is part of PortaPack.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; see the file COPYING.  If not, write to
- * the Free Software Foundation, Inc., 51 Franklin Street,
- * Boston, MA 02110-1301, USA.
- */
-
-#include "dsp_iir.hpp"
-
-#include <hal.h>
-
-void IIRBiquadFilter::configure(const iir_biquad_config_t& new_config) {
-	config = new_config;
-}
-
-void IIRBiquadFilter::execute(const buffer_f32_t& buffer_in, const buffer_f32_t& buffer_out) {
-	const auto a_ = config.a;
-	const auto b_ = config.b;
-	
-	auto x_ = x;
-	auto y_ = y;
-
-	// TODO: Assert that buffer_out.count == buffer_in.count.
-	for(size_t i=0; i<buffer_out.count; i++) {
-		x_[0] = x_[1];
-		x_[1] = x_[2];
-		x_[2] = buffer_in.p[i];
-
-		y_[0] = y_[1];
-		y_[1] = y_[2];
-		y_[2] = b_[0] * x_[2] + b_[1] * x_[1] + b_[2] * x_[0]
-		                      - a_[1] * y_[1] - a_[2] * y_[0];
-
-		buffer_out.p[i] = y_[2];
-	}
-
-	x = x_;
-	y = y_;
-}
-
-void IIRBiquadFilter::execute_in_place(const buffer_f32_t& buffer) {
-	execute(buffer, buffer);
-}

firmware/baseband-tx/dsp_iir.hpp

@@ -1,70 +0,0 @@
-/*
- * Copyright (C) 2015 Jared Boone, ShareBrained Technology, Inc.
- *
- * This file is part of PortaPack.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; see the file COPYING.  If not, write to
- * the Free Software Foundation, Inc., 51 Franklin Street,
- * Boston, MA 02110-1301, USA.
- */
-
-#ifndef __DSP_IIR_H__
-#define __DSP_IIR_H__
-
-#include <array>
-
-#include "dsp_types.hpp"
-
-struct iir_biquad_config_t {
-	std::array<float, 3> b;
-	std::array<float, 3> a;
-};
-
-constexpr iir_biquad_config_t iir_config_passthrough {
-	{ { 1.0f, 0.0f, 0.0f } },
-	{ { 0.0f, 0.0f, 0.0f } },
-};
-
-constexpr iir_biquad_config_t iir_config_no_pass {
-	{ { 0.0f, 0.0f, 0.0f } },
-	{ { 0.0f, 0.0f, 0.0f } },
-};
-
-class IIRBiquadFilter {
-public:
-	// http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt
-	constexpr IIRBiquadFilter(
-	) : IIRBiquadFilter(iir_config_no_pass)
-	{
-	}
-
-	// Assume all coefficients are normalized so that a0=1.0
-	constexpr IIRBiquadFilter(
-		const iir_biquad_config_t& config
-	) : config(config)
-	{
-	}
-
-	void configure(const iir_biquad_config_t& new_config);
-
-	void execute(const buffer_f32_t& buffer_in, const buffer_f32_t& buffer_out);
-	void execute_in_place(const buffer_f32_t& buffer);
-
-private:
-	iir_biquad_config_t config;
-	std::array<float, 3> x { { 0.0f, 0.0f, 0.0f } };
-	std::array<float, 3> y { { 0.0f, 0.0f, 0.0f } };
-};
-
-#endif/*__DSP_IIR_H__*/

firmware/baseband-tx/dsp_iir_config.hpp

@@ -1,37 +0,0 @@
-/*
- * Copyright (C) 2015 Jared Boone, ShareBrained Technology, Inc.
- *
- * This file is part of PortaPack.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; see the file COPYING.  If not, write to
- * the Free Software Foundation, Inc., 51 Franklin Street,
- * Boston, MA 02110-1301, USA.
- */
-
-#ifndef __DSP_IIR_CONFIG_H__
-#define __DSP_IIR_CONFIG_H__
-
-#include "dsp_iir.hpp"
-
-constexpr iir_biquad_config_t audio_hpf_config {
-	{  0.93346032f, -1.86687724f,  0.93346032f },
-	{  1.0f       , -1.97730264f,  0.97773668f }
-};
-
-constexpr iir_biquad_config_t non_audio_hpf_config {
-	{  0.51891061f, -0.95714180f,  0.51891061f },
-	{  1.0f       , -0.79878302f,  0.43960231f }
-};
-
-#endif/*__DSP_IIR_CONFIG_H__*/

firmware/baseband-tx/event_m4.cpp

@@ -86,11 +86,9 @@ void EventDispatcher::dispatch(const eventmask_t events) {
 }
 
 void EventDispatcher::handle_baseband_queue() {
-	std::array<uint8_t, Message::MAX_SIZE> message_buffer;
-	while(Message* const message = shared_memory.baseband_queue.peek(message_buffer)) {
-		on_message(message);
-		shared_memory.baseband_queue.skip();
-	}
+	shared_memory.baseband_queue.handle([this](Message* const message) {
+		this->on_message(message);
+	});
 }
 
 void EventDispatcher::on_message(const Message* const message) {

firmware/baseband-tx/halconf.h

@@ -261,7 +261,7 @@
  *          lower priority, this may slow down the driver a bit however.
  */
 #if !defined(SDC_NICE_WAITING) || defined(__DOXYGEN__)
-#define SDC_NICE_WAITING            TRUE
+#define SDC_NICE_WAITING            FALSE
 #endif
 
 /*===========================================================================*/

firmware/baseband-tx/main.cpp

@@ -72,19 +72,10 @@ void __late_init(void) {
 }
 
 static void init() {
-	i2s::i2s0::configure(
-		audio::i2s0_config_tx,
-		audio::i2s0_config_rx,
-		audio::i2s0_config_dma
-	);
-
 	audio::dma::init();
 	audio::dma::configure();
 	audio::dma::enable();
 
-	i2s::i2s0::tx_start();
-	i2s::i2s0::rx_start();
-
 	LPC_CREG->DMAMUX = portapack::gpdma_mux;
 	gpdma::controller.enable();
 	nvicEnableVector(DMA_IRQn, CORTEX_PRIORITY_MASK(LPC_DMA_IRQ_PRIORITY));
@@ -128,185 +119,3 @@ int main(void) {
 
 	return 0;
 }
-
-/*
-	void run() override {
-		while(true) {
-			if (direction == baseband::Direction::Transmit) {
-				const auto buffer_tmp = baseband::dma::wait_for_tx_buffer();
-				
-				const buffer_c8_t buffer {
-					buffer_tmp.p, buffer_tmp.count, baseband_configuration.sampling_rate
-				};
-
-				if( baseband_processor ) {
-					baseband_processor->execute(buffer);
-				}
-			} else {
-				const auto buffer_tmp = baseband::dma::wait_for_rx_buffer();
-				
-				const buffer_c8_t buffer {
-					buffer_tmp.p, buffer_tmp.count, baseband_configuration.sampling_rate
-				};
-
-				if( baseband_processor ) {
-					baseband_processor->execute(buffer);
-				}
-			}
-		}
-	}
-};
-
-class ToneProcessor : public BasebandProcessor {
-public:
-	void execute(buffer_c8_t buffer) override {
-        
-		for (size_t i = 0; i<buffer.count; i++) {
-			
-			//Sample generation 2.28M/10 = 228kHz
-			if (s >= 9) {
-				s = 0;
-				aphase += 353205;	// DEBUG
-				//sample = sintab[(aphase & 0x03FF0000)>>16];
-			} else {
-				s++;
-			}
-			
-			//sample = sintab[(aphase & 0x03FF0000)>>16];
-			
-			//FM
-			frq = sample * 500;		// DEBUG
-			
-			phase = (phase + frq);
-			sphase = phase + (256<<16);
-
-			//re = sintab[(sphase & 0x03FF0000)>>16];
-			//im = sintab[(phase & 0x03FF0000)>>16];
-			
-			buffer.p[i] = {(int8_t)re,(int8_t)im};
-		}
-	}
-
-private:
-	int8_t re, im;
-	uint8_t s;
-    uint32_t sample_count;
-	uint32_t aphase, phase, sphase;
-	int32_t sample, sig, frq;
-};
-
-char ram_loop[32];
-typedef int (*fn_ptr)(void);
-fn_ptr loop_ptr;
-	
-void ram_loop_fn(void) {
-	while(1) {}
-}
-	
-void wait_for_switch(void) {
-	memcpy(&ram_loop[0], reinterpret_cast<char*>(&ram_loop_fn), 32);
-	loop_ptr = reinterpret_cast<fn_ptr>(&ram_loop[0]);
-	ReadyForSwitchMessage message;
-	shared_memory.application_queue.push(message);
-	(*loop_ptr)();
-}
-		
-int main(void) {
-	init();
-
-	events_initialize(chThdSelf());
-	m0apptxevent_interrupt_enable();
-
-	EventDispatcher event_dispatcher;
-	auto& message_handlers = event_dispatcher.message_handlers();
-	
-	message_handlers.register_handler(Message::ID::ModuleID,
-		[](Message* p) {
-			ModuleIDMessage reply;
-			auto message = static_cast<ModuleIDMessage*>(p);
-			if (message->query == true) {	// Shouldn't be needed
-				memcpy(reply.md5_signature, (const void *)(0x10087FF0), 16);
-				reply.query = false;
-				shared_memory.application_queue.push(reply);
-			}
-		}
-	);
-
-	message_handlers.register_handler(Message::ID::BasebandConfiguration,
-		[&message_handlers](const Message* const p) {
-			auto message = reinterpret_cast<const BasebandConfigurationMessage*>(p);
-			if( message->configuration.mode != baseband_thread.baseband_configuration.mode ) {
-
-				if( baseband_thread.baseband_processor ) {
-					i2s::i2s0::tx_mute();
-					baseband::dma::disable();
-				}
-
-				// TODO: Timing problem around disabling DMA and nulling and deleting old processor
-				auto old_p = baseband_thread.baseband_processor;
-				baseband_thread.baseband_processor = nullptr;
-				delete old_p;
-
-				switch(message->configuration.mode) {
-				case TX_RDS:
-					direction = baseband::Direction::Transmit;
-					baseband_thread.baseband_processor = new RDSProcessor();
-					break;
-				
-				case TX_LCR:
-					direction = baseband::Direction::Transmit;
-					baseband_thread.baseband_processor = new LCRFSKProcessor();
-					break;
-			
-				case TX_TONE:
-					direction = baseband::Direction::Transmit;
-					baseband_thread.baseband_processor = new ToneProcessor();
-					break;
-				
-				case TX_JAMMER:
-					direction = baseband::Direction::Transmit;
-					baseband_thread.baseband_processor = new JammerProcessor();
-					break;
-					
-				case TX_XYLOS:
-					direction = baseband::Direction::Transmit;
-					baseband_thread.baseband_processor = new XylosProcessor();
-					break;
-					
-				case PLAY_AUDIO:
-					direction = baseband::Direction::Transmit;
-					baseband_thread.baseband_processor = new PlayAudioProcessor();
-					message_handlers.register_handler(Message::ID::FIFOData,
-						[](Message* p) {
-							auto message = static_cast<FIFODataMessage*>(p);
-							baseband_thread.baseband_processor->fill_buffer(message->data);
-						}
-					);
-					break;
-					
-				case SWITCH:
-					wait_for_switch();
-
-				default:
-					break;
-				}
-
-				if( baseband_thread.baseband_processor )
-					baseband::dma::enable(direction);
-			}
-			
-			baseband::dma::configure(
-				baseband_buffer->data(),
-				direction
-			);
-
-			baseband_thread.baseband_configuration = message->configuration;
-		}
-	);
-
-	message_handlers.register_handler(Message::ID::Shutdown,
-		[&event_dispatcher](const Message* const) {
-			event_dispatcher.request_stop();
-		}
-	);
-*/

firmware/baseband-tx/matched_filter.cpp

@@ -21,9 +21,27 @@
 
 #include "matched_filter.hpp"
 
+#include <algorithm>
+#include <cmath>
+
+#include "utility.hpp"
+
 namespace dsp {
 namespace matched_filter {
 
+void MatchedFilter::configure(
+	const tap_t* const taps,
+	const size_t taps_count,
+	const size_t decimation_factor
+) {
+	samples_ = std::make_unique<samples_t>(taps_count);
+	taps_reversed_ = std::make_unique<taps_t>(taps_count);
+	taps_count_ = taps_count;
+	decimation_factor_ = decimation_factor;
+	output = 0;
+	std::reverse_copy(&taps[0], &taps[taps_count], &taps_reversed_[0]);
+}
+
 bool MatchedFilter::execute_once(
 	const sample_t input
 ) {

firmware/baseband-tx/matched_filter.hpp

@@ -22,17 +22,10 @@
 #ifndef __MATCHED_FILTER_H__
 #define __MATCHED_FILTER_H__
 
-#include "utility.hpp"
-
 #include <cstddef>
-
 #include <complex>
-#include <array>
 #include <memory>
 
-#include <algorithm>
-#include <numeric>
-
 namespace dsp {
 namespace matched_filter {
 
@@ -61,11 +54,7 @@ public:
 		const T& taps,
 		size_t decimation_factor
 	) {
-		samples_ = std::make_unique<samples_t>(taps.size());
-		taps_reversed_ = std::make_unique<taps_t>(taps.size());
-		taps_count_ = taps.size();
-		decimation_factor_ = decimation_factor;
-		std::reverse_copy(taps.cbegin(), taps.cend(), &taps_reversed_[0]);
+		configure(taps.data(), taps.size(), decimation_factor);
  	}
 
 	bool execute_once(const sample_t input);
@@ -82,7 +71,7 @@ private:
 	size_t taps_count_ { 0 };
 	size_t decimation_factor_ { 1 };
 	size_t decimation_phase { 0 };
-	float output;
+	float output { 0 };
 
 	void shift_by_decimation_factor();
 
@@ -93,6 +82,12 @@ private:
 	bool is_new_decimation_cycle() const {
 		return (decimation_phase == 0);
 	}
+
+	void configure(
+		const tap_t* const taps,
+		const size_t taps_count,
+		const size_t decimation_factor
+	);
 };
 
 } /* namespace matched_filter */

firmware/baseband-tx/packet_builder.hpp

@@ -28,19 +28,33 @@
 #include <functional>
 
 #include "bit_pattern.hpp"
+#include "baseband_packet.hpp"
+
+struct NeverMatch {
+	bool operator()(const BitHistory&, const size_t) const {
+		return false;
+	}
+};
+
+struct FixedLength {
+	bool operator()(const BitHistory&, const size_t symbols_received) const {
+		return symbols_received >= length;
+	}
+
+	const size_t length;
+};
 
 template<typename PreambleMatcher, typename UnstuffMatcher, typename EndMatcher>
 class PacketBuilder {
 public:
-	using PayloadType = std::bitset<1024>;
-	using PayloadHandlerFunc = std::function<void(const PayloadType& payload, const size_t bits_received)>;
+	using PayloadHandlerFunc = std::function<void(const baseband::Packet& packet)>;
 
 	PacketBuilder(
 		const PreambleMatcher preamble_matcher,
 		const UnstuffMatcher unstuff_matcher,
 		const EndMatcher end_matcher,
-		const PayloadHandlerFunc payload_handler
-	) : payload_handler { payload_handler },
+		PayloadHandlerFunc payload_handler
+	) : payload_handler { std::move(payload_handler) },
 		preamble(preamble_matcher),
 		unstuff(unstuff_matcher),
 		end(end_matcher)
@@ -64,18 +78,24 @@ public:
 
 		switch(state) {
 		case State::Preamble:
-			if( preamble(bit_history, bits_received) ) {
+			if( preamble(bit_history, packet.size()) ) {
 				state = State::Payload;
 			}
 			break;
 
 		case State::Payload:
-			if( !unstuff(bit_history, bits_received) ) {
-				payload[bits_received++] = symbol;
+			if( !unstuff(bit_history, packet.size()) ) {
+				packet.add(symbol);
 			}
 
-			if( end(bit_history, bits_received) ) {
-				payload_handler(payload, bits_received);
+			if( end(bit_history, packet.size()) ) {
+				// NOTE: This check is to avoid std::function nullptr check, which
+				// brings in "_ZSt25__throw_bad_function_callv" and a lot of extra code.
+				// TODO: Make payload_handler known at compile time.
+				if( payload_handler ) {
+					packet.set_timestamp(Timestamp::now());
+					payload_handler(packet);
+				}
 				reset_state();
 			} else {
 				if( packet_truncated() ) {
@@ -97,7 +117,7 @@ private:
 	};
 
 	bool packet_truncated() const {
-		return bits_received >= payload.size();
+		return packet.size() >= packet.capacity();
 	}
 
 	const PayloadHandlerFunc payload_handler;
@@ -107,12 +127,11 @@ private:
 	UnstuffMatcher unstuff;
 	EndMatcher end;
 
-	size_t bits_received { 0 };
 	State state { State::Preamble };
-	PayloadType payload;
+	baseband::Packet packet;
 
 	void reset_state() {
-		bits_received = 0;
+		packet.clear();
 		state = State::Preamble;
 	}
 };

firmware/baseband-tx/proc_audiotx.cpp

@@ -23,9 +23,84 @@
 #include "proc_audiotx.hpp"
 #include "portapack_shared_memory.hpp"
 #include "sine_table.hpp"
+#include "audio_output.hpp"
+#include "lfsr_random.hpp"
 
 #include <cstdint>
 
-void AudioTXProcessor::execute(const buffer_c8_t& buffer) {
+uint32_t lfsr(uint32_t v) {
 
+	enum {
+		length         = 31,
+		tap_0          = 31,
+		tap_1          = 18,
+		shift_amount_0 = 12,
+		shift_amount_1 = 12,
+		shift_amount_2 =  8
+	};
+
+	const lfsr_word_t zero = 0;
+	v = (
+		(
+			v << shift_amount_0
+		) | (
+			(
+				(v >> (tap_0 - shift_amount_0)) ^
+				(v >> (tap_1 - shift_amount_0))
+			) & (
+				~(~zero << shift_amount_0)
+			)
+		)
+	);
+	v = (
+		(
+			v << shift_amount_1
+		) | (
+			(
+				(v >> (tap_0 - shift_amount_1)) ^
+				(v >> (tap_1 - shift_amount_1))
+			) & (
+				~(~zero << shift_amount_1)
+			)
+		)
+	);
+	v = (
+		(
+			v << shift_amount_2
+		) | (
+			(
+				(v >> (tap_0 - shift_amount_2)) ^
+				(v >> (tap_1 - shift_amount_2))
+			) & (
+				~(~zero << shift_amount_2)
+			)
+		)
+	);
+	
+	return v;
+}
+
+void AudioTXProcessor::execute(const buffer_c8_t& buffer){
+	
+	for (size_t i = 0; i<buffer.count; i++) {
+		sample = (sine_table_f32[(aphase & 0x03FF0000)>>18]*127);	//(int8_t)lfsr(sample + i);
+
+		if (bc & 0x40)
+			aphase += 60000;
+		else
+			aphase += 90000;
+			
+		//FM
+		frq = sample * 2500;
+		
+		phase = (phase + frq);
+		sphase = phase + (256<<16);
+
+		re = (sine_table_f32[(sphase & 0x03FF0000)>>18]*127);
+		im = (sine_table_f32[(phase & 0x03FF0000)>>18]*127);
+		
+		buffer.p[i] = {(int8_t)re,(int8_t)im};
+	}
+	
+	bc++;
 }

firmware/baseband-tx/proc_audiotx.hpp

@@ -25,23 +25,25 @@
 
 #include "baseband_processor.hpp"
 
-#define SAMPLERATE 44100/4
+#include "dsp_decimate.hpp"
+#include "dsp_demodulate.hpp"
+
+#include "audio_output.hpp"
+#include "spectrum_collector.hpp"
+
+#include <cstdint>
 
 class AudioTXProcessor : public BasebandProcessor {
 public:
 	void execute(const buffer_c8_t& buffer) override;
 	
-
 private:
-	int8_t audio_fifo[SAMPLERATE];
-
 	int8_t re, im;
 	uint8_t s, as = 0, ai;
     uint8_t byte_pos = 0;
     uint8_t digit = 0;
 	uint32_t aphase, phase, sphase;
-	int32_t sample, frq;
-	TXDoneMessage message;
+	int32_t sample, frq, bc;
 };
 
 #endif

firmware/baseband-tx/proc_playaudio.cpp

@@ -23,14 +23,17 @@
 #include "proc_playaudio.hpp"
 #include "portapack_shared_memory.hpp"
 #include "sine_table.hpp"
+#include "audio_output.hpp"
 
 #include <cstdint>
 
-// This is diry :(
-void PlayAudioProcessor::fill_buffer(int8_t * inptr) {
-	memcpy(&audio_fifo[fifo_put], inptr, 1024);
-	fifo_put = (fifo_put + 1024) & 0x0FFF;
-	asked = false;
+void PlayAudioProcessor::on_message(const Message* const msg) {
+	if (msg->id == Message::ID::FIFOData) {
+		const auto message = static_cast<const FIFODataMessage*>(msg);
+		memcpy(&audio_fifo[fifo_put], message->data, 1024);
+		fifo_put = (fifo_put + 1024) & 0x0FFF;
+		asked = false;
+	}
 }
 
 void PlayAudioProcessor::execute(const buffer_c8_t& buffer){
@@ -69,5 +72,5 @@ void PlayAudioProcessor::execute(const buffer_c8_t& buffer){
 		buffer.p[i] = {(int8_t)re,(int8_t)im};
 	}
 	
-	//fill_audio_buffer(preview_audio_buffer);
+	//AudioOutput::fill_audio_buffer(preview_audio_buffer, true);
 }

firmware/baseband-tx/proc_playaudio.hpp

@@ -28,7 +28,7 @@
 class PlayAudioProcessor : public BasebandProcessor {
 public:
 	void execute(const buffer_c8_t& buffer) override;
-	void fill_buffer(int8_t * inptr);
+	void on_message(const Message* const msg) override;
 
 private:
 	int8_t audio_fifo[4096];		// Probably too much (=85ms @ 48000Hz)

firmware/baseband-tx/rssi_dma.cpp

@@ -33,6 +33,8 @@ using namespace lpc43xx;
 #include "portapack_dma.hpp"
 #include "portapack_adc.hpp"
 
+#include "thread_wait.hpp"
+
 namespace rf {
 namespace rssi {
 namespace dma {
@@ -99,20 +101,19 @@ static buffers_config_t			buffers_config;
 static sample_t				*samples	{ nullptr };
 static gpdma::channel::LLI	*lli		{ nullptr };
 
-static Semaphore semaphore;
-static volatile const gpdma::channel::LLI* next_lli = nullptr;
+static ThreadWait thread_wait;
 
 static void transfer_complete() {
-	next_lli = gpdma_channel.next_lli();
-	chSemSignalI(&semaphore);
+	const auto next_lli_index = gpdma_channel.next_lli() - &lli[0];
+	thread_wait.wake_from_interrupt(next_lli_index);
 }
 
 static void dma_error() {
+	thread_wait.wake_from_interrupt(-1);
 	disable();
 }
 
 void init() {
-	chSemInit(&semaphore, 0);
 	gpdma_channel.set_handlers(transfer_complete, dma_error);
 
 	// LPC_GPDMA->SYNC |= (1 << gpdma_peripheral);
@@ -147,8 +148,6 @@ void free() {
 void enable() {
 	const auto gpdma_config = config();
 	gpdma_channel.configure(lli[0], gpdma_config);
-
-	chSemReset(&semaphore, 0);
 	gpdma_channel.enable();
 }
 
@@ -161,16 +160,11 @@ void disable() {
 }
 
 rf::rssi::buffer_t wait_for_buffer() {
-	const auto status = chSemWait(&semaphore);
-	if( status == RDY_OK ) {
-		const auto next = next_lli;
-		if( next ) {
-			const size_t next_index = next - &lli[0];
-			const size_t free_index = (next_index + buffers_config.count - 2) % buffers_config.count;
-			return { reinterpret_cast<sample_t*>(lli[free_index].destaddr), buffers_config.items_per_buffer };
-		} else {
-			return { nullptr, 0 };
-		}
+	const auto next_index = thread_wait.sleep();
+
+	if( next_index >= 0 ) {
+		const size_t free_index = (next_index + buffers_config.count - 2) % buffers_config.count;
+		return { reinterpret_cast<sample_t*>(lli[free_index].destaddr), buffers_config.items_per_buffer };
 	} else {
 		// TODO: Should I return here, or loop if RDY_RESET?
 		return { nullptr, 0 };

firmware/baseband-tx/rssi_stats_collector.hpp

@@ -31,7 +31,7 @@
 class RSSIStatisticsCollector {
 public:
 	template<typename Callback>
-	void process(rf::rssi::buffer_t buffer, Callback callback) {
+	void process(const rf::rssi::buffer_t& buffer, Callback callback) {
 		auto p = buffer.p;
 		if( p == nullptr ) {
 			return;

firmware/baseband-tx/rssi_thread.hpp

@@ -30,11 +30,6 @@
 
 class RSSIThread : public ThreadBase {
 public:
-	RSSIThread(
-	) : ThreadBase { "rssi" }
-	{
-	}
-
 	Thread* start(const tprio_t priority);
 
 private:

firmware/baseband-tx/spectrum_collector.cpp

@@ -117,8 +117,8 @@ void SpectrumCollector::update() {
 			// Three point Hamming window.
 			const auto corrected_sample = channel_spectrum[i] * 0.54f
 				+ (channel_spectrum[(i-1) & 0xff] + channel_spectrum[(i+1) & 0xff]) * -0.23f;
-			const auto mag2 = magnitude_squared(corrected_sample);
-			const float db = complex16_mag_squared_to_dbv_norm(mag2);
+			const auto mag2 = magnitude_squared(corrected_sample * (1.0f / 32768.0f));
+			const float db = mag2_to_dbv_norm(mag2);
 			constexpr float mag_scale = 5.0f;
 			const unsigned int v = (db * mag_scale) + 255.0f;
 			spectrum.db[i] = std::max(0U, std::min(255U, v));

firmware/baseband-tx/spectrum_collector.hpp

@@ -35,7 +35,8 @@
 class SpectrumCollector {
 public:
 	constexpr SpectrumCollector(
-	) : channel_spectrum_decimator { 1 }
+	) : channel_spectrum_decimator { 1 },
+		fifo { fifo_data, ChannelSpectrumConfigMessage::fifo_k }
 	{
 	}
 
@@ -50,8 +51,9 @@ public:
 	);
 
 private:
-	BlockDecimator<256> channel_spectrum_decimator;
+	BlockDecimator<complex16_t, 256> channel_spectrum_decimator;
 	ChannelSpectrumFIFO fifo;
+	ChannelSpectrum fifo_data[1 << ChannelSpectrumConfigMessage::fifo_k];
 
 	volatile bool channel_spectrum_request_update { false };
 	bool streaming { false };

firmware/baseband-tx/thread_base.hpp

@@ -26,24 +26,17 @@
 
 class ThreadBase {
 public:
-	constexpr ThreadBase(
-		const char* const name
-	) : name { name }
-	{
-	}
-
+	virtual ~ThreadBase() = default;
+	
 protected:
 	static msg_t fn(void* arg) {
 		auto obj = static_cast<ThreadBase*>(arg);
-		chRegSetThreadName(obj->name);
 		obj->run();
 
 		return 0;
 	}
 
 private:
-	const char* const name;
-
 	virtual void run() = 0;
 };