diff --git a/firmware/application/analog_audio_app.cpp b/firmware/application/analog_audio_app.cpp
index 91a682f5..199c68d9 100644
--- a/firmware/application/analog_audio_app.cpp
+++ b/firmware/application/analog_audio_app.cpp
@@ -26,6 +26,7 @@
 #include "portapack.hpp"
 #include "portapack_persistent_memory.hpp"
 using namespace portapack;
+using namespace tonekey;
 
 #include "audio.hpp"
 #include "file.hpp"
@@ -97,6 +98,7 @@ AnalogAudioView::AnalogAudioView(
 		&field_vga,
 		&options_modulation,
 		&field_volume,
+		&text_ctcss,
 		&record_view,
 		&waterfall,
 	});
@@ -203,6 +205,8 @@ void AnalogAudioView::remove_options_widget() {
 		options_widget.reset();
 	}
 
+	text_ctcss.hidden(true);
+
 	field_lna.set_style(nullptr);
 	options_modulation.set_style(nullptr);
 	field_frequency.set_style(nullptr);
@@ -253,7 +257,7 @@ void AnalogAudioView::on_show_options_modulation() {
 		break;
 
 	case ReceiverModel::Mode::NarrowbandFMAudio:
-		widget = std::make_unique<NBFMOptionsView>(options_view_rect, &style_options_group);
+		widget = std::make_unique<NBFMOptionsView>(nbfm_view_rect, &style_options_group);
 		break;
 
 	default:
@@ -262,6 +266,8 @@ void AnalogAudioView::on_show_options_modulation() {
 
 	set_options_widget(std::move(widget));
 	options_modulation.set_style(&style_options_group);
+	
+	if (modulation == ReceiverModel::Mode::NarrowbandFMAudio) text_ctcss.hidden(false);
 }
 
 void AnalogAudioView::on_frequency_step_changed(rf::Frequency f) {
@@ -326,4 +332,24 @@ void AnalogAudioView::squelched() {
 	if (exit_on_squelch) nav_.pop();
 }
 
+void AnalogAudioView::handle_coded_squelch(const uint32_t value) {
+	//const std::string value_string = to_string_dec_uint(value / 10) + "." + to_string_dec_uint(value % 10);
+	float diff, min_diff = value;
+	size_t min_idx { 0 };
+	size_t c;
+	
+	for (c = 0; c < KEY_TONES_NB; c++) {
+		diff = abs(((float)value / 100.0) - tone_keys[c].second);
+		if (diff < min_diff) {
+			min_idx = c;
+			min_diff = diff;
+		}
+	}
+	
+	if (min_diff < 40)
+		text_ctcss.set("CTCSS " + tone_keys[min_idx].first);
+	else
+		text_ctcss.set("???");
+}
+
 } /* namespace ui */
diff --git a/firmware/application/analog_audio_app.hpp b/firmware/application/analog_audio_app.hpp
index 61294e01..fdbf5b1a 100644
--- a/firmware/application/analog_audio_app.hpp
+++ b/firmware/application/analog_audio_app.hpp
@@ -30,6 +30,8 @@
 
 #include "ui_font_fixed_8x16.hpp"
 
+#include "tone_key.hpp"
+
 namespace ui {
 
 constexpr Style style_options_group {
@@ -68,7 +70,6 @@ private:
 		{ 0 * 8, 0 * 16, 2 * 8, 1 * 16 },
 		"BW",
 	};
-
 	OptionsField options_config {
 		{ 3 * 8, 0 * 16 },
 		4,
@@ -80,14 +81,13 @@ private:
 	};
 	
 	Text text_squelch {
-		{ 9 * 8, 0 * 16, 7 * 8, 1 * 16 },
-		"SQ    /100"
+		{ 9 * 8, 0 * 16, 8 * 8, 1 * 16 },
+		"SQ   /99"
 	};
-
 	NumberField field_squelch {
 		{ 12 * 8, 0 * 16 },
-		3,
-		{ 0, 100 },
+		2,
+		{ 0, 99 },
 		1,
 		' ',
 	};
@@ -108,6 +108,7 @@ private:
 	static constexpr ui::Dim header_height = 3 * 16;
 
 	const Rect options_view_rect { 0 * 8, 1 * 16, 30 * 8, 1 * 16 };
+	const Rect nbfm_view_rect { 0 * 8, 1 * 16, 18 * 8, 1 * 16 };
 
 	NavigationView& nav_;
 	bool exit_on_squelch { false };
@@ -154,6 +155,11 @@ private:
 		1,
 		' ',
 	};
+	
+	Text text_ctcss {
+		{ 19 * 8, 1 * 16, 11 * 8, 1 * 16 },
+		""
+	};
 
 	std::unique_ptr<Widget> options_widget { };
 
@@ -181,6 +187,7 @@ private:
 	void update_modulation(const ReceiverModel::Mode modulation);
 	
 	void squelched();
+	void handle_coded_squelch(const uint32_t value);
 	
 	MessageHandlerRegistration message_handler_squelch_signal {
 		Message::ID::RequestSignal,
@@ -189,7 +196,14 @@ private:
 			this->squelched();
 		}
 	};
-
+	
+	MessageHandlerRegistration message_handler_coded_squelch {
+		Message::ID::CodedSquelch,
+		[this](const Message* const p) {
+			const auto message = *reinterpret_cast<const CodedSquelchMessage*>(p);
+			this->handle_coded_squelch(message.value);
+		}
+	};
 };
 
 } /* namespace ui */
diff --git a/firmware/application/baseband_api.cpp b/firmware/application/baseband_api.cpp
index d8867d8d..79094834 100644
--- a/firmware/application/baseband_api.cpp
+++ b/firmware/application/baseband_api.cpp
@@ -171,10 +171,9 @@ void set_fifo_data(const int8_t * data) {
 	send_message(&message);
 }
 
-void set_pwmrssi(int32_t avg, bool enabled) {
-	const PWMRSSIConfigureMessage message {
+void set_pitch_rssi(int32_t avg, bool enabled) {
+	const PitchRSSIConfigureMessage message {
 		enabled,
-		1000,	// 1kHz
 		avg
 	};
 	send_message(&message);	
diff --git a/firmware/application/baseband_api.hpp b/firmware/application/baseband_api.hpp
index 78b93950..01c38a9c 100644
--- a/firmware/application/baseband_api.hpp
+++ b/firmware/application/baseband_api.hpp
@@ -63,7 +63,7 @@ void set_sstv_data(const uint8_t vis_code, const uint32_t pixel_duration);
 void set_audiotx_data(const uint32_t divider, const uint32_t bw, const uint32_t gain_x10,
 					const uint32_t tone_key_delta, const float tone_key_mix_weight);
 void set_fifo_data(const int8_t * data);
-void set_pwmrssi(int32_t avg, bool enabled);
+void set_pitch_rssi(int32_t avg, bool enabled);
 void set_afsk_data(const uint32_t afsk_samples_per_bit, const uint32_t afsk_phase_inc_mark, const uint32_t afsk_phase_inc_space,
 					const uint8_t afsk_repeat, const uint32_t afsk_bw, const uint8_t symbol_count);
 void kill_afsk();
diff --git a/firmware/application/tone_key.cpp b/firmware/application/tone_key.cpp
index 044e8e26..60f7a6a4 100644
--- a/firmware/application/tone_key.cpp
+++ b/firmware/application/tone_key.cpp
@@ -79,7 +79,8 @@ const tone_key_t tone_keys[] = {
 	{ "50 0Z", 254.100 },
 	{ "Axient 28kHz", 28000.0 },
 	{ "Sennheiser 32.768k", 32768.0 },
-	{ "Sennheiser 32kHz", 32000.0 },
+	{ "Sennheiser 32.000k", 32000.0 },
+	{ "Sony 32.382k", 32382.0 },
 	{ "Shure 19kHz", 19000.0 }
 };
 
diff --git a/firmware/application/tone_key.hpp b/firmware/application/tone_key.hpp
index d16faf62..41fdcd67 100644
--- a/firmware/application/tone_key.hpp
+++ b/firmware/application/tone_key.hpp
@@ -28,7 +28,7 @@
 
 using namespace ui;
 
-#define KEY_TONES_NB (sizeof(tone_keys) / sizeof(tone_keys[0]))
+#define KEY_TONES_NB	56
 
 namespace tonekey {
 
diff --git a/firmware/application/ui_record_view.cpp b/firmware/application/ui_record_view.cpp
index 640661a5..aa8cea1c 100644
--- a/firmware/application/ui_record_view.cpp
+++ b/firmware/application/ui_record_view.cpp
@@ -36,21 +36,20 @@ using namespace portapack;
 
 namespace ui {
 
-void RecordView::toggle_pwmrssi() {
-	pwmrssi_enabled = !pwmrssi_enabled;
+void RecordView::toggle_pitch_rssi() {
+	pitch_rssi_enabled = !pitch_rssi_enabled;
 	
 	// Send to RSSI widget
-	const PWMRSSIConfigureMessage message {
-		pwmrssi_enabled,
-		64,
+	const PitchRSSIConfigureMessage message {
+		pitch_rssi_enabled,
 		0
 	};
 	shared_memory.application_queue.push(message);
 	
-	if( !pwmrssi_enabled ) {
-		button_pwmrssi.set_foreground(Color::orange());
+	if( !pitch_rssi_enabled ) {
+		button_pitch_rssi.set_foreground(Color::orange());
 	} else {
-		button_pwmrssi.set_foreground(Color::green());
+		button_pitch_rssi.set_foreground(Color::green());
 	}
 }
 
@@ -68,7 +67,7 @@ RecordView::RecordView(
 {
 	add_children({
 		&rect_background,
-		&button_pwmrssi,
+		&button_pitch_rssi,
 		&button_record,
 		&text_record_filename,
 		&text_record_dropped,
@@ -77,8 +76,8 @@ RecordView::RecordView(
 
 	rect_background.set_parent_rect({ { 0, 0 }, size() });
 	
-	button_pwmrssi.on_select = [this](ImageButton&) {
-		this->toggle_pwmrssi();
+	button_pitch_rssi.on_select = [this](ImageButton&) {
+		this->toggle_pitch_rssi();
 	};
 
 	button_record.on_select = [this](ImageButton&) {
@@ -238,8 +237,8 @@ void RecordView::update_status_display() {
 		text_record_dropped.set(s);
 	}
 	
-	if (pwmrssi_enabled) {
-		button_pwmrssi.invert_colors();
+	if (pitch_rssi_enabled) {
+		button_pitch_rssi.invert_colors();
 	}
 
 	if( sampling_rate ) {
diff --git a/firmware/application/ui_record_view.hpp b/firmware/application/ui_record_view.hpp
index 02ea30c1..72603570 100644
--- a/firmware/application/ui_record_view.hpp
+++ b/firmware/application/ui_record_view.hpp
@@ -64,7 +64,7 @@ public:
 
 private:
 	void toggle();
-	void toggle_pwmrssi();
+	void toggle_pitch_rssi();
 	Optional<File::Error> write_metadata_file(const std::filesystem::path& filename);
 
 	void on_tick_second();
@@ -73,7 +73,7 @@ private:
 	void handle_capture_thread_done(const File::Error error);
 	void handle_error(const File::Error error);
 
-	bool pwmrssi_enabled = false;
+	bool pitch_rssi_enabled = false;
 	const std::filesystem::path filename_stem_pattern;
 	const FileType file_type;
 	const size_t write_size;
@@ -85,7 +85,7 @@ private:
 		Color::black()
 	};
 	
-	ImageButton button_pwmrssi {
+	ImageButton button_pitch_rssi {
 		{ 2, 0 * 16, 3 * 8, 1 * 16 },
 		&bitmap_rssipwm,
 		Color::orange(),
diff --git a/firmware/application/ui_rssi.cpp b/firmware/application/ui_rssi.cpp
index 36f73d0c..98cfed63 100644
--- a/firmware/application/ui_rssi.cpp
+++ b/firmware/application/ui_rssi.cpp
@@ -75,16 +75,13 @@ void RSSI::paint(Painter& painter) {
 		Color::black()
 	);
 	
-	if (pwmrssi_enabled) {
-		const range_t<int> pwmrssi_avg_range { 0, 96 };
-		const auto pwmrssi_avg = pwmrssi_avg_range.clip((avg_ - raw_min) * 96 / raw_delta);
-		baseband::set_pwmrssi(pwmrssi_avg, true);
-	}
+	if (pitch_rssi_enabled)
+		baseband::set_pitch_rssi((avg_ - raw_min) * 2000 / raw_delta, true);
 }
 
-void RSSI::set_pwmrssi(bool enabled) {
-	pwmrssi_enabled = enabled;
-	if (!enabled) baseband::set_pwmrssi(0, false);
+void RSSI::set_pitch_rssi(bool enabled) {
+	pitch_rssi_enabled = enabled;
+	if (!enabled) baseband::set_pitch_rssi(0, false);
 }
 
 void RSSI::on_statistics_update(const RSSIStatistics& statistics) {
diff --git a/firmware/application/ui_rssi.hpp b/firmware/application/ui_rssi.hpp
index d07f26b0..e22a61af 100644
--- a/firmware/application/ui_rssi.hpp
+++ b/firmware/application/ui_rssi.hpp
@@ -52,7 +52,7 @@ private:
 	int32_t avg_;
 	int32_t max_;
 	
-	bool pwmrssi_enabled = false;
+	bool pitch_rssi_enabled = false;
 
 	MessageHandlerRegistration message_handler_stats {
 		Message::ID::RSSIStatistics,
@@ -61,16 +61,16 @@ private:
 		}
 	};
 	
-	MessageHandlerRegistration message_handler_pwmrssi {
-		Message::ID::PWMRSSIConfigure,
+	MessageHandlerRegistration message_handler_pitch_rssi {
+		Message::ID::PitchRSSIConfigure,
 		[this](const Message* const p) {
-			const auto message = *reinterpret_cast<const PWMRSSIConfigureMessage*>(p);
-			this->set_pwmrssi(message.enabled);
+			const auto message = *reinterpret_cast<const PitchRSSIConfigureMessage*>(p);
+			this->set_pitch_rssi(message.enabled);
 		}
 	};
 
 	void on_statistics_update(const RSSIStatistics& statistics);
-	void set_pwmrssi(bool enabled);
+	void set_pitch_rssi(bool enabled);
 };
 
 }
diff --git a/firmware/baseband/proc_nfm_audio.cpp b/firmware/baseband/proc_nfm_audio.cpp
index ecd60996..48bc274f 100644
--- a/firmware/baseband/proc_nfm_audio.cpp
+++ b/firmware/baseband/proc_nfm_audio.cpp
@@ -21,6 +21,7 @@
  */
 
 #include "proc_nfm_audio.hpp"
+#include "sine_table_int8.hpp"
 #include "portapack_shared_memory.hpp"
 
 #include "event_m4.hpp"
@@ -29,7 +30,7 @@
 #include <cstddef>
 
 void NarrowbandFMAudio::execute(const buffer_c8_t& buffer) {
-	bool new_state;
+	//bool new_state;
 	
 	if( !configured ) {
 		return;
@@ -42,30 +43,63 @@ void NarrowbandFMAudio::execute(const buffer_c8_t& buffer) {
 	feed_channel_stats(channel_out);
 	channel_spectrum.feed(channel_out, channel_filter_pass_f, channel_filter_stop_f);
 
-	if (!pwmrssi_enabled) {
+	if (!pitch_rssi_enabled) {
+		// Normal mode, output demodulated audio
 		auto audio = demod.execute(channel_out, audio_buffer);
 		audio_output.write(audio);
-	} else {
-		for (c = 0; c < 32; c++) {
-			if (synth_acc < pwmrssi_avg)
-				pwmrssi_audio_buffer.p[c] = 32767;
-			else
-				pwmrssi_audio_buffer.p[c] = -32768;
-			
-			if (synth_acc < synth_div)
-				synth_acc++;
-			else
-				synth_acc = 0;
-		}
-
-		audio_output.write(pwmrssi_audio_buffer);
 		
-		new_state = audio_output.is_squelched();
+		if (ctcss_detect_enabled) {
+			/* 24kHz int16_t[16]
+			 * -> FIR filter, <300Hz pass, >300Hz stop, gain of 1
+			 * -> 12kHz int16_t[8] */
+			auto audio_ctcss = ctcss_filter.execute(audio, work_audio_buffer);
+			
+			// s16 to f32 for hpf
+			std::array<float, 8> audio_f;
+			for (size_t i = 0; i < audio_ctcss.count; i++) {
+				audio_f[i] = audio_ctcss.p[i] * ki;
+			}
+			
+			hpf.execute_in_place(buffer_f32_t {
+				audio_f.data(),
+				audio_ctcss.count,
+				audio_ctcss.sampling_rate
+				});
+			
+			// Zero-crossing detection
+			for (size_t c = 0; c < audio_ctcss.count; c++) {
+				cur_sample = audio_f[c];
+				if (cur_sample * prev_sample < 0.0) {
+					z_acc += z_timer;
+					z_timer = 0;
+					z_count++;
+				} else
+					z_timer++;
+				prev_sample = cur_sample;
+			}
+			
+			if (z_count >= 30) {
+				ctcss_message.value = (100 * 12000 / 2 * z_count) / z_acc;
+				shared_memory.application_queue.push(ctcss_message);
+				z_count = 0;
+				z_acc = 0;
+			}
+		}
+	} else {
+		// Direction-finding mode; output tone with pitch related to RSSI
+		for (size_t c = 0; c < 16; c++) {
+			tone_buffer.p[c] = (sine_table_i8[(tone_phase & 0xFF000000U) >> 24]) * 128;
+			tone_phase += tone_delta;
+		}
+		
+		audio_output.write(tone_buffer);
+		
+		/*new_state = audio_output.is_squelched();
 		
 		if (new_state && !old_state)
 			shared_memory.application_queue.push(sig_message);
 		
-		old_state = new_state;	
+		old_state = new_state;*/	
 	}
 }
 
@@ -84,8 +118,8 @@ void NarrowbandFMAudio::on_message(const Message* const message) {
 		capture_config(*reinterpret_cast<const CaptureConfigMessage*>(message));
 		break;
 	
-	case Message::ID::PWMRSSIConfigure:
-		pwmrssi_config(*reinterpret_cast<const PWMRSSIConfigureMessage*>(message));
+	case Message::ID::PitchRSSIConfigure:
+		pitch_rssi_config(*reinterpret_cast<const PitchRSSIConfigureMessage*>(message));
 		break;
 		
 	default:
@@ -114,16 +148,15 @@ void NarrowbandFMAudio::configure(const NBFMConfigureMessage& message) {
 	channel_spectrum.set_decimation_factor(std::floor(channel_filter_output_fs / (channel_filter_pass_f + channel_filter_stop_f)));
 	audio_output.configure(message.audio_hpf_config, message.audio_deemph_config, (float)message.squelch_level / 100.0);
 	
-	synth_acc = 0;
-	
+	hpf.configure(audio_24k_hpf_30hz_config);
+	ctcss_filter.configure(taps_64_lp_025_025.taps);
+
 	configured = true;
 }
 
-void NarrowbandFMAudio::pwmrssi_config(const PWMRSSIConfigureMessage& message) {
-	pwmrssi_enabled = message.enabled;
-	pwmrssi_avg = message.avg / 3;
-	synth_div = message.synth_div;
-	synth_acc = 0;
+void NarrowbandFMAudio::pitch_rssi_config(const PitchRSSIConfigureMessage& message) {
+	pitch_rssi_enabled = message.enabled;
+	tone_delta = (message.rssi + 1000) * ((1ULL << 32) / 24000);
 }
 
 void NarrowbandFMAudio::capture_config(const CaptureConfigMessage& message) {
diff --git a/firmware/baseband/proc_nfm_audio.hpp b/firmware/baseband/proc_nfm_audio.hpp
index 92d9d4f7..94fb4dc1 100644
--- a/firmware/baseband/proc_nfm_audio.hpp
+++ b/firmware/baseband/proc_nfm_audio.hpp
@@ -29,6 +29,7 @@
 
 #include "dsp_decimate.hpp"
 #include "dsp_demodulate.hpp"
+#include "dsp_iir.hpp"
 
 #include "audio_output.hpp"
 #include "spectrum_collector.hpp"
@@ -52,16 +53,21 @@ private:
 		dst.data(),
 		dst.size()
 	};
-	std::array<float, 32> audio { };
-	const buffer_f32_t audio_buffer {
-		audio.data(),
-		audio.size()
+	const buffer_s16_t work_audio_buffer {
+		(int16_t*)dst.data(),
+		sizeof(dst) / sizeof(int16_t)
 	};
 	
-	std::array<int16_t, 32> pwm { };
-	const buffer_s16_t pwmrssi_audio_buffer {
-		(int16_t*)pwm.data(),
-		sizeof(pwm) / sizeof(int16_t)
+	std::array<int16_t, 16> audio { };
+	const buffer_s16_t audio_buffer {
+		(int16_t*)audio.data(),
+		sizeof(audio) / sizeof(int16_t)
+	};
+	
+	std::array<int16_t, 16> tone { };
+	const buffer_s16_t tone_buffer {
+		(int16_t*)tone.data(),
+		sizeof(tone) / sizeof(int16_t)
 	};
 
 	dsp::decimate::FIRC8xR16x24FS4Decim8 decim_0 { };
@@ -69,25 +75,34 @@ private:
 	dsp::decimate::FIRAndDecimateComplex channel_filter { };
 	uint32_t channel_filter_pass_f = 0;
 	uint32_t channel_filter_stop_f = 0;
+	
+	// For CTCSS decoding
+	dsp::decimate::FIR64AndDecimateBy2Real ctcss_filter { };
+	IIRBiquadFilter hpf { };
 
 	dsp::demodulate::FM demod { };
 
 	AudioOutput audio_output { };
-	bool old_state { };
 
 	SpectrumCollector channel_spectrum { };
 	
-	unsigned int c { 0 }, synth_acc { 0 };
-	uint32_t synth_div { 0 };
-	bool pwmrssi_enabled = false;
-	uint32_t pwmrssi_avg { 0 };
+	uint32_t tone_phase { 0 };
+	uint32_t tone_delta { 0 };
+	bool pitch_rssi_enabled { false };
+	
+	float cur_sample { }, prev_sample { };
+	uint32_t z_acc { 0}, z_timer { 0 }, z_count { 0 };
+	bool ctcss_detect_enabled { true };
+	static constexpr float k = 32768.0f;
+	static constexpr float ki = 1.0f / k;
 
 	bool configured { false };
-	void pwmrssi_config(const PWMRSSIConfigureMessage& message);
+	void pitch_rssi_config(const PitchRSSIConfigureMessage& message);
 	void configure(const NBFMConfigureMessage& message);
 	void capture_config(const CaptureConfigMessage& message);
 	
-	RequestSignalMessage sig_message { RequestSignalMessage::Signal::Squelched };
+	//RequestSignalMessage sig_message { RequestSignalMessage::Signal::Squelched };
+	CodedSquelchMessage ctcss_message { 0 };
 };
 
 #endif/*__PROC_NFM_AUDIO_H__*/
diff --git a/firmware/baseband/proc_wfm_audio.cpp b/firmware/baseband/proc_wfm_audio.cpp
index 04270bda..89abe9d3 100644
--- a/firmware/baseband/proc_wfm_audio.cpp
+++ b/firmware/baseband/proc_wfm_audio.cpp
@@ -45,49 +45,32 @@ void WidebandFMAudio::execute(const buffer_c8_t& buffer) {
 		channel_spectrum.feed(channel, channel_filter_pass_f, channel_filter_stop_f);
 	}
 
-	if ( !pwmrssi_enabled ) {
-		/* 384kHz complex<int16_t>[256]
-		 * -> FM demodulation
-		 * -> 384kHz int16_t[256] */
-		/* TODO: To improve adjacent channel rejection, implement complex channel filter:
-		 *		pass < +/- 100kHz, stop > +/- 200kHz
-		 */
+	/* 384kHz complex<int16_t>[256]
+	 * -> FM demodulation
+	 * -> 384kHz int16_t[256] */
+	/* TODO: To improve adjacent channel rejection, implement complex channel filter:
+	 *		pass < +/- 100kHz, stop > +/- 200kHz
+	 */
 
-		auto audio_oversampled = demod.execute(channel, work_audio_buffer);
+	auto audio_oversampled = demod.execute(channel, work_audio_buffer);
 
-		/* 384kHz int16_t[256]
-		 * -> 4th order CIC decimation by 2, gain of 1
-		 * -> 192kHz int16_t[128] */
-		auto audio_4fs = audio_dec_1.execute(audio_oversampled, work_audio_buffer);
+	/* 384kHz int16_t[256]
+	 * -> 4th order CIC decimation by 2, gain of 1
+	 * -> 192kHz int16_t[128] */
+	auto audio_4fs = audio_dec_1.execute(audio_oversampled, work_audio_buffer);
 
-		/* 192kHz int16_t[128]
-		 * -> 4th order CIC decimation by 2, gain of 1
-		 * -> 96kHz int16_t[64] */
-		auto audio_2fs = audio_dec_2.execute(audio_4fs, work_audio_buffer);
+	/* 192kHz int16_t[128]
+	 * -> 4th order CIC decimation by 2, gain of 1
+	 * -> 96kHz int16_t[64] */
+	auto audio_2fs = audio_dec_2.execute(audio_4fs, work_audio_buffer);
 
-		/* 96kHz int16_t[64]
-		 * -> FIR filter, <15kHz (0.156fs) pass, >19kHz (0.198fs) stop, gain of 1
-		 * -> 48kHz int16_t[32] */
-		auto audio = audio_filter.execute(audio_2fs, work_audio_buffer);
+	/* 96kHz int16_t[64]
+	 * -> FIR filter, <15kHz (0.156fs) pass, >19kHz (0.198fs) stop, gain of 1
+	 * -> 48kHz int16_t[32] */
+	auto audio = audio_filter.execute(audio_2fs, work_audio_buffer);
 
-		/* -> 48kHz int16_t[32] */
-		audio_output.write(audio);
-	} else {
-		for (c = 0; c < 32; c++) {
-			if (synth_acc < pwmrssi_avg)
-				pwmrssi_audio_buffer.p[c] = 32767;
-			else
-				pwmrssi_audio_buffer.p[c] = -32768;
-			
-			if (synth_acc < synth_div)		// 48kHz / 96 = 500Hz
-				synth_acc++;
-			else
-				synth_acc = 0;
-		}
-
-		/* -> 48kHz int16_t[32] */
-		audio_output.write(pwmrssi_audio_buffer);
-	}
+	/* -> 48kHz int16_t[32] */
+	audio_output.write(audio);
 	
 }
 
@@ -105,10 +88,6 @@ void WidebandFMAudio::on_message(const Message* const message) {
 	case Message::ID::CaptureConfig:
 		capture_config(*reinterpret_cast<const CaptureConfigMessage*>(message));
 		break;
-	
-	case Message::ID::PWMRSSIConfigure:
-		pwmrssi_config(*reinterpret_cast<const PWMRSSIConfigureMessage*>(message));
-		break;
 		
 	default:
 		break;
@@ -137,18 +116,9 @@ void WidebandFMAudio::configure(const WFMConfigureMessage& message) {
 
 	channel_spectrum.set_decimation_factor(1);
 
-	synth_acc = 0;
-
 	configured = true;
 }
 
-void WidebandFMAudio::pwmrssi_config(const PWMRSSIConfigureMessage& message) {
-	pwmrssi_enabled = message.enabled;
-	pwmrssi_avg = message.avg;
-	synth_div = message.synth_div;
-	synth_acc = 0;
-}
-
 void WidebandFMAudio::capture_config(const CaptureConfigMessage& message) {
 	if( message.config ) {
 		audio_output.set_stream(std::make_unique<StreamInput>(message.config));
diff --git a/firmware/baseband/proc_wfm_audio.hpp b/firmware/baseband/proc_wfm_audio.hpp
index 2438720d..669410c6 100644
--- a/firmware/baseband/proc_wfm_audio.hpp
+++ b/firmware/baseband/proc_wfm_audio.hpp
@@ -55,12 +55,6 @@ private:
 		(int16_t*)dst.data(),
 		sizeof(dst) / sizeof(int16_t)
 	};
-	
-	std::array<int16_t, 32> pwm { };
-	const buffer_s16_t pwmrssi_audio_buffer {
-		(int16_t*)pwm.data(),
-		sizeof(pwm) / sizeof(int16_t)
-	};
 
 	dsp::decimate::FIRC8xR16x24FS4Decim4 decim_0 { };
 	dsp::decimate::FIRC16xR16x16Decim2 decim_1 { };
@@ -77,14 +71,8 @@ private:
 	SpectrumCollector channel_spectrum { };
 	size_t spectrum_interval_samples = 0;
 	size_t spectrum_samples = 0;
-	
-	unsigned int c { 0 }, synth_acc { 0 };
-	uint32_t synth_div { 0 };
-	bool pwmrssi_enabled { false };
-	uint32_t pwmrssi_avg { 0 };
 
 	bool configured { false };
-	void pwmrssi_config(const PWMRSSIConfigureMessage& message);
 	void configure(const WFMConfigureMessage& message);
 	void capture_config(const CaptureConfigMessage& message);
 };
diff --git a/firmware/common/dsp_fir_taps.hpp b/firmware/common/dsp_fir_taps.hpp
index 2eb13b69..1329b832 100644
--- a/firmware/common/dsp_fir_taps.hpp
+++ b/firmware/common/dsp_fir_taps.hpp
@@ -1,5 +1,6 @@
 /*
  * Copyright (C) 2015 Jared Boone, ShareBrained Technology, Inc.
+ * Copyright (C) 2017 Furrtek
  *
  * This file is part of PortaPack.
  *
@@ -115,7 +116,7 @@ constexpr fir_taps_real<32> taps_11k0_channel {
 	} },
 };
 
-/// NBFM 8K50F3E emission type ////////////////////////////////////////////
+// NBFM 8K50F3E emission type /////////////////////////////////////////////
 
 // IFIR image-reject filter: fs=3072000, pass=4250, stop=340250, decim=8, fout=384000
 constexpr fir_taps_real<24> taps_4k25_decim_0 {
@@ -152,6 +153,50 @@ constexpr fir_taps_real<32> taps_4k25_channel {
 	} },
 };
 
+/* CTCSS audio filter */
+/* 12kHz int16_t input
+ * -> FIR filter, <300Hz pass, >300Hz stop, gain of 1
+ * -> 6kHz int16_t output, gain of 1.0 (I think).
+ * Padded to multiple of four taps for unrolled FIR code.
+ * sum(abs(taps)): 125270
+ */
+/*constexpr fir_taps_real<64> taps_64_lp_025_025 {
+	.pass_frequency_normalized = 0.025f,
+	.stop_frequency_normalized = 0.025f,
+	.taps = { {
+		     0,      0,     -3,     -7,    -13,    -20,    -27,    -32,
+		   -34,    -33,    -25,    -10,     13,     47,     94,    152,
+		   223,    307,    402,    508,    622,    742,    866,    991,
+		  1113,   1229,   1336,   1430,   1510,   1571,   1614,   1635,
+		  1635,   1614,   1571,   1510,   1430,   1336,   1229,   1113,
+		   991,    866,    742,    622,    508,    402,    307,    223,
+		   152,     94,     47,     13,    -10,    -25,    -33,    -34,
+		   -32,    -27,    -20,    -13,     -7,     -3,      0,      0
+	} },
+};*/
+
+/* CTCSS audio filter */
+/* 24kHz int16_t input
+ * -> FIR filter, <300Hz pass, >300Hz stop, gain of 1
+ * -> 12kHz int16_t output, gain of 1.0 (I think).
+ * Padded to multiple of four taps for unrolled FIR code.
+ * sum(abs(taps)): 125270
+ */
+constexpr fir_taps_real<64> taps_64_lp_025_025 {
+	.pass_frequency_normalized = 0.0125f,
+	.stop_frequency_normalized = 0.0125f,
+	.taps = { {
+		0, 0, 2, 6, 12, 20, 32, 46,
+		64, 85, 110, 138, 169, 204, 241, 281,
+		323, 367, 412, 457, 502, 547, 590, 631,
+		669, 704, 735, 762, 784, 801, 812, 818,
+		818, 812, 801, 784, 762, 735, 704, 669,
+		631, 590, 547, 502, 457, 412, 367, 323,
+		281, 241, 204, 169, 138, 110, 85, 64,
+		46, 32, 20, 12, 6, 2, 0, 0
+	} },
+};
+
 // DSB AM 6K00A3E emission type ///////////////////////////////////////////
 
 // IFIR image-reject filter: fs=3072000, pass=3000, stop=339000, decim=8, fout=384000
diff --git a/firmware/common/dsp_iir_config.hpp b/firmware/common/dsp_iir_config.hpp
index 657d0197..3ccaaa49 100644
--- a/firmware/common/dsp_iir_config.hpp
+++ b/firmware/common/dsp_iir_config.hpp
@@ -1,5 +1,6 @@
 /*
  * Copyright (C) 2015 Jared Boone, ShareBrained Technology, Inc.
+ * Copyright (C) 2017 Furrtek
  *
  * This file is part of PortaPack.
  *
@@ -43,6 +44,12 @@ constexpr iir_biquad_config_t audio_24k_hpf_300hz_config {
 
 };
 
+// scipy.signal.butter(2, 30 / 12000.0, 'highpass', analog=False)
+constexpr iir_biquad_config_t audio_24k_hpf_30hz_config {
+	{  0.99446179f, -1.98892358f,  0.99446179f },
+	{  1.00000000f, -1.98889291f,  0.98895425f }
+};
+
 // scipy.signal.butter(2, 300 / 8000.0, 'highpass', analog=False)
 constexpr iir_biquad_config_t audio_16k_hpf_300hz_config {
 	{  0.92006616f, -1.84013232f,  0.92006616f },
diff --git a/firmware/common/message.hpp b/firmware/common/message.hpp
index 63b2ae21..da5dac8c 100644
--- a/firmware/common/message.hpp
+++ b/firmware/common/message.hpp
@@ -80,7 +80,7 @@ public:
 		
 		TonesConfigure = 32,
 		AFSKTxConfigure = 33,
-		PWMRSSIConfigure = 34,
+		PitchRSSIConfigure = 34,
 		OOKConfigure = 35,
 		RDSConfigure = 36,
 		AudioTXConfig = 37,
@@ -103,6 +103,7 @@ public:
 		FIFOData = 61,
 		
 		AudioLevelReport = 70,
+		CodedSquelch = 71,
 		MAX
 	};
 
@@ -351,6 +352,18 @@ public:
 	uint32_t value;
 };
 
+class CodedSquelchMessage : public Message {
+public:
+	constexpr CodedSquelchMessage(
+		const uint32_t value
+	) : Message { ID::CodedSquelch },
+		value { value }
+	{
+	}
+	
+	uint32_t value;
+};
+
 class ShutdownMessage : public Message {
 public:
 	constexpr ShutdownMessage(
@@ -667,22 +680,19 @@ public:
 	const bool trigger_word;
 };
 
-class PWMRSSIConfigureMessage : public Message {
+class PitchRSSIConfigureMessage : public Message {
 public:
-	constexpr PWMRSSIConfigureMessage(
+	constexpr PitchRSSIConfigureMessage(
 		const bool enabled,
-		const uint32_t synth_div,
-		const int32_t avg
-	) : Message { ID::PWMRSSIConfigure },
+		const int32_t rssi
+	) : Message { ID::PitchRSSIConfigure },
 		enabled(enabled),
-		synth_div(synth_div),
-		avg(avg)
+		rssi(rssi)
 	{
 	}
 	
 	const bool enabled;
-	const uint32_t synth_div;
-	const int32_t avg;
+	const int32_t rssi;
 };
 
 class TonesConfigureMessage : public Message {
diff --git a/firmware/portapack-h1-havoc.bin b/firmware/portapack-h1-havoc.bin
index dbb88114..76756ed2 100644
Binary files a/firmware/portapack-h1-havoc.bin and b/firmware/portapack-h1-havoc.bin differ
diff --git a/firmware/tools/fir_lpf.py b/firmware/tools/fir_lpf.py
new file mode 100644
index 00000000..8a78c7ba
--- /dev/null
+++ b/firmware/tools/fir_lpf.py
@@ -0,0 +1,20 @@
+import math
+
+m = 64 - 1
+ft = 300.0 / 24000.0
+
+taps = []
+window = []
+
+print("Normalized ft = " + str(ft))
+
+for n in range(0, 64):
+	taps.append(math.sin(2 * math.pi * ft * (n - (m / 2.0))) / (math.pi * (n - (m / 2.0))))
+
+for n in range(0, 64):
+	window.append(0.5 - 0.5 * math.cos(2 * math.pi * n / m))
+
+for n in range(0, 64):
+	taps[n] = int(taps[n] * window[n] * 32768)
+
+print(taps)