Merge remote-tracking branch 'upstream/master'

Conflicts: firmware/application/Makefile firmware/application/core_control.cpp firmware/application/touch.cpp firmware/application/ui_debug.cpp firmware/application/ui_debug.hpp firmware/application/ui_navigation.cpp firmware/baseband/baseband_thread.cpp
2025-12-02 19:01:48 +00:00 · 2016-04-21 20:36:19 +02:00
parent ba94ce55ef e4b86d149c
commit 1b0da68d65
252 changed files with 10752 additions and 7183 deletions
--- a/firmware/common/ais_packet.cpp
+++ b/firmware/common/ais_packet.cpp
@@ -185,7 +185,7 @@ Longitude Packet::longitude(const size_t start_bit) const {

 bool Packet::crc_ok() const {
 	CRCReader field_crc { packet_ };
-	CRC<uint16_t> ais_fcs { 0x1021, 0xffff, 0xffff };
+	CRC<16> ais_fcs { 0x1021, 0xffff, 0xffff };
 	
 	for(size_t i=0; i<data_length(); i+=8) {
 		ais_fcs.process_byte(field_crc.read(i, 8));
--- a/firmware/common/audio.hpp
+++ b/firmware/common/audio.hpp
@@ -1,108 +0,0 @@
-/*
- * Copyright (C) 2014 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 __AUDIO_H__
-#define __AUDIO_H__
-
-#include "buffer.hpp"
-
-#include "i2s.hpp"
-using namespace lpc43xx;
-
-namespace audio {
-
-struct sample_t {
-	union {
-		struct {
-			int16_t left;
-			int16_t right;
-		};
-		uint32_t raw;
-	};
-};
-
-using buffer_t = buffer_t<sample_t>;
-
-constexpr i2s::ConfigTX i2s0_config_tx {
-	.dao = i2s::DAO {
-		.wordwidth = i2s::WordWidth::Bits16,
-		.mono = 0,
-		.stop = 1,
-		.reset = 0,
-		.ws_sel = 0,
-		.ws_halfperiod = 0x0f,
-		.mute = 1,
-	},
-	.txrate = i2s::MCLKRate {
-		.x_divider = 0,
-		.y_divider = 0,
-	},
-	.txbitrate = i2s::BitRate {
-		.bitrate = 7,
-	},
-	.txmode = i2s::Mode {
-		.clksel = i2s::ClockSelect::BaseAudioClkOrExternalMCLK,
-		.four_pin = 0,
-		.mclk_out_en = 1,
-	},
-};
-
-constexpr i2s::ConfigRX i2s0_config_rx {
-	.dai = i2s::DAI {
-		.wordwidth = i2s::WordWidth::Bits16,
-		.mono = 0,
-		.stop = 1,
-		.reset = 0,
-		.ws_sel = 1,
-		.ws_halfperiod = 0x0f,
-	},
-	.rxrate = i2s::MCLKRate {
-		.x_divider = 0,
-		.y_divider = 0,
-	},
-	.rxbitrate = i2s::BitRate {
-		.bitrate = 7,
-	},
-	.rxmode = i2s::Mode {
-		.clksel = i2s::ClockSelect::BaseAudioClkOrExternalMCLK,
-		.four_pin = 1,
-		.mclk_out_en = 0,
-	},
-};
-
-constexpr i2s::ConfigDMA i2s0_config_dma {
-	.dma1 = i2s::DMA {
-		.rx_enable = 1,
-		.tx_enable = 0,
-		.rx_depth = 4,
-		.tx_depth = 0,
-	},
-	.dma2 = i2s::DMA {
-		.rx_enable = 0,
-		.tx_enable = 1,
-		.rx_depth = 0,
-		.tx_depth = 4,
-	},
-};
-
-} /* namespace audio */
-
-#endif/*__AUDIO_H__*/
--- a/firmware/common/chibios_cpp.cpp
+++ b/firmware/common/chibios_cpp.cpp
@@ -21,6 +21,8 @@

 #include "chibios_cpp.hpp"

+#include <cstdint>
+
 #include <ch.h>

 void* operator new(size_t size) {
@@ -38,3 +40,21 @@ void operator delete(void* p) noexcept {
 void operator delete[](void* p) noexcept {
 	chHeapFree(p);
 }
+
+extern uint8_t __heap_base__[];
+extern uint8_t __heap_end__[];
+
+namespace chibios {
+
+size_t heap_size() {
+	return __heap_end__ - __heap_base__;
+}
+
+size_t heap_used() {
+	const auto core_free = chCoreStatus();
+	size_t heap_free = 0;
+	chHeapStatus(NULL, &heap_free);
+	return heap_size() - (core_free + heap_free);
+}
+
+} /* namespace chibios */
--- a/firmware/common/chibios_cpp.hpp
+++ b/firmware/common/chibios_cpp.hpp
@@ -31,4 +31,11 @@ void* operator new[](size_t size);
 void operator delete(void* p);
 void operator delete[](void* p);

+namespace chibios {
+
+size_t heap_size();
+size_t heap_used();
+
+} /* namespace chibios */
+
 #endif/*__CHIBIOS_CPP_H__*/
--- a/firmware/common/cpu_clock.cpp
+++ b/firmware/common/cpu_clock.cpp
@@ -1,133 +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 "cpu_clock.hpp"
-
-#include <ch.h>
-#include <hal.h>
-
-#include "lpc43xx_cpp.hpp"
-using namespace lpc43xx;
-
-constexpr uint32_t systick_count(const uint32_t clock_source_f) {
-	return clock_source_f / CH_FREQUENCY;
-}
-
-constexpr uint32_t systick_load(const uint32_t clock_source_f) {
-	return systick_count(clock_source_f) - 1;
-}
-
-constexpr auto systick_count_irc = systick_load(clock_source_irc_f);
-constexpr auto systick_count_pll1 = systick_load(clock_source_pll1_f);
-constexpr auto systick_count_pll1_step = systick_load(clock_source_pll1_step_f);
-
-static void set_clock(LPC_CGU_BASE_CLK_Type& clk, const cgu::CLK_SEL clock_source) {
-  clk.AUTOBLOCK = 1;
-  clk.CLK_SEL = toUType(clock_source);
-}
-
-void cpu_clock_irc() {
-	/* Set M4 clock to safe default speed (~12MHz IRC) */
-	set_clock(LPC_CGU->BASE_M4_CLK, cgu::CLK_SEL::IRC);
-	systick_adjust_period(systick_count_irc);
-	halLPCSetSystemClock(clock_source_irc_f);
-}
-
-void cpu_xtal_start() {
-	LPC_CGU->XTAL_OSC_CTRL.BYPASS = 0;
-	LPC_CGU->XTAL_OSC_CTRL.HF = 0;
-	LPC_CGU->XTAL_OSC_CTRL.ENABLE = 0;
-	halPolledDelay(US2RTT(250));
-}
-
-void cpu_clock_max_speed() {
-	/* Incantation from LPC43xx UM10503 section 12.2.1.1, to bring the M4
-	 * core clock speed to the 110 - 204MHz range.
-	 */
-	cpu_clock_irc();
-
-	cpu_xtal_start();
-
-	/* Step into the 90-110MHz M4 clock range */
-	cgu::pll1::ctrl({
-		.pd = 0,
-		.bypass = 0,
-		.fbsel = 0,
-		.direct = 0,
-		.psel = 0,
-		.autoblock = 1,
-		.nsel = 0,
-		.msel = 16,
-		.clk_sel = cgu::CLK_SEL::XTAL,
-	});
-	while( !cgu::pll1::is_locked() );
-
-	/* Switch M4 clock to PLL1 running at intermediate rate */
-	set_clock(LPC_CGU->BASE_M4_CLK, cgu::CLK_SEL::PLL1);
-	systick_adjust_period(systick_count_pll1_step);
-	halLPCSetSystemClock(clock_source_pll1_step_f);
-
-	/* Delay >50us at 90-110MHz clock speed */
-	halPolledDelay(US2RTT(50));
-
-	/* Remove /2P divider from PLL1 output to achieve full speed */
-	cgu::pll1::direct();
-	systick_adjust_period(systick_count_pll1);
-	halLPCSetSystemClock(clock_source_pll1_f);
-}
-
-void cpu_start_audio_pll() {
-	cgu::pll0audio::ctrl({
-		.pd = 1,
-		.bypass = 0,
-		.directi = 0,
-		.directo = 0,
-		.clken = 0,
-		.frm = 0,
-		.autoblock = 1,
-		.pllfract_req = 1,
-		.sel_ext = 0,
-		.mod_pd = 0,
-		.clk_sel = cgu::CLK_SEL::XTAL,
-	});
-
-	/* For 12MHz clock source, 48kHz audio rate, 256Fs MCLK:
-	 * 		Fout=12.288MHz, Fcco=417.792MHz
-	 *		PDEC=3, NDEC=1, PLLFRACT=0x1a1cac
-	 */
-	cgu::pll0audio::mdiv({
-		.mdec = 0x5B6A,
-	});
-	cgu::pll0audio::np_div({
-		.pdec = 3,
-		.ndec = 1,
-	});
-
-	cgu::pll0audio::frac({
-		.pllfract_ctrl = 0x1a1cac,
-	});
-
-	cgu::pll0audio::power_up();
-	while( !cgu::pll0audio::is_locked() );
-	cgu::pll0audio::clock_enable();
-
-	set_clock(LPC_CGU->BASE_AUDIO_CLK, cgu::CLK_SEL::PLL0AUDIO);
-}
--- a/firmware/common/crc.hpp
+++ b/firmware/common/crc.hpp
@@ -24,21 +24,31 @@

 #include <cstddef>
 #include <cstdint>
+#include <limits>
+#include <array>

 /* Inspired by
 * http://www.barrgroup.com/Embedded-Systems/How-To/CRC-Calculation-C-Code
 *
- * ...then munged into a shape resembling boost::crc_basic.
+ * ...then munged into a simplified implementation of boost::crc_basic and
+ * boost::crc_optimal.
 * http://www.boost.org/doc/libs/release/libs/crc/
+ *
+ *  Copyright 2001, 2004 Daryle Walker.  Use, modification, and distribution are
+ *  subject to the Boost Software License, Version 1.0.  (See accompanying file
+ *  LICENSE_1_0.txt or a copy at <http://www.boost.org/LICENSE_1_0.txt>.)
+ *
 */

-template<typename T>
+template<size_t Width, bool RevIn = false, bool RevOut = false>
 class CRC {
 public:
+	using value_type = uint32_t;
+
 	constexpr CRC(
-		const T truncated_polynomial,
-		const T initial_remainder = 0,
-		const T final_xor_value = 0
+		const value_type truncated_polynomial,
+		const value_type initial_remainder = 0,
+		const value_type final_xor_value = 0
 	) : truncated_polynomial { truncated_polynomial },
 		initial_remainder { initial_remainder },
 		final_xor_value { final_xor_value },
@@ -46,11 +56,11 @@ public:
 	{
 	}

-	T get_initial_remainder() const {
+	value_type get_initial_remainder() const {
 		return initial_remainder;
 	}

-	void reset(T new_initial_remainder) {
+	void reset(value_type new_initial_remainder) {
 		remainder = new_initial_remainder;
 	}

@@ -59,49 +69,123 @@ public:
 	}

 	void process_bit(bool bit) {
-		remainder ^= bit << (width() - 1);
-		if( remainder & top_bit() ) {
-			remainder = (remainder << 1) ^ truncated_polynomial;
+		remainder ^= (bit ? top_bit() : 0U);
+		const auto do_poly_div = static_cast<bool>(remainder & top_bit());
+		remainder <<= 1;
+		if( do_poly_div ) {
+			remainder ^= truncated_polynomial;
+		}
+	}
+
+	void process_bits(value_type bits, size_t bit_count) {
+		constexpr auto digits = std::numeric_limits<value_type>::digits;
+		constexpr auto mask = static_cast<value_type>(1) << (digits - 1);
+
+		bits <<= (std::numeric_limits<value_type>::digits - bit_count);
+		for(size_t i=bit_count; i>0; --i, bits <<= 1) {
+			process_bit(static_cast<bool>(bits & mask));
+		}
+	}
+
+	void process_bits_lsb_first(value_type bits, size_t bit_count) {
+		for(size_t i=bit_count; i>0; --i, bits >>= 1) {
+			process_bit(static_cast<bool>(bits & 0x01));
+		}
+	}
+
+	void process_byte(const uint8_t byte) {
+		if( RevIn ) {
+			process_bits_lsb_first(byte, 8);
 		} else {
-			remainder = (remainder << 1);
+			process_bits(byte, 8);
 		}
 	}

-	void process_byte(const uint8_t data) {
-		remainder ^= data << (width() - 8);
-		for(size_t bit=0; bit<8; bit++) {
-			if( remainder & top_bit() ) {
-				remainder = (remainder << 1) ^ truncated_polynomial;
-			} else {
-				remainder = (remainder << 1);
-			}
-		}
-	}
-
-	void process_bytes(const uint8_t* const data, const size_t length) {
+	void process_bytes(const void* const data, const size_t length) {
+		const uint8_t* const p = reinterpret_cast<const uint8_t*>(data);
 		for(size_t i=0; i<length; i++) {
-			process_byte(data[i]);
+			process_byte(p[i]);
 		}
 	}

-	T checksum() const {
-		return remainder ^ final_xor_value;
+	template<size_t N>
+	void process_bytes(const std::array<uint8_t, N>& data) {
+		process_bytes(data.data(), data.size());
+	}
+
+	value_type checksum() const {
+		return ((RevOut ? reflect(remainder) : remainder) ^ final_xor_value) & mask();
 	}

 private:
-	const T truncated_polynomial;
-	const T initial_remainder;
-	const T final_xor_value;
-	T remainder;
+	const value_type truncated_polynomial;
+	const value_type initial_remainder;
+	const value_type final_xor_value;
+	value_type remainder;

 	static constexpr size_t width() {
-		return 8 * sizeof(T);
+		return Width;
 	}

-	static constexpr T top_bit() {
+	static constexpr value_type top_bit() {
 		return 1U << (width() - 1);
 	}
+
+	static constexpr value_type mask() {
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wshift-count-overflow"
+		return (~(~(0UL) << width()));
+#pragma GCC diagnostic pop
+	}
+
+	static value_type reflect(value_type x) {
+		value_type reflection = 0;
+		for(size_t i=0; i<width(); ++i) {
+			reflection <<= 1;
+			reflection |= (x & 1);
+			x >>= 1;
+		}
+		return reflection;
+	}
 };

+class Adler32 {
+public:
+	void feed(const uint8_t v) {
+		feed_one(v);
+	}
+
+	void feed(const void* const data, const size_t n) {
+		const uint8_t* const p = reinterpret_cast<const uint8_t*>(data);
+		for(size_t i=0; i<n; i++) {
+			feed_one(p[i]);
+		}
+	}
+
+	template<typename T>
+	void feed(const T& a) {
+		feed(a.data(), sizeof(T));
+	}
+
+	std::array<uint8_t, 4> bytes() const {
+		return {
+			static_cast<uint8_t>((b >> 8) & 0xff),
+			static_cast<uint8_t>((b >> 0) & 0xff),
+			static_cast<uint8_t>((a >> 8) & 0xff),
+			static_cast<uint8_t>((a >> 0) & 0xff)
+		};
+	}
+
+private:
+	static constexpr uint32_t mod = 65521;
+
+	uint32_t a { 1 };
+	uint32_t b { 0 };
+
+	void feed_one(const uint8_t c) {
+		a = (a + c) % mod;
+		b = (b + a) % mod;
+	}
+};

 #endif/*__CRC_H__*/
--- a/firmware/common/debug.cpp
+++ b/firmware/common/debug.cpp
@@ -81,18 +81,27 @@ void port_halt(void) {

 #if defined(LPC43XX_M4)
 CH_IRQ_HANDLER(MemManageVector) {
-	CH_IRQ_PROLOGUE();
+#if CH_DBG_ENABLED
+	chDbgPanic("MemManage");
+#else
 	chSysHalt();
+#endif
 }

 CH_IRQ_HANDLER(BusFaultVector) {
-	CH_IRQ_PROLOGUE();
+#if CH_DBG_ENABLED
+	chDbgPanic("BusFault");
+#else
 	chSysHalt();
+#endif
 }

 CH_IRQ_HANDLER(UsageFaultVector) {
-	CH_IRQ_PROLOGUE();
+#if CH_DBG_ENABLED
+	chDbgPanic("UsageFault");
+#else
 	chSysHalt();
+#endif
 }
 #endif

--- a/firmware/common/dsp_fft.hpp
+++ b/firmware/common/dsp_fft.hpp
@@ -32,6 +32,7 @@
 #include "dsp_types.hpp"
 #include "complex.hpp"
 #include "hal.h"
+#include "utility.hpp"

 namespace std {
 	/* https://github.com/AE9RB/fftbench/blob/master/cxlr.hpp
@@ -47,14 +48,6 @@ namespace std {
 	}
 } /* namespace std */

-constexpr bool power_of_two(const size_t n) {
-	return (n & (n - 1)) == 0;
-}
-
-constexpr size_t log_2(const size_t n, const size_t p = 0) {
-	return (n <= 1) ? p : log_2(n / 2, p + 1);
-}
-
 template<typename T, size_t N>
 void fft_swap(const buffer_c16_t src, std::array<T, N>& dst) {
 	static_assert(power_of_two(N), "only defined for N == power of two");
--- a/firmware/common/dsp_fir_taps.hpp
+++ b/firmware/common/dsp_fir_taps.hpp
@@ -313,4 +313,27 @@ constexpr fir_taps_real<64> taps_64_lp_156_198 {
 	} },
 };

+// TPMS decimation filters ////////////////////////////////////////////////
+
+// IFIR image-reject filter: fs=2457600, pass=100000, stop=407200, decim=4, fout=614400
+static constexpr fir_taps_real<24> taps_200k_decim_0 = {
+	.pass_frequency_normalized = 100000.0f / 2457600.0f,
+	.stop_frequency_normalized = 407200.0f / 2457600.0f,
+	.taps = { {
+	    90,     94,      4,   -240,   -570,   -776,   -563,    309,
+	  1861,   3808,   5618,   6710,   6710,   5618,   3808,   1861,
+	   309,   -563,   -776,   -570,   -240,      4,     94,     90,
+	} },
+};
+
+// IFIR prototype filter: fs=614400, pass=100000, stop=207200, decim=2, fout=307200
+static constexpr fir_taps_real<16> taps_200k_decim_1 = {
+	.pass_frequency_normalized = 100000.0f / 614400.0f,
+	.stop_frequency_normalized = 207200.0f / 614400.0f,
+	.taps = { {
+		  -132,   -256,    545,    834,  -1507,  -2401,   4666,  14583,
+		 14583,   4666,  -2401,  -1507,    834,    545,   -256,   -132,
+	} },
+};
+
 #endif/*__DSP_FIR_TAPS_H__*/
--- a/firmware/common/ert_packet.cpp
+++ b/firmware/common/ert_packet.cpp
@@ -63,6 +63,16 @@ Consumption Packet::consumption() const {
 	return invalid_consumption;
 }

+CommodityType Packet::commodity_type() const {
+	if( type() == Type::SCM ) {
+		return reader_.read(5, 4);
+	}
+	if( type() == Type::IDM ) {
+		return reader_.read(4 * 8 + 4, 4);
+	}
+	return invalid_commodity_type;
+}
+
 ManchesterFormatted Packet::symbols_formatted() const {
 	return format_manchester(decoder_);
 }
@@ -76,7 +86,7 @@ bool Packet::crc_ok() const {
 }

 bool Packet::crc_ok_scm() const {
-	CRC<uint16_t> ert_bch { 0x6f63 };
+	CRC<16> ert_bch { 0x6f63 };
 	size_t start_bit = 5;
 	ert_bch.process_byte(reader_.read(0, start_bit));
 	for(size_t i=start_bit; i<length(); i+=8) {
@@ -86,7 +96,7 @@ bool Packet::crc_ok_scm() const {
 }

 bool Packet::crc_ok_idm() const {
-	CRC<uint16_t> ert_crc_ccitt { 0x1021, 0xffff, 0x1d0f };
+	CRC<16> ert_crc_ccitt { 0x1021, 0xffff, 0x1d0f };
 	for(size_t i=0; i<length(); i+=8) {
 		ert_crc_ccitt.process_byte(reader_.read(i, 8));
 	}
--- a/firmware/common/ert_packet.hpp
+++ b/firmware/common/ert_packet.hpp
@@ -33,6 +33,11 @@ namespace ert {

 using ID = uint32_t;
 using Consumption = uint32_t;
+using CommodityType = uint32_t;
+
+constexpr ID invalid_id = 0;
+constexpr CommodityType invalid_commodity_type = -1;
+constexpr Consumption invalid_consumption = 0;

 class Packet {
 public:
@@ -60,6 +65,7 @@ public:

 	Type type() const;
 	ID id() const;
+	CommodityType commodity_type() const;
 	Consumption consumption() const;

 	ManchesterFormatted symbols_formatted() const;
@@ -74,9 +80,6 @@ private:
 	const Reader reader_;
 	const Type type_;

-	const ID invalid_id = 0;
-	const Consumption invalid_consumption = 0;
-
 	bool crc_ok_idm() const;
 	bool crc_ok_scm() const;
 };
--- a/firmware/common/fifo.hpp
+++ b/firmware/common/fifo.hpp
@@ -22,18 +22,24 @@
 #ifndef __FIFO_H__
 #define __FIFO_H__

+#include <cstddef>
 #include <algorithm>
 #include <cstring>
+#include <memory>

 #include <hal.h>

 /* FIFO implementation inspired by Linux kfifo. */

-template<typename T, size_t K>
+template<typename T>
 class FIFO {
 public:
 	constexpr FIFO(
-	) : _in { 0 },
+		T* data,
+		size_t k
+	) : _data { data },
+		_size { 1U << k },
+		_in { 0 },
 		_out { 0 }
 	{
 	}
@@ -150,15 +156,15 @@ public:
 	}

 private:
-	static constexpr size_t size() {
-		return (1UL << K);
+	size_t size() const {
+		return _size;
 	}

 	static constexpr size_t esize() {
 		return sizeof(T);
 	}

-	static constexpr size_t mask() {
+	size_t mask() const {
 		return size() - 1;
 	}

@@ -224,7 +230,8 @@ private:
 		return buf_len;
 	}

-	T _data[size()];
+	T* const _data;
+	const size_t _size;
 	volatile size_t _in;
 	volatile size_t _out;
 };
--- a/firmware/common/lcd_ili9341.cpp
+++ b/firmware/common/lcd_ili9341.cpp
@@ -181,6 +181,11 @@ void lcd_ramwr_start() {
 	io.lcd_data_write_command_and_data(0x2c, {});
 }

+void lcd_ramrd_start() {
+	io.lcd_data_write_command_and_data(0x2e, {});
+	io.lcd_read_word();
+}
+
 void lcd_caset(const uint_fast16_t start_column, uint_fast16_t end_column) {
 	lcd_set(0x2a, start_column, end_column);
 }
@@ -198,12 +203,27 @@ void lcd_start_ram_write(
 	lcd_ramwr_start();
 }

+void lcd_start_ram_read(
+	const ui::Point p,
+	const ui::Size s
+) {
+	lcd_caset(p.x, p.x + s.w - 1);
+	lcd_paset(p.y, p.y + s.h - 1);
+	lcd_ramrd_start();
+}
+
 void lcd_start_ram_write(
 	const ui::Rect& r
 ) {
 	lcd_start_ram_write(r.pos, r.size);
 }

+void lcd_start_ram_read(
+	const ui::Rect& r
+) {
+	lcd_start_ram_read(r.pos, r.size);
+}
+
 void lcd_vertical_scrolling_definition(
 	const uint_fast16_t top_fixed_area,
 	const uint_fast16_t vertical_scrolling_area,
@@ -394,6 +414,19 @@ void ILI9341::draw_pixels(
 	io.lcd_write_pixels(colors, count);
 }

+void ILI9341::read_pixels(
+	const ui::Rect r,
+	ui::ColorRGB888* const colors,
+	const size_t count
+) {
+	/* TODO: Assert that rectangle width x height < count */
+	lcd_start_ram_read(r);
+	io.lcd_read_bytes(
+		reinterpret_cast<uint8_t*>(colors),
+		count * sizeof(ui::ColorRGB888)
+	);
+}
+
 void ILI9341::draw_bitmap(
 	const ui::Point p,
 	const ui::Size size,
--- a/firmware/common/lcd_ili9341.hpp
+++ b/firmware/common/lcd_ili9341.hpp
@@ -69,6 +69,14 @@ public:
 		draw_pixels(r, colors.data(), colors.size());
 	}

+	template<size_t N>
+	void read_pixels(
+		const ui::Rect r,
+		std::array<ui::ColorRGB888, N>& colors
+	) {
+		read_pixels(r, colors.data(), colors.size());
+	}
+
 	void draw_bitmap(
 		const ui::Point p,
 		const ui::Size size,
@@ -105,6 +113,7 @@ private:
 	scroll_t scroll_state;

 	void draw_pixels(const ui::Rect r, const ui::Color* const colors, const size_t count);
+	void read_pixels(const ui::Rect r, ui::ColorRGB888* const colors, const size_t count);
 };

 } /* namespace lcd */
--- a/firmware/common/lfsr_random.cpp
+++ b/firmware/common/lfsr_random.cpp
@@ -0,0 +1,106 @@
+/*
+ * 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 "lfsr_random.hpp"
+
+static void lfsr_iterate_internal(lfsr_word_t& v)
+{
+	/*
+	    Generated with lfsr-generator:
+	    http://lfsr-generator.sourceforge.net
+	    =============================================
+		config          : fibonacci
+		length          : 31
+		taps            : (31, 18)
+		shift-amounts   : (12, 12, 8)
+		shift-direction : left
+	*/
+	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)
+			)
+		)
+	);
+}
+
+lfsr_word_t lfsr_iterate(lfsr_word_t v) {
+	lfsr_iterate_internal(v);
+	return v;
+}
+
+void lfsr_fill(lfsr_word_t& v, lfsr_word_t* buffer, size_t word_count) {
+	while( word_count != 0 ) {
+		lfsr_iterate_internal(v);
+		*(buffer++) = v;
+		word_count--;
+	}
+}
+
+bool lfsr_compare(lfsr_word_t& v, const lfsr_word_t* buffer, size_t word_count) {
+	while( word_count != 0 ) {
+		lfsr_iterate_internal(v);
+		if( *(buffer++) != v ) {
+			return false;
+		}
+		word_count--;
+	}
+	return true;
+}
--- a/firmware/common/lfsr_random.hpp
+++ b/firmware/common/lfsr_random.hpp
@@ -19,17 +19,16 @@
 * Boston, MA 02110-1301, USA.
 */

-#ifndef __CPU_CLOCK_H__
-#define __CPU_CLOCK_H__
+#ifndef __LFSR_RANDOM_HPP__
+#define __LFSR_RANDOM_HPP__

 #include <cstdint>
+#include <cstddef>

-constexpr uint32_t clock_source_irc_f		=  12000000;
-constexpr uint32_t clock_source_gp_clkin	=  20000000;
-constexpr uint32_t clock_source_pll1_step_f	= 100000000;
-constexpr uint32_t clock_source_pll1_f		= 200000000;
+using lfsr_word_t = uint32_t;

-void cpu_clock_max_speed();
-void cpu_start_audio_pll();
+lfsr_word_t lfsr_iterate(lfsr_word_t v);
+void lfsr_fill(lfsr_word_t& v, lfsr_word_t* buffer, size_t word_count);
+bool lfsr_compare(lfsr_word_t& v, const lfsr_word_t* buffer, size_t word_count);

-#endif/*__CPU_CLOCK_H__*/
+#endif/*__LFSR_RANDOM_HPP__*/
--- a/firmware/common/memory_map.hpp
+++ b/firmware/common/memory_map.hpp
@@ -69,6 +69,8 @@ constexpr region_t ahb_ram_0		{ 0x20000000,  32_KiB };
 constexpr region_t ahb_ram_1		{ 0x20008000,  16_KiB };
 constexpr region_t ahb_ram_2		{ 0x2000c000,  16_KiB };

+constexpr region_t backup_ram		{ LPC_BACKUP_REG_BASE,        256   };
+
 constexpr region_t spifi_uncached	{ LPC_SPIFI_DATA_BASE,        1_MiB };
 constexpr region_t spifi_cached		{ LPC_SPIFI_DATA_CACHED_BASE, spifi_uncached.size() };

--- a/firmware/common/message.hpp
+++ b/firmware/common/message.hpp
@@ -29,6 +29,7 @@

 #include "baseband_packet.hpp"
 #include "ert_packet.hpp"
+#include "tpms_packet.hpp"
 #include "dsp_fir_taps.hpp"
 #include "dsp_iir.hpp"
 #include "fifo.hpp"
@@ -251,10 +252,12 @@ struct ChannelSpectrum {
 	uint32_t channel_filter_stop_frequency { 0 };
 };

-using ChannelSpectrumFIFO = FIFO<ChannelSpectrum, 2>;
+using ChannelSpectrumFIFO = FIFO<ChannelSpectrum>;

 class ChannelSpectrumConfigMessage : public Message {
 public:
+	static constexpr size_t fifo_k = 2;
+	
 	constexpr ChannelSpectrumConfigMessage(
 		ChannelSpectrumFIFO* fifo
 	) : Message { ID::ChannelSpectrumConfig },
@@ -280,12 +283,15 @@ public:
 class TPMSPacketMessage : public Message {
 public:
 	constexpr TPMSPacketMessage(
+		const tpms::SignalType signal_type,
 		const baseband::Packet& packet
 	) : Message { ID::TPMSPacket },
+		signal_type { signal_type },
 		packet { packet }
 	{
 	}

+	tpms::SignalType signal_type;
 	baseband::Packet packet;
 };

--- a/firmware/common/message_queue.hpp
+++ b/firmware/common/message_queue.hpp
@@ -32,10 +32,17 @@ using namespace lpc43xx;

 #include <ch.h>

-template<size_t K>
 class MessageQueue {
 public:
-	MessageQueue() {
+	MessageQueue() = delete;
+	MessageQueue(const MessageQueue&) = delete;
+	MessageQueue(MessageQueue&&) = delete;
+	
+	MessageQueue(
+		uint8_t* const data,
+		size_t k
+	) : fifo { data, k }
+	{
 		chMtxInit(&mutex_write);
 	}

@@ -57,6 +64,19 @@ public:
 		return result;
 	}

+	template<typename HandlerFn>
+	void handle(HandlerFn handler) {
+		std::array<uint8_t, Message::MAX_SIZE> message_buffer;
+		while(Message* const message = peek(message_buffer)) {
+			handler(message);
+			skip();
+		}
+	}
+
+private:
+	FIFO<uint8_t> fifo;
+	Mutex mutex_write;
+
 	Message* peek(std::array<uint8_t, Message::MAX_SIZE>& buf) {
 		Message* const p = reinterpret_cast<Message*>(buf.data());
 		return fifo.peek_r(buf.data(), buf.size()) ? p : nullptr;
@@ -79,10 +99,6 @@ public:
 		return fifo.is_empty();
 	}

-private:
-	FIFO<uint8_t, K> fifo;
-	Mutex mutex_write;
-
 	bool push(const void* const buf, const size_t len) {
 		chMtxLock(&mutex_write);
 		const auto result = fifo.in_r(buf, len);
--- a/firmware/common/png_writer.cpp
+++ b/firmware/common/png_writer.cpp
@@ -0,0 +1,127 @@
+/*
+ * Copyright (C) 2016 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 "png_writer.hpp"
+
+static constexpr std::array<uint8_t, 8> png_file_header { {
+	0x89, 0x50, 0x4e, 0x47,
+	0x0d, 0x0a, 0x1a, 0x0a,
+} };
+
+static constexpr std::array<uint8_t, 25> png_ihdr_screen_capture { {
+	0x00, 0x00, 0x00, 0x0d,		// IHDR length
+	0x49, 0x48, 0x44, 0x52,		// IHDR type
+	0x00, 0x00, 0x00, 0xf0,		// width = 240
+	0x00, 0x00, 0x01, 0x40,		// height = 320
+	0x08,						// bit_depth = 8
+	0x02,						// color_type = 2
+	0x00,						// compression_method = 0
+	0x00,						// filter_method = 0
+	0x00,						// interlace_method = 0
+	0x0d, 0x8a, 0x66, 0x04,		// CRC
+} };
+
+static constexpr std::array<uint8_t, 4> png_idat_chunk_type { {
+	0x49, 0x44, 0x41, 0x54,		// IDAT type
+} };
+
+static constexpr std::array<uint8_t, 12> png_iend { {
+	0x00, 0x00, 0x00, 0x00,		// IEND length
+	0x49, 0x45, 0x4e, 0x44,		// IEND type
+	0xae, 0x42, 0x60, 0x82,		// CRC
+} };
+
+PNGWriter::PNGWriter(
+	const std::string& filename
+)
+{
+	file.open_for_writing(filename);
+	file.write(png_file_header);
+	file.write(png_ihdr_screen_capture);
+	
+	write_chunk_header(
+		2 + height * (5 + 1 + width * 3) + 4,
+		png_idat_chunk_type
+	);
+
+	constexpr std::array<uint8_t, 2> zlib_header { 0x78, 0x01 };	// Zlib CM, CINFO, FLG.
+	write_chunk_content(zlib_header);
+}
+
+PNGWriter::~PNGWriter() {
+	write_chunk_content(adler_32.bytes());
+	write_chunk_crc();
+
+	file.write(png_iend);
+}
+
+void PNGWriter::write_scanline(const std::array<ui::ColorRGB888, 240>& scanline) {
+	constexpr uint8_t scanline_filter_type = 0;
+	constexpr uint32_t deflate_block_length = 1 + sizeof(scanline);
+
+	const std::array<uint8_t, 6> deflate_and_scanline_header {
+		static_cast<uint8_t>((scanline_count == (height - 1)) ? 0x01 : 0x00),	// DEFLATE header bits, bfinal=0, btype=00
+		static_cast<uint8_t>((deflate_block_length >> 0) & 0xff),			// Length LSB
+		static_cast<uint8_t>((deflate_block_length >> 8) & 0xff),			// Length MSB
+		static_cast<uint8_t>((deflate_block_length >> 0) & 0xff) ^ 0xff,	// ~Length LSB
+		static_cast<uint8_t>((deflate_block_length >> 8) & 0xff) ^ 0xff,	// ~Length MSB
+		scanline_filter_type,
+	};
+	write_chunk_content(deflate_and_scanline_header);
+
+	adler_32.feed(scanline_filter_type);
+	adler_32.feed(scanline);
+
+	// Small writes to avoid some sort of large-transfer plus block
+	// boundary FatFs or SDC driver bug?
+	write_chunk_content(&scanline[  0], 80 * sizeof(ui::ColorRGB888));
+	write_chunk_content(&scanline[ 80], 80 * sizeof(ui::ColorRGB888));
+	write_chunk_content(&scanline[160], 80 * sizeof(ui::ColorRGB888));
+
+	scanline_count++;
+}
+
+void PNGWriter::write_chunk_header(
+	const size_t length,
+	const std::array<uint8_t, 4>& type
+) {
+	write_uint32_be(length);
+	crc.reset();
+	write_chunk_content(type);
+}
+
+void PNGWriter::write_chunk_content(const void* const p, const size_t count) {
+	file.write(p, count);
+	crc.process_bytes(p, count);
+}
+
+void PNGWriter::write_chunk_crc() {
+	write_uint32_be(crc.checksum());
+}
+
+void PNGWriter::write_uint32_be(const uint32_t v) {
+	file.write(std::array<uint8_t, 4> { {
+		static_cast<uint8_t>((v >> 24) & 0xff),
+		static_cast<uint8_t>((v >> 16) & 0xff),
+		static_cast<uint8_t>((v >>  8) & 0xff),
+		static_cast<uint8_t>((v >>  0) & 0xff),
+	} });
+}
--- a/firmware/common/png_writer.hpp
+++ b/firmware/common/png_writer.hpp
@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) 2016 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 __PNG_WRITER_H__
+#define __PNG_WRITER_H__
+
+#include <cstdint>
+#include <cstddef>
+#include <string>
+#include <array>
+
+#include "ui.hpp"
+#include "file.hpp"
+#include "crc.hpp"
+
+class PNGWriter {
+public:
+	explicit PNGWriter(const std::string& filename);
+	~PNGWriter();
+
+	void write_scanline(const std::array<ui::ColorRGB888, 240>& scanline);
+
+private:
+	// TODO: These constants are baked in a few places, do not change blithely.
+	static constexpr int width { 240 };
+	static constexpr int height { 320 };
+
+	File file;
+	int scanline_count { 0 };
+	CRC<32, true, true> crc { 0x04c11db7, 0xffffffff, 0xffffffff };
+	Adler32 adler_32;
+
+	void write_chunk_header(const size_t length, const std::array<uint8_t, 4>& type);
+	void write_chunk_content(const void* const p, const size_t count);
+
+	template<size_t N>
+	void write_chunk_content(const std::array<uint8_t, N>& data) {
+		write_chunk_content(data.data(), sizeof(data));
+	}
+
+	void write_chunk_crc();
+	void write_uint32_be(const uint32_t v);
+};
+
+#endif/*__PNG_WRITER_H__*/
--- a/firmware/common/portapack_hal.hpp
+++ b/firmware/common/portapack_hal.hpp
@@ -56,10 +56,6 @@ constexpr GPIO gpio_cpld_tdo = gpio[GPIO1_8];	// P1_5
 constexpr GPIO gpio_cpld_tck = gpio[GPIO3_0];	// P6_1
 constexpr GPIO gpio_cpld_tdi = gpio[GPIO3_1];	// P6_2

-/* I2C0 */
-
-constexpr uint8_t wm8731_i2c_address = 0x1a;
-
 } /* namespace portapack */

 #endif/*__PORTAPACK_HAL_H__*/
--- a/firmware/common/portapack_io.hpp
+++ b/firmware/common/portapack_io.hpp
@@ -139,13 +139,7 @@ public:
 	uint32_t lcd_read_word() {
 		return lcd_read_data_frame_memory();
 	}
-/*
-	// NOTE: Pixels read in RGB24 format, not the RGB565 format used
-	// to write pixels to frame memory. This makes reading very tricky!
-	ui::Color lcd_read_pixel() {
-		return ui::Color { lcd_read_data_frame_memory() };
-	}
-*/
+
 	void lcd_write_pixels(const ui::Color pixel, size_t n) {
 		while(n--) {
 			lcd_write_data_fast(pixel.v);
@@ -158,6 +152,20 @@ public:
 		}
 	}

+	void lcd_read_bytes(uint8_t* byte, size_t byte_count) {
+		size_t word_count = byte_count / 2;
+		while(word_count) {
+			const auto word = lcd_read_data_frame_memory();
+			*(byte++) = word >> 8;
+			*(byte++) = word >> 0;
+			word_count--;
+		}
+		if( byte_count & 1 ) {
+			const auto word = lcd_read_data_frame_memory();
+			*(byte++) = word >> 8;
+		}
+	}
+
 	uint32_t io_read() {
 		io_stb_assert();
 		dir_read();
--- a/firmware/common/portapack_persistent_memory.cpp
+++ b/firmware/common/portapack_persistent_memory.cpp
@@ -21,10 +21,15 @@

 #include "portapack_persistent_memory.hpp"

+#include "portapack.hpp"
+
 #include "hal.h"

 #include "utility.hpp"

+#include "memory_map.hpp"
+using portapack::memory::map::backup_ram;
+
 #include <algorithm>
 #include <utility>

@@ -70,9 +75,9 @@ struct data_t {
 	int32_t ui_config;
 };

-static_assert(sizeof(data_t) <= 0x100, "Persistent memory structure too large for VBAT-maintained region");
+static_assert(sizeof(data_t) <= backup_ram.size(), "Persistent memory structure too large for VBAT-maintained region");

-static data_t* const data = reinterpret_cast<data_t*>(LPC_BACKUP_REG_BASE);
+static data_t* const data = reinterpret_cast<data_t*>(backup_ram.base());

 rf::Frequency tuned_frequency() {
 	rf::tuning_range.reset_if_outside(data->tuned_frequency, tuned_frequency_reset_value);
@@ -89,7 +94,9 @@ ppb_t correction_ppb() {
 }

 void set_correction_ppb(const ppb_t new_value) {
-	data->correction_ppb = ppb_range.clip(new_value);
+	const auto clipped_value = ppb_range.clip(new_value);
+	data->correction_ppb = clipped_value;
+	portapack::clock_manager.set_reference_ppb(clipped_value);
 }

 int32_t afsk_mark_freq() {
--- a/firmware/common/portapack_shared_memory.hpp
+++ b/firmware/common/portapack_shared_memory.hpp
@@ -23,6 +23,7 @@
 #define __PORTAPACK_SHARED_MEMORY_H__

 #include <cstdint>
+#include <cstddef>

 #include "message_queue.hpp"

@@ -39,8 +40,14 @@ struct JammerRange {

 /* NOTE: These structures must be located in the same location in both M4 and M0 binaries */
 struct SharedMemory {
-	MessageQueue<12> baseband_queue;
-	MessageQueue<11> application_queue;
+	static constexpr size_t baseband_queue_k = 12;
+	static constexpr size_t application_queue_k = 11;
+
+	MessageQueue baseband_queue;
+	uint8_t baseband_queue_data[1 << baseband_queue_k];
+	MessageQueue application_queue;
+	uint8_t application_queue_data[1 << application_queue_k];
+	void* FIFO_HACK;

 	// TODO: M0 should directly configure and control DMA channel that is
 	// acquiring ADC samples.
@@ -66,11 +73,4 @@ struct SharedMemory {

 extern SharedMemory& shared_memory;

-#if defined(LPC43XX_M0)
-inline void init_message_queues() {
-	new (&shared_memory.baseband_queue) MessageQueue<12>();
-	new (&shared_memory.application_queue) MessageQueue<11>();
-}
-#endif
-
 #endif/*__PORTAPACK_SHARED_MEMORY_H__*/
--- a/firmware/common/thread_wait.cpp
+++ b/firmware/common/thread_wait.cpp
@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) 2016 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 "thread_wait.hpp"
+
+int ThreadWait::sleep() {
+	chSysLock();
+	thread_to_wake = chThdSelf();
+	chSchGoSleepS(THD_STATE_SUSPENDED);
+	const auto result = chThdSelf()->p_u.rdymsg;
+	chSysUnlock();
+	return result;
+}
+
+bool ThreadWait::wake_from_interrupt(const int value) {
+	if( thread_to_wake ) {
+		thread_to_wake->p_u.rdymsg = value;
+		chSchReadyI(thread_to_wake);
+		thread_to_wake = nullptr;
+		return true;
+	} else {
+		return false;
+	}
+}
--- a/firmware/common/thread_wait.hpp
+++ b/firmware/common/thread_wait.hpp
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2014 Jared Boone, ShareBrained Technology, Inc.
+ * Copyright (C) 2016 Jared Boone, ShareBrained Technology, Inc.
 *
 * This file is part of PortaPack.
 *
@@ -19,8 +19,18 @@
 * Boston, MA 02110-1301, USA.
 */

-#include "audio.hpp"
+#ifndef __THREAD_WAIT_H__
+#define __THREAD_WAIT_H__

-namespace audio {
+#include <ch.h>

-} /* namespace audio */
+class ThreadWait {
+public:
+	int sleep();
+	bool wake_from_interrupt(const int value);
+
+private:
+	Thread* thread_to_wake { nullptr };
+};
+
+#endif/*__THREAD_WAIT_H__*/
--- a/firmware/common/tpms_packet.cpp
+++ b/firmware/common/tpms_packet.cpp
@@ -0,0 +1,129 @@
+/*
+ * 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 "tpms_packet.hpp"
+
+#include "crc.hpp"
+
+namespace tpms {
+
+Timestamp Packet::received_at() const {
+	return packet_.timestamp();
+}
+
+ManchesterFormatted Packet::symbols_formatted() const {
+	return format_manchester(decoder_);
+}
+
+Optional<Reading> Packet::reading(const SignalType signal_type) const {
+	if( signal_type == SignalType::FLM ) {
+		const auto length = crc_valid_length();
+
+		switch(length) {
+		case 64:
+			return Reading {
+				Reading::Type::FLM_64,
+				reader_.read(0, 32),
+				Pressure { static_cast<int>(reader_.read(32, 8)) * 4 / 3 },
+				Temperature { static_cast<int>(reader_.read(40, 8) & 0x7f) - 50 }
+			};
+
+		case 72:
+			return Reading {
+				Reading::Type::FLM_72,
+				reader_.read(0, 32),
+				Pressure { static_cast<int>(reader_.read(40, 8)) * 4 / 3 },
+				Temperature { static_cast<int>(reader_.read(48, 8)) - 50 }
+			};
+
+		case 80:
+			return Reading {
+				Reading::Type::FLM_80,
+				reader_.read(8, 32),
+				Pressure { static_cast<int>(reader_.read(48, 8)) * 4 / 3 },
+				Temperature { static_cast<int>(reader_.read(56, 8)) - 50 }
+			};
+
+		default:
+			return { };
+		}
+	}
+
+	if( signal_type == SignalType::Subaru ) {
+		return Reading {
+			Reading::Type::SUB_35,
+			reader_.read(3, 25),
+			Pressure { static_cast<int>(reader_.read(28, 8)) }
+		};
+	}
+
+	if( signal_type == SignalType::GMC ) {
+		return Reading {
+			Reading::Type::GMC_96,
+			reader_.read(20, 32),
+			Pressure { static_cast<int>(reader_.read(52, 8)) }
+		};
+	}
+
+	return { };
+}
+
+size_t Packet::crc_valid_length() const {
+	constexpr uint32_t checksum_bytes = 0b1111111;
+	constexpr uint32_t crc_72_bytes = 0b111111111;
+	constexpr uint32_t crc_80_bytes = 0b1111111110;
+
+	std::array<uint8_t, 10> bytes;
+	for(size_t i=0; i<bytes.size(); i++) {
+		bytes[i] = reader_.read(i * 8, 8);
+	}
+
+	uint32_t checksum = 0;
+	CRC<8> crc_72 { 0x01, 0x00 };
+	CRC<8> crc_80 { 0x01, 0x00 };
+
+	for(size_t i=0; i<bytes.size(); i++) {
+		const uint32_t byte_mask = 1 << i;
+		const auto byte = bytes[i];
+
+		if( checksum_bytes & byte_mask ) {
+			checksum += byte;
+		}
+		if( crc_72_bytes & byte_mask ) {
+			crc_72.process_byte(byte);
+		}
+		if( crc_80_bytes & byte_mask ) {
+			crc_80.process_byte(byte);
+		}
+	}
+
+	if( crc_80.checksum() == 0 ) {
+		return 80;
+	} else if( crc_72.checksum() == 0 ) {
+		return 72;
+	} else if( (checksum & 0xff) == bytes[7] ) {
+		return 64;
+	} else {
+		return 0;
+	}
+}
+
+} /* namespace tpms */
--- a/firmware/common/tpms_packet.hpp
+++ b/firmware/common/tpms_packet.hpp
@@ -0,0 +1,155 @@
+/*
+ * 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 __TPMS_PACKET_H__
+#define __TPMS_PACKET_H__
+
+#include <cstdint>
+#include <cstddef>
+
+#include "optional.hpp"
+
+#include "units.hpp"
+using units::Temperature;
+using units::Pressure;
+
+#include "baseband_packet.hpp"
+#include "manchester.hpp"
+#include "field_reader.hpp"
+
+namespace tpms {
+
+enum SignalType : uint32_t {
+	FLM = 1,
+	Subaru = 2,
+	GMC = 3,
+};
+
+class TransponderID {
+public:
+	constexpr TransponderID(
+	) : id_ { 0 }
+	{
+	}
+
+	constexpr TransponderID(
+		const uint32_t id
+	) : id_ { id }
+	{
+	}
+
+	constexpr uint32_t value() const {
+		return id_;
+	}
+
+private:
+	uint32_t id_;
+};
+
+class Reading {
+public:
+	enum Type {
+		None = 0,
+		FLM_64 = 1,
+		FLM_72 = 2,
+		FLM_80 = 3,
+		SUB_35 = 4,
+		GMC_96 = 5,
+	};
+
+	constexpr Reading(
+	) : type_ { Type::None }
+	{
+	}
+	
+	constexpr Reading(
+		Type type,
+		TransponderID id
+	) : type_ { type },
+		id_ { id }
+	{
+	}
+	
+	constexpr Reading(
+		Type type,
+		TransponderID id,
+		Optional<Pressure> pressure = { },
+		Optional<Temperature> temperature = { }
+	) : type_ { type },
+		id_ { id },
+		pressure_ { pressure },
+		temperature_ { temperature }
+	{
+	}
+
+	Type type() const {
+		return type_;
+	}
+
+	TransponderID id() const {
+		return id_;
+	}
+
+	Optional<Pressure> pressure() const {
+		return pressure_;
+	}
+
+	Optional<Temperature> temperature() const {
+		return temperature_;
+	}
+
+private:
+	Type type_ { Type::None };
+	TransponderID id_ { 0 };
+	Optional<Pressure> pressure_ { };
+	Optional<Temperature> temperature_ { };
+};
+
+class Packet {
+public:
+	constexpr Packet(
+		const baseband::Packet& packet
+	) : packet_ { packet },
+		decoder_ { packet_, 0 },
+		reader_ { decoder_ }
+	{
+	}
+
+	Timestamp received_at() const;
+
+	ManchesterFormatted symbols_formatted() const;
+
+	Optional<Reading> reading(const SignalType signal_type) const;
+
+private:
+	using Reader = FieldReader<ManchesterDecoder, BitRemapNone>;
+
+	const baseband::Packet packet_;
+	const ManchesterDecoder decoder_;
+
+	const Reader reader_;
+
+	size_t crc_valid_length() const;
+};
+
+} /* namespace tpms */
+
+#endif/*__TPMS_PACKET_H__*/
--- a/firmware/common/ui.hpp
+++ b/firmware/common/ui.hpp
@@ -36,13 +36,7 @@ struct Color {
 	) : v { 0 }
 	{
 	}
-/*
-	explicit constexpr Color(
-		const uint32_t value
-	) : v { static_cast<uint16_t>(value) }
-	{
-	}
-*/
+
 	constexpr Color(
 		uint8_t r,
 		uint8_t g,
@@ -96,14 +90,13 @@ struct Color {
 		return { 204, 0, 102 };
 	}
 };
-#if 0
-enum class CardinalDirection : uint8_t {
-	West,
-	South,
-	North,
-	East,
+
+struct ColorRGB888 {
+	uint8_t r;
+	uint8_t g;
+	uint8_t b;
 };
-#endif
+
 struct Point {
 	Coord x;
 	Coord y;
@@ -139,41 +132,6 @@ struct Point {
 		y += p.y;
 		return *this;
 	}
-#if 0
-	uint32_t magnitude_squared() const {
-		return (x * x) + (y * y);
-	}
-
-	CardinalDirection cardinal_direction() const {
-		/* TODO: Doesn't handle 0,0 */
-
-		const auto rotated = sloppy_rotate_by_45_degrees();
-
-		if( rotated.x > 0 ) {
-			if( rotated.y > 0 ) {
-				return CardinalDirection::East;
-			} else {
-				// y<=0
-				return CardinalDirection::North;
-			}
-		} else {
-			// x<=0
-			if( rotated.y > 0 ) {
-				return CardinalDirection::South;
-			} else {
-				return CardinalDirection::West;
-			}
-		}
-	}
-
-private:
-	Point sloppy_rotate_by_45_degrees() const {
-		/* Clockwise rotate (in screen coordinates), with a gain in
-		 * magnitude of sqrt(2).
-		 */
-		return { x - y, x + y };
-	}
-#endif
 };

 struct Size {
@@ -224,7 +182,6 @@ struct Rect {
 		size(size)
 	{
 	}
-
 	
 	int top() const {
 		return pos.y;
--- a/firmware/common/ui_focus.cpp
+++ b/firmware/common/ui_focus.cpp
@@ -49,9 +49,6 @@ void FocusManager::set_focus_widget(Widget* const new_focus_widget) {
 		// 	return;
 		// }
 		if( !new_focus_widget->focusable() ) {
-			// New widget is not focusable. Does it have a preferred child that
-			// can receive focus?
-			set_focus_widget(new_focus_widget->last_child_focus());
 			return;
 		}
 	}
@@ -67,7 +64,6 @@ void FocusManager::set_focus_widget(Widget* const new_focus_widget) {
 	if( focus_widget() ) {
 		focus_widget()->on_focus();
 		focus_widget()->set_dirty();
-		focus_widget()->parent()->set_last_child_focus(focus_widget());
 	}
 }

--- a/firmware/common/ui_widget.cpp
+++ b/firmware/common/ui_widget.cpp
@@ -28,8 +28,10 @@ bool is_dirty() {

 /* Widget ****************************************************************/

+const std::vector<Widget*> Widget::no_children { };
+
 Point Widget::screen_pos() {
-	return parent() ? (parent()->screen_pos() + parent_rect.pos) : parent_rect.pos;
+	return screen_rect().pos;
 }

 Size Widget::size() const {
@@ -112,19 +114,14 @@ bool Widget::focusable() const {
 	return flags.focusable;
 }

+void Widget::set_focusable(const bool value) {
+	flags.focusable = value;
+}
+
 bool Widget::has_focus() {
 	return (context().focus_manager().focus_widget() == this);
 }

-Widget* Widget::last_child_focus() const {
-	return nullptr;
-}
-
-void Widget::set_last_child_focus(Widget* const child) {
-	// Ignore.
-	(void)child;
-}
-
 bool Widget::on_key(const KeyEvent event) {
 	(void)event;
 	return false;
@@ -140,8 +137,8 @@ bool Widget::on_touch(const TouchEvent event) {
 	return false;
 }

-const std::vector<Widget*> Widget::children() const {
-	return { };
+const std::vector<Widget*>& Widget::children() const {
+	return no_children;
 }

 Context& Widget::context() const {
@@ -182,6 +179,14 @@ void Widget::visible(bool v) {
 	}
 }

+bool Widget::highlighted() const {
+	return flags.highlighted;
+}
+
+void Widget::set_highlighted(const bool value) {
+	flags.highlighted = value;
+}
+
 void Widget::dirty_overlapping_children_in_rect(const Rect& child_rect) {
 	for(auto child : children()) {
 		if( !child_rect.intersect(child->parent_rect).is_empty() ) {
@@ -192,11 +197,6 @@ void Widget::dirty_overlapping_children_in_rect(const Rect& child_rect) {

 /* View ******************************************************************/

-void View::set_parent_rect(const Rect new_parent_rect) {
-	Widget::set_parent_rect(new_parent_rect);
-	dirty_screen_rect += screen_rect();
-}
-
 void View::paint(Painter& painter) {
 	painter.fill_rectangle(
 		screen_rect(),
@@ -223,22 +223,15 @@ void View::add_children(const std::vector<Widget*>& children) {
 void View::remove_child(Widget* const widget) {
 	if( widget ) {
 		children_.erase(std::remove(children_.begin(), children_.end(), widget), children_.end());
-		dirty_screen_rect += widget->screen_rect();
+		dirty_overlapping_children_in_rect(widget->screen_rect());
 		widget->set_parent(nullptr);
-		if( dirty_screen_rect ) {
-			set_dirty();
-		}
 	}
 }

-const std::vector<Widget*> View::children() const {
+const std::vector<Widget*>& View::children() const {
 	return children_;
 }

-Widget* View::initial_focus() {
-	return nullptr;
-}
-
 std::string View::title() const {
 	return "";
 };
@@ -441,7 +434,7 @@ Button::Button(
 ) : Widget { parent_rect },
 	text_ { text }
 {
-	flags.focusable = true;
+	set_focusable(true);
 }

 void Button::set_text(const std::string value) {
@@ -456,7 +449,7 @@ std::string Button::text() const {
 void Button::paint(Painter& painter) {
 	const auto r = screen_rect();

-	const auto paint_style = (has_focus() || flags.highlighted) ? style().invert() : style();
+	const auto paint_style = (has_focus() || highlighted()) ? style().invert() : style();

 	painter.draw_rectangle(r, style().foreground);

@@ -492,13 +485,13 @@ bool Button::on_key(const KeyEvent key) {
 bool Button::on_touch(const TouchEvent event) {
 	switch(event.type) {
 	case TouchEvent::Type::Start:
-		flags.highlighted = true;
+		set_highlighted(true);
 		set_dirty();
 		return true;


 	case TouchEvent::Type::End:
-		flags.highlighted = false;
+		set_highlighted(false);
 		set_dirty();
 		if( on_select ) {
 			on_select(*this);
@@ -575,7 +568,7 @@ void Image::set_background(const Color color) {
 void Image::paint(Painter& painter) {
 	if( bitmap_ ) {
 		// Code also handles ImageButton behavior.
-		const bool selected = (has_focus() || flags.highlighted);
+		const bool selected = (has_focus() || highlighted());
 		painter.draw_bitmap(
 			screen_pos(),
 			*bitmap_,
@@ -596,7 +589,7 @@ ImageButton::ImageButton(
 	const Color background
 ) : Image { parent_rect, bitmap, foreground, background }
 {
-	flags.focusable = true;
+	set_focusable(true);
 }

 bool ImageButton::on_key(const KeyEvent key) {
@@ -613,13 +606,13 @@ bool ImageButton::on_key(const KeyEvent key) {
 bool ImageButton::on_touch(const TouchEvent event) {
 	switch(event.type) {
 	case TouchEvent::Type::Start:
-		flags.highlighted = true;
+		set_highlighted(true);
 		set_dirty();
 		return true;


 	case TouchEvent::Type::End:
-		flags.highlighted = false;
+		set_highlighted(false);
 		set_dirty();
 		if( on_select ) {
 			on_select(*this);
@@ -641,7 +634,7 @@ OptionsField::OptionsField(
 	length_ { length },
 	options { options }
 {
-	flags.focusable = true;
+	set_focusable(true);
 }

 size_t OptionsField::selected_index() const {
@@ -722,7 +715,7 @@ NumberField::NumberField(
 	length_ { length },
 	fill_char { fill_char }
 {
-	flags.focusable = true;
+	set_focusable(true);
 }

 int32_t NumberField::value() const {
--- a/firmware/common/ui_widget.hpp
+++ b/firmware/common/ui_widget.hpp
@@ -83,9 +83,8 @@ public:
 	virtual void blur();
 	virtual void on_blur();
 	bool focusable() const;
+	void set_focusable(const bool value);
 	bool has_focus();
-	virtual Widget* last_child_focus() const;
-	virtual void set_last_child_focus(Widget* const child);

 	virtual void paint(Painter& painter) = 0;

@@ -95,7 +94,7 @@ public:
 	virtual bool on_key(const KeyEvent event);
 	virtual bool on_encoder(const EncoderEvent event);
 	virtual bool on_touch(const TouchEvent event);
-	virtual const std::vector<Widget*> children() const;
+	virtual const std::vector<Widget*>& children() const;

 	virtual Context& context() const;

@@ -110,7 +109,13 @@ public:

 	void visible(bool v);

+	bool highlighted() const;
+	void set_highlighted(const bool value);
+
 protected:
+	void dirty_overlapping_children_in_rect(const Rect& child_rect);
+
+private:
 	/* Widget rectangle relative to parent pos(). */
 	Rect parent_rect;
 	const Style* style_ { nullptr };
@@ -132,7 +137,7 @@ protected:
 		.visible = false,
 	};

-	void dirty_overlapping_children_in_rect(const Rect& child_rect);
+	static const std::vector<Widget*> no_children;
 };

 class View : public Widget {
@@ -146,22 +151,17 @@ public:

 	// TODO: ~View() should on_hide() all children?

-	void set_parent_rect(const Rect new_parent_rect) override;
-
 	void paint(Painter& painter) override;

 	void add_child(Widget* const widget);
 	void add_children(const std::vector<Widget*>& children);
 	void remove_child(Widget* const widget);
-	const std::vector<Widget*> children() const override;
-
-	virtual Widget* initial_focus();
+	const std::vector<Widget*>& children() const override;

 	virtual std::string title() const;

 protected:
 	std::vector<Widget*> children_;
-	Rect dirty_screen_rect;

 	void invalidate_child(Widget* const widget);
 };
--- a/firmware/common/utility.cpp
+++ b/firmware/common/utility.cpp
@@ -22,7 +22,6 @@
 #include "utility.hpp"

 #include <cstdint>
-#include <cmath>

 #if 0
 uint32_t gcd(const uint32_t u, const uint32_t v) {
@@ -60,11 +59,36 @@ uint32_t gcd(const uint32_t u, const uint32_t v) {
 }
 #endif

-float complex16_mag_squared_to_dbv_norm(const float c16_mag_squared) {
-	constexpr float mag2_max = -32768.0f * -32768.0f + -32768.0f * -32768.0f;
-	constexpr float mag2_log10_max = std::log10(mag2_max);
-	constexpr float mag2_to_db_factor = 20.0f / 2.0f;
-	return (std::log10(c16_mag_squared) - mag2_log10_max) * mag2_to_db_factor;
+float fast_log2(const float val) {
+	// Thank you Stack Overflow!
+	// http://stackoverflow.com/questions/9411823/fast-log2float-x-implementation-c
+	union {
+		float val;
+		int32_t x;
+	} u = { val };
+	float log_2 = (((u.x >> 23) & 255) - 128);
+	u.x &= ~(255 << 23);
+	u.x +=  (127 << 23);
+	log_2 += ((-0.34484843f) * u.val + 2.02466578f) * u.val - 0.67487759f;
+	return log_2;
+}
+
+float fast_pow2(const float val) {
+	union {
+		float f;
+		uint32_t n;
+	} u;
+	u.n = val * 8388608 + (0x3f800000 - 60801 * 8);
+	return u.f;
+}
+
+float mag2_to_dbv_norm(const float mag2) {
+	constexpr float mag2_log2_max = 0.0f; //std::log2(1.0f);
+	constexpr float log_mag2_mag_factor = 0.5f;
+	constexpr float log2_log10_factor = 0.3010299956639812f; //std::log10(2.0f);
+	constexpr float log10_dbv_factor = 20.0f;
+	constexpr float mag2_to_db_factor = log_mag2_mag_factor * log2_log10_factor * log10_dbv_factor;
+	return (fast_log2(mag2) - mag2_log2_max) * mag2_to_db_factor;
 }

 /* GCD implementation derived from recursive implementation at
--- a/firmware/common/utility.hpp
+++ b/firmware/common/utility.hpp
@@ -61,7 +61,18 @@ inline constexpr T pow(const T base, unsigned const exponent) {
 	return (exponent == 0) ? 1 : (base * pow(base, exponent - 1));
 }

-float complex16_mag_squared_to_dbv_norm(const float c16_mag_squared);
+constexpr bool power_of_two(const size_t n) {
+	return (n & (n - 1)) == 0;
+}
+
+constexpr size_t log_2(const size_t n, const size_t p = 0) {
+	return (n <= 1) ? p : log_2(n / 2, p + 1);
+}
+
+float fast_log2(const float val);
+float fast_pow2(const float val);
+
+float mag2_to_dbv_norm(const float mag2);

 inline float magnitude_squared(const std::complex<float> c) {
 	const auto r = c.real();