diff --git a/.cproject b/.cproject
index e1bda8d..1627e0b 100644
--- a/.cproject
+++ b/.cproject
@@ -27,7 +27,7 @@
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.format.1959890047" name="Debug format" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.debugging.format" useByScannerDiscovery="true"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.toolchain.name.606808162" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.toolchain.name" useByScannerDiscovery="false" value="GNU Tools for ARM Embedded Processors" valueType="string"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.architecture.355585421" name="Architecture" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.architecture" useByScannerDiscovery="false" value="ilg.gnuarmeclipse.managedbuild.cross.option.architecture.arm" valueType="enumerated"/>
-							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.family.1992692312" name="ARM family" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.family" useByScannerDiscovery="false" value="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.mcpu.cortex-m4" valueType="enumerated"/>
+							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.family.1992692312" name="ARM family (-mcpu)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.family" useByScannerDiscovery="false" value="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.mcpu.cortex-m4" valueType="enumerated"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.instructionset.203230960" name="Instruction set" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.instructionset" useByScannerDiscovery="false" value="ilg.gnuarmeclipse.managedbuild.cross.option.arm.target.instructionset.thumb" valueType="enumerated"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.prefix.2009350133" name="Prefix" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.prefix" useByScannerDiscovery="false" value="arm-none-eabi-" valueType="string"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.command.c.529320578" name="C compiler" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.command.c" useByScannerDiscovery="false" value="gcc" valueType="string"/>
@@ -53,7 +53,7 @@
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.other.1937611003" name="Other optimization flags" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.other" useByScannerDiscovery="true" value="-fstack-usage -fstrict-aliasing -ffast-math" valueType="string"/>
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.noinlinefunctions.1239408347" name="Do not inline functions (-fno-inline-functions)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.noinlinefunctions" useByScannerDiscovery="true" value="true" valueType="boolean"/>
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="ilg.gnuarmeclipse.managedbuild.cross.targetPlatform.555069223" isAbstract="false" osList="all" superClass="ilg.gnuarmeclipse.managedbuild.cross.targetPlatform"/>
-							<builder autoBuildTarget="all" buildPath="${workspace_loc:/TNC3-firmware}/ARM Debug" cleanBuildTarget="clean" command="${cross_make}" enableAutoBuild="true" id="org.eclipse.cdt.build.core.internal.builder.1584568207" incrementalBuildTarget="all" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="CDT Internal Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="org.eclipse.cdt.build.core.internal.builder"/>
+							<builder autoBuildTarget="all" buildPath="${workspace_loc:/TNC3-firmware}/ARM Debug" cleanBuildTarget="clean" command="${cross_make}" enableAutoBuild="true" id="org.eclipse.cdt.build.core.internal.builder.1584568207" incrementalBuildTarget="all" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="CDT Internal Builder" parallelBuildOn="false" superClass="org.eclipse.cdt.build.core.internal.builder"/>
 							<tool id="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.1401064567" name="GNU ARM Cross Assembler" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler">
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.usepreprocessor.33292167" name="Use preprocessor" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.usepreprocessor" useByScannerDiscovery="false" value="true" valueType="boolean"/>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.include.paths.685185658" name="Include paths (-I)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.include.paths" useByScannerDiscovery="true" valueType="includePath">
@@ -194,7 +194,7 @@
 				</extensions>
 			</storageModule>
 			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration artifactName="firmware" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug" cleanCommand="${cross_rm} -rf" description="" id="ilg.gnuarmeclipse.managedbuild.cross.config.elf.debug.1309195742.6250637.815824991" name="ARM_Release" optionalBuildProperties="" parent="ilg.gnuarmeclipse.managedbuild.cross.config.elf.debug" postbuildStep="${cross_prefix}${cross_objcopy}${cross_suffix} firmware.elf -O binary firmware.bin">
+				<configuration artifactName="firmware" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug" cleanCommand="${cross_rm} -rf" description="" id="ilg.gnuarmeclipse.managedbuild.cross.config.elf.debug.1309195742.6250637.815824991" name="ARM_Release" optionalBuildProperties="" parent="ilg.gnuarmeclipse.managedbuild.cross.config.elf.debug" postbuildStep="">
 					<folderInfo id="ilg.gnuarmeclipse.managedbuild.cross.config.elf.debug.1309195742.6250637.815824991." name="/" resourcePath="">
 						<toolChain id="ilg.gnuarmeclipse.managedbuild.cross.toolchain.elf.debug.74600812" name="ARM Cross GCC" nonInternalBuilderId="ilg.gnuarmeclipse.managedbuild.cross.builder" superClass="ilg.gnuarmeclipse.managedbuild.cross.toolchain.elf.debug">
 							<option id="ilg.gnuarmeclipse.managedbuild.cross.option.addtools.createflash.1538196244" name="Create flash image" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.addtools.createflash" useByScannerDiscovery="false" value="true" valueType="boolean"/>
@@ -238,7 +238,7 @@
 							<builder autoBuildTarget="all" buildPath="${workspace_loc:/TNC3-firmware}/ARM Debug" cleanBuildTarget="clean" command="${cross_make}" enableAutoBuild="true" id="org.eclipse.cdt.build.core.internal.builder.2110076473" incrementalBuildTarget="all" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="CDT Internal Builder" parallelBuildOn="false" superClass="org.eclipse.cdt.build.core.internal.builder"/>
 							<tool id="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.1093600654" name="GNU ARM Cross Assembler" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler">
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.usepreprocessor.1179297555" name="Use preprocessor" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.usepreprocessor" useByScannerDiscovery="false" value="true" valueType="boolean"/>
-								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.include.paths.1845769595" name="Include paths (-I)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.include.paths" useByScannerDiscovery="true" valueType="includePath">
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.include.paths.1845769595" name="Include paths (-I)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.include.paths" useByScannerDiscovery="true" valueType="includePath">
 									<listOptionValue builtIn="false" value="/usr/arm-none-eabi/include"/>
 									<listOptionValue builtIn="false" value="../Drivers/STM32L4xx_HAL_Driver/Inc"/>
 									<listOptionValue builtIn="false" value="../Inc"/>
@@ -251,7 +251,7 @@
 									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/"/>
 									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Core/Inc"/>
 								</option>
-								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.defs.208015513" name="Defined symbols (-D)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.defs" useByScannerDiscovery="true" valueType="definedSymbols">
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.defs.208015513" name="Defined symbols (-D)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.assembler.defs" useByScannerDiscovery="true" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="__FPU_PRESENT=1"/>
 									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
 									<listOptionValue builtIn="false" value="ARM_MATH_CM4"/>
@@ -261,7 +261,7 @@
 								<inputType id="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.input.1462552668" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.assembler.input"/>
 							</tool>
 							<tool id="ilg.gnuarmeclipse.managedbuild.cross.tool.c.compiler.2037524494" name="GNU ARM Cross C Compiler" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.c.compiler">
-								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.include.paths.549073773" name="Include paths (-I)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.include.paths" useByScannerDiscovery="true" valueType="includePath">
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.include.paths.549073773" name="Include paths (-I)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.include.paths" useByScannerDiscovery="true" valueType="includePath">
 									<listOptionValue builtIn="false" value="/usr/arm-none-eabi/include"/>
 									<listOptionValue builtIn="false" value="../Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/STM32L4xx_HAL_Driver/Inc"/>
@@ -274,7 +274,7 @@
 									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/"/>
 									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Core/Inc"/>
 								</option>
-								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.defs.1057000273" name="Defined symbols (-D)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.defs" useByScannerDiscovery="true" valueType="definedSymbols">
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.defs.1057000273" name="Defined symbols (-D)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.c.compiler.defs" useByScannerDiscovery="true" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="__FPU_PRESENT=1"/>
 									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
 									<listOptionValue builtIn="false" value="ARM_MATH_CM4"/>
@@ -287,7 +287,7 @@
 								<inputType id="ilg.gnuarmeclipse.managedbuild.cross.tool.c.compiler.input.1311038531" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.c.compiler.input"/>
 							</tool>
 							<tool id="ilg.gnuarmeclipse.managedbuild.cross.tool.cpp.compiler.1738445104" name="GNU ARM Cross C++ Compiler" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.cpp.compiler">
-								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.compiler.include.paths.1151610661" name="Include paths (-I)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.compiler.include.paths" useByScannerDiscovery="true" valueType="includePath">
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.compiler.include.paths.1151610661" name="Include paths (-I)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.compiler.include.paths" useByScannerDiscovery="true" valueType="includePath">
 									<listOptionValue builtIn="false" value="/usr/arm-none-eabi/include"/>
 									<listOptionValue builtIn="false" value="/usr/local/include"/>
 									<listOptionValue builtIn="false" value="../Inc"/>
@@ -301,7 +301,7 @@
 									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Core/Inc"/>
 									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/"/>
 								</option>
-								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.compiler.defs.602101294" name="Defined symbols (-D)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.compiler.defs" useByScannerDiscovery="true" valueType="definedSymbols">
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.compiler.defs.602101294" name="Defined symbols (-D)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.compiler.defs" useByScannerDiscovery="true" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="__FPU_PRESENT=1"/>
 									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
 									<listOptionValue builtIn="false" value="ARM_MATH_CM4"/>
@@ -327,7 +327,7 @@
 							</tool>
 							<tool id="ilg.gnuarmeclipse.managedbuild.cross.tool.cpp.linker.1881696985" name="GNU ARM Cross C++ Linker" superClass="ilg.gnuarmeclipse.managedbuild.cross.tool.cpp.linker">
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.gcsections.1261027452" name="Remove unused sections (-Xlinker --gc-sections)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.gcsections" useByScannerDiscovery="false" value="true" valueType="boolean"/>
-								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.scriptfile.1929266642" name="Script files (-T)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.scriptfile" useByScannerDiscovery="false" valueType="stringList">
+								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.scriptfile.1929266642" name="Script files (-T)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.scriptfile" useByScannerDiscovery="false" valueType="stringList">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/STM32L433CCUx_FLASH.ld}&quot;"/>
 								</option>
 								<option id="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.nostdlibs.118214268" name="No startup or default libs (-nostdlib)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.cpp.linker.nostdlibs" useByScannerDiscovery="false" value="false" valueType="boolean"/>
diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml
index 2bda0b1..8fe73b1 100644
--- a/.settings/language.settings.xml
+++ b/.settings/language.settings.xml
@@ -3,7 +3,7 @@
 	<configuration id="ilg.gnuarmeclipse.managedbuild.cross.config.elf.debug.1309195742.6250637" name="ARM_Debug">
 		<extension point="org.eclipse.cdt.core.LanguageSettingsProvider">
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
-			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-17584508179154847" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT ARM Cross GCC Built-in Compiler Settings " parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-875080106735501948" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT ARM Cross GCC Built-in Compiler Settings " parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
@@ -14,7 +14,7 @@
 	<configuration id="ilg.gnuarmeclipse.managedbuild.cross.config.elf.debug.1309195742.6250637.815824991" name="ARM_Release">
 		<extension point="org.eclipse.cdt.core.LanguageSettingsProvider">
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
-			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="39340766364977905" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT ARM Cross GCC Built-in Compiler Settings " parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-789413234192079628" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT ARM Cross GCC Built-in Compiler Settings " parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
diff --git a/Inc/FreeRTOSConfig.h b/Inc/FreeRTOSConfig.h
index d083621..388a1d8 100644
--- a/Inc/FreeRTOSConfig.h
+++ b/Inc/FreeRTOSConfig.h
@@ -67,7 +67,8 @@
 #define configCHECK_FOR_STACK_OVERFLOW           1
 #define configENABLE_BACKWARD_COMPATIBILITY      0
 #define configUSE_PORT_OPTIMISED_TASK_SELECTION  1
-#define configUSE_TICKLESS_IDLE                  1
+// FIXME: configUSE_TICKLESS_IDLE must be 0 or changing modem type locks up.
+#define configUSE_TICKLESS_IDLE                  0
 
 /* Co-routine definitions. */
 #define configUSE_CO_ROUTINES                    0
diff --git a/Inc/main.h b/Inc/main.h
index 66e94aa..b39688d 100644
--- a/Inc/main.h
+++ b/Inc/main.h
@@ -168,12 +168,26 @@ extern osMutexId hardwareInitMutexHandle;
 
 #define CxxErrorHandler() _Error_Handler(const_cast<char*>(__FILE__), __LINE__)
 
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+void SysClock48(void);
+void SysClock80(void);
+void SysClock4(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#define SystemClock_Config_48MHz SystemClock_Config
+
 /* USER CODE END Private defines */
 
 #ifdef __cplusplus
  extern "C" {
 #endif
-void _Error_Handler(char *, int);
+void _Error_Handler(char *, int) __attribute__ ((noreturn));
 
 #define Error_Handler() _Error_Handler(__FILE__, __LINE__)
 #ifdef __cplusplus
diff --git a/Inc/usbd_cdc_if.h b/Inc/usbd_cdc_if.h
index 140c224..bd7a0fd 100644
--- a/Inc/usbd_cdc_if.h
+++ b/Inc/usbd_cdc_if.h
@@ -78,6 +78,9 @@
   */
 /* USER CODE BEGIN EXPORTED_DEFINES */
 
+#define APP_RX_DATA_SIZE  64
+#define APP_TX_DATA_SIZE  64
+
 /* USER CODE END EXPORTED_DEFINES */
 
 /**
@@ -91,6 +94,14 @@
 
 /* USER CODE BEGIN EXPORTED_TYPES */
 
+struct UsbCdcRxBuffer
+{
+    uint8_t buffer[APP_RX_DATA_SIZE];
+    uint32_t size;
+};
+
+typedef struct UsbCdcRxBuffer UsbCdcRxBuffer_t;
+
 /* USER CODE END EXPORTED_TYPES */
 
 /**
@@ -120,6 +131,10 @@ extern USBD_CDC_ItfTypeDef USBD_Interface_fops_FS;
 
 /* USER CODE BEGIN EXPORTED_VARIABLES */
 
+// USB CDC receive ping pong buffer.
+extern UsbCdcRxBuffer_t* usbCdcRxBuffer_1;
+extern UsbCdcRxBuffer_t* usbCdcRxBuffer_2;
+
 /* USER CODE END EXPORTED_VARIABLES */
 
 /**
diff --git a/Src/main.c b/Src/main.c
index 5b65f58..d1bdce7 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -60,6 +60,7 @@
 #include "bm78.h"
 #include "base64.h"
 #include "KissHardware.h"
+#include "Log.h"
 
 /* USER CODE END Includes */
 
@@ -421,6 +422,10 @@ int main(void)
   /* USER CODE BEGIN SysInit */
 #ifdef KISS_LOGGING
   printf("start\r\n");
+  if (error_message[0] != 0) {
+      printf(error_message);
+      error_message[0] = 0;
+  }
 #endif
 
   // Note that it is important that all GPIO interrupts are disabled until
@@ -457,6 +462,10 @@ int main(void)
 
   encode_serial_number();
 
+  // Manchester encoding mode on SWO.
+  // *((volatile unsigned *)(TPI->SPPR)) = 0x00000001;
+
+
   if (!go_back_to_sleep) {
       // The Bluetooth module is powered on during MX_GPIO_Init().  BT_CMD
       // has a weak pull-up on the BT module and is in OD mode.  Pull the
@@ -701,11 +710,13 @@ void SystemClock_Config(void)
     /**Initializes the CPU, AHB and APB busses clocks 
     */
   RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_LSI|RCC_OSCILLATORTYPE_LSE
-                              |RCC_OSCILLATORTYPE_MSI;
+                              |RCC_OSCILLATORTYPE_MSI|RCC_OSCILLATORTYPE_HSI;
   RCC_OscInitStruct.LSEState = RCC_LSE_ON;
   RCC_OscInitStruct.LSIState = RCC_LSI_OFF;
   RCC_OscInitStruct.MSIState = RCC_MSI_ON;
-  RCC_OscInitStruct.MSICalibrationValue = 0;
+  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
+  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
+  RCC_OscInitStruct.MSICalibrationValue = RCC_MSICALIBRATION_DEFAULT;
   RCC_OscInitStruct.MSIClockRange = RCC_MSIRANGE_6;
   RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
   RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_MSI;
@@ -719,7 +730,7 @@ void SystemClock_Config(void)
     _Error_Handler(__FILE__, __LINE__);
   }
 
-    /**Initializes the CPU, AHB and APB busses clocks 
+    /**Initializes the CPU, AHB and APB busses clocks
     */
   RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                               |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
@@ -736,9 +747,9 @@ void SystemClock_Config(void)
   PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_RTC|RCC_PERIPHCLK_USART3
                               |RCC_PERIPHCLK_I2C1|RCC_PERIPHCLK_USB
                               |RCC_PERIPHCLK_RNG|RCC_PERIPHCLK_ADC;
-  PeriphClkInit.Usart3ClockSelection = RCC_USART3CLKSOURCE_PCLK1;
-  PeriphClkInit.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1;
-  PeriphClkInit.AdcClockSelection = RCC_ADCCLKSOURCE_PLLSAI1;
+  PeriphClkInit.Usart3ClockSelection = RCC_USART3CLKSOURCE_HSI;
+  PeriphClkInit.I2c1ClockSelection = RCC_I2C1CLKSOURCE_HSI;
+  PeriphClkInit.AdcClockSelection = RCC_ADCCLKSOURCE_SYSCLK;
   PeriphClkInit.RTCClockSelection = RCC_RTCCLKSOURCE_LSE;
   PeriphClkInit.UsbClockSelection = RCC_USBCLKSOURCE_PLLSAI1;
   PeriphClkInit.RngClockSelection = RCC_RNGCLKSOURCE_PLLSAI1;
@@ -756,32 +767,34 @@ void SystemClock_Config(void)
 
 #ifdef KISS_LOGGING
   HAL_RCCEx_EnableLSCO(RCC_LSCOSOURCE_LSE);
+#else
+  HAL_RCCEx_DisableLSCO();
 #endif
 
-    /**Configure the main internal regulator output voltage 
+    /**Configure the main internal regulator output voltage
     */
   if (HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1) != HAL_OK)
   {
     _Error_Handler(__FILE__, __LINE__);
   }
 
-    /**Configure the Systick interrupt time 
+    /**Configure the Systick interrupt time
     */
   HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);
 
-    /**Configure the Systick 
+    /**Configure the Systick
     */
   HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
 
-    /**Enable MSI Auto calibration 
+    /**Enable MSI Auto calibration
     */
   HAL_RCCEx_EnableMSIPLLMode();
 
-    /**Enable the SYSCFG APB clock 
+    /**Enable the SYSCFG APB clock
     */
   __HAL_RCC_CRS_CLK_ENABLE();
 
-    /**Configures CRS 
+    /**Configures CRS
     */
   RCC_CRSInitStruct.Prescaler = RCC_CRS_SYNC_DIV1;
   RCC_CRSInitStruct.Source = RCC_CRS_SYNC_SOURCE_LSE;
@@ -882,7 +895,7 @@ static void MX_DAC1_Init(void)
   sConfig.DAC_SampleAndHold = DAC_SAMPLEANDHOLD_DISABLE;
   sConfig.DAC_Trigger = DAC_TRIGGER_T7_TRGO;
   sConfig.DAC_OutputBuffer = DAC_OUTPUTBUFFER_ENABLE;
-  sConfig.DAC_ConnectOnChipPeripheral = DAC_CHIPCONNECT_ENABLE;
+  sConfig.DAC_ConnectOnChipPeripheral = DAC_CHIPCONNECT_DISABLE;
   sConfig.DAC_UserTrimming = DAC_TRIMMING_FACTORY;
   if (HAL_DAC_ConfigChannel(&hdac1, &sConfig, DAC_CHANNEL_1) != HAL_OK)
   {
@@ -891,8 +904,11 @@ static void MX_DAC1_Init(void)
 
     /**DAC channel OUT2 config 
     */
+  sConfig.DAC_SampleAndHold = DAC_SAMPLEANDHOLD_DISABLE;
   sConfig.DAC_Trigger = DAC_TRIGGER_NONE;
-  sConfig.DAC_ConnectOnChipPeripheral = DAC_CHIPCONNECT_ENABLE;
+  sConfig.DAC_OutputBuffer = DAC_OUTPUTBUFFER_ENABLE;
+  sConfig.DAC_ConnectOnChipPeripheral = DAC_CHIPCONNECT_DISABLE;
+  sConfig.DAC_UserTrimming = DAC_TRIMMING_FACTORY;
   if (HAL_DAC_ConfigChannel(&hdac1, &sConfig, DAC_CHANNEL_2) != HAL_OK)
   {
     _Error_Handler(__FILE__, __LINE__);
@@ -905,7 +921,9 @@ static void MX_I2C1_Init(void)
 {
 
   hi2c1.Instance = I2C1;
-  hi2c1.Init.Timing = 0x20000209;
+//  hi2c1.Init.Timing = 0x00300F33;   // 80MHz
+//  hi2c1.Init.Timing = 0x20000209;   // 48MHz
+  hi2c1.Init.Timing = 0x00000107;   // HSI
   hi2c1.Init.OwnAddress1 = 0;
   hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
   hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
@@ -1434,6 +1452,118 @@ void init_rtc_alarm()
 
 }
 
+void SysClock48()
+{
+    RCC_OscInitTypeDef RCC_OscInitStruct;
+    RCC_ClkInitTypeDef RCC_ClkInitStruct;
+
+    if (HAL_RCC_GetHCLKFreq() == 48000000) return;
+
+    INFO("Setting 48MHz SysClock.");
+
+    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_SYSCLK;
+    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
+
+    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK)
+    {
+      _Error_Handler(__FILE__, __LINE__);
+    }
+
+    /**Configure the Systick interrupt time
+    */
+    HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);
+
+    RCC_OscInitStruct.OscillatorType = 0;
+    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
+    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_MSI;
+    RCC_OscInitStruct.PLL.PLLM = 1;
+    RCC_OscInitStruct.PLL.PLLN = 24;
+    RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV7;
+    RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
+    RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
+    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
+    {
+      _Error_Handler(__FILE__, __LINE__);
+    }
+
+    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_SYSCLK;
+    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
+
+    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
+    {
+      _Error_Handler(__FILE__, __LINE__);
+    }
+
+    /**Configure the Systick interrupt time
+    */
+    HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);
+}
+
+void SysClock80()
+{
+    RCC_OscInitTypeDef RCC_OscInitStruct;
+    RCC_ClkInitTypeDef RCC_ClkInitStruct;
+
+    if (HAL_RCC_GetHCLKFreq() == 80000000) return;
+
+    INFO("Setting 80MHz SysClock.");
+
+    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_SYSCLK;
+    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
+
+    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK)
+    {
+      _Error_Handler(__FILE__, __LINE__);
+    }
+
+    RCC_OscInitStruct.OscillatorType = 0;
+    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
+    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_MSI;
+    RCC_OscInitStruct.PLL.PLLM = 1;
+    RCC_OscInitStruct.PLL.PLLN = 40;
+    RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV7;
+    RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
+    RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
+    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
+    {
+      _Error_Handler(__FILE__, __LINE__);
+    }
+
+    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_SYSCLK;
+    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
+
+    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK)
+    {
+      _Error_Handler(__FILE__, __LINE__);
+    }
+
+    /**Configure the Systick interrupt time
+    */
+    HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);
+}
+
+void SysClock4()
+{
+    RCC_OscInitTypeDef RCC_OscInitStruct;
+    RCC_ClkInitTypeDef RCC_ClkInitStruct;
+
+    taskENTER_CRITICAL();
+
+    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_SYSCLK;
+    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_MSI;
+
+    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK)
+    {
+      _Error_Handler(__FILE__, __LINE__);
+    }
+
+    /**Configure the Systick interrupt time
+    */
+    HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);
+
+    taskEXIT_CRITICAL();
+}
+
 /* USER CODE END 4 */
 
 /* USER CODE BEGIN Header_StartDefaultTask */
@@ -1461,6 +1591,7 @@ void StartDefaultTask(void const * argument)
 #ifdef KISS_LOGGING
     printf("VBUS not detected\r\n");
 #endif
+//    SysClock4();
   }
 /* Infinite loop */
   for(;;)
@@ -1507,7 +1638,8 @@ void _Error_Handler(char *file, int line)
 #ifdef KISS_LOGGING
   printf("Error handler called from file %s on line %d\r\n", file, line);
 #endif
-  snprintf(error_message, sizeof(error_message), "Error: %s:%d", file, line);
+  snprintf(error_message, sizeof(error_message), "Error: %s:%d\r\n", file, line);
+  error_message[sizeof(error_message) - 1] = 0;
 
   stop_now = 0;
   go_back_to_sleep = 0;
diff --git a/Src/stm32l4xx_it.c b/Src/stm32l4xx_it.c
index b8bf14e..6f28f29 100644
--- a/Src/stm32l4xx_it.c
+++ b/Src/stm32l4xx_it.c
@@ -41,6 +41,8 @@
 #include "main.h"
 extern osMessageQId ioEventQueueHandle;
 
+void idleInterruptCallback(UART_HandleTypeDef* huart);
+
 /* USER CODE END 0 */
 
 /* External variables --------------------------------------------------------*/
@@ -386,6 +388,12 @@ void USART3_IRQHandler(void)
 {
   /* USER CODE BEGIN USART3_IRQn 0 */
 
+    if (__HAL_UART_GET_FLAG(&huart3, UART_FLAG_IDLE)) {
+        idleInterruptCallback(&huart3);
+        __HAL_UART_CLEAR_FLAG(&huart3, UART_FLAG_IDLE);
+        return;
+    }
+
   /* USER CODE END USART3_IRQn 0 */
   HAL_UART_IRQHandler(&huart3);
   /* USER CODE BEGIN USART3_IRQn 1 */
diff --git a/Src/usbd_cdc_if.c b/Src/usbd_cdc_if.c
index 583ee90..2e36f0e 100644
--- a/Src/usbd_cdc_if.c
+++ b/Src/usbd_cdc_if.c
@@ -97,8 +97,6 @@
 /* USER CODE BEGIN PRIVATE_DEFINES */
 /* Define size for the receive and transmit buffer over CDC */
 /* It's up to user to redefine and/or remove those define */
-#define APP_RX_DATA_SIZE  64
-#define APP_TX_DATA_SIZE  64
 /* USER CODE END PRIVATE_DEFINES */
 
 /**
@@ -124,8 +122,6 @@
   */
 /* Create buffer for reception and transmission           */
 /* It's up to user to redefine and/or remove those define */
-/** Received data over USB are stored in this buffer      */
-uint8_t UserRxBufferFS[APP_RX_DATA_SIZE];
 
 /** Data to send over USB CDC are stored in this buffer   */
 uint8_t UserTxBufferFS[APP_TX_DATA_SIZE];
@@ -138,6 +134,11 @@ USBD_CDC_LineCodingTypeDef LineCoding = {
     0x08    // number of bits: 8
 };
 
+// USB CDC receive ping pong buffer.
+UsbCdcRxBuffer_t usbCdcRxBuffer[2];
+UsbCdcRxBuffer_t* usbCdcRxBuffer_1 = &usbCdcRxBuffer[0];
+UsbCdcRxBuffer_t* usbCdcRxBuffer_2 = &usbCdcRxBuffer[1];
+
 /* USER CODE END PRIVATE_VARIABLES */
 
 /**
@@ -195,7 +196,7 @@ static int8_t CDC_Init_FS(void)
   /* USER CODE BEGIN 3 */
   /* Set Application Buffers */
   USBD_CDC_SetTxBuffer(&hUsbDeviceFS, UserTxBufferFS, 0);
-  USBD_CDC_SetRxBuffer(&hUsbDeviceFS, UserRxBufferFS);
+  USBD_CDC_SetRxBuffer(&hUsbDeviceFS, usbCdcRxBuffer_1->buffer);
   return (USBD_OK);
   /* USER CODE END 3 */
 }
@@ -304,12 +305,6 @@ static int8_t CDC_Control_FS(uint8_t cmd, uint8_t* pbuf, uint16_t length)
   * @brief  Data received over USB OUT endpoint are sent over CDC interface
   *         through this function.
   *
-  *         @note
-  *         This function will block any OUT packet reception on USB endpoint
-  *         until exiting this function. If you exit this function before transfer
-  *         is complete on CDC interface (ie. using DMA controller) it will result
-  *         in receiving more data while previous ones are still not sent.
-  *
   * @param  Buf: Buffer of data to be received
   * @param  Len: Number of data received (in bytes)
   * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL
@@ -320,10 +315,8 @@ static int8_t CDC_Receive_FS(uint8_t* Buf, uint32_t *Len)
     if (!cdc_connected) {
         osMessagePut(ioEventQueueHandle, CMD_USB_CDC_CONNECT, 0);
     }
-  cdc_receive(Buf, *Len);
-  USBD_CDC_SetRxBuffer(&hUsbDeviceFS, UserRxBufferFS);
-  USBD_CDC_ReceivePacket(&hUsbDeviceFS);
-  return (USBD_OK);
+    cdc_receive(Buf, *Len);
+    return (USBD_OK);
   /* USER CODE END 6 */
 }
 
diff --git a/TNC/AFSKModulator.cpp b/TNC/AFSKModulator.cpp
new file mode 100644
index 0000000..d4d1c66
--- /dev/null
+++ b/TNC/AFSKModulator.cpp
@@ -0,0 +1,38 @@
+// Copyright 2020 Rob Riggs <rob@mobilinkd.com>
+// All rights reserved.
+
+#include "AFSKModulator.hpp"
+
+namespace mobilinkd { namespace tnc {
+
+void AFSKModulator::init(const kiss::Hardware& hw)
+{
+    set_twist(hw.tx_twist);
+
+    SysClock48();
+
+    // Configure 48MHz clock for 26.4ksps.
+    htim7.Init.Period = 1817;
+    if (HAL_TIM_Base_Init(&htim7) != HAL_OK)
+    {
+        ERROR("htim7 init failed");
+        CxxErrorHandler();
+    }
+
+    DAC_ChannelConfTypeDef sConfig;
+
+    sConfig.DAC_SampleAndHold = DAC_SAMPLEANDHOLD_DISABLE;
+    sConfig.DAC_Trigger = DAC_TRIGGER_NONE;
+    sConfig.DAC_OutputBuffer = DAC_OUTPUTBUFFER_ENABLE;
+    sConfig.DAC_ConnectOnChipPeripheral = DAC_CHIPCONNECT_ENABLE;
+    sConfig.DAC_UserTrimming = DAC_TRIMMING_FACTORY;
+    if (HAL_DAC_ConfigChannel(&hdac1, &sConfig, DAC_CHANNEL_1) != HAL_OK)
+    {
+      CxxErrorHandler();
+    }
+
+    if (HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, 2048) != HAL_OK) CxxErrorHandler();
+    if (HAL_DAC_Start(&hdac1, DAC_CHANNEL_1) != HAL_OK) CxxErrorHandler();
+}
+
+}} // mobilinkd::tnc
diff --git a/TNC/AFSKModulator.hpp b/TNC/AFSKModulator.hpp
index 7f31e04..a6c80c9 100644
--- a/TNC/AFSKModulator.hpp
+++ b/TNC/AFSKModulator.hpp
@@ -1,13 +1,13 @@
-// Copyright 2015-2019 Mobilinkd LLC <rob@mobilinkd.com>
+// Copyright 2015-2020 Mobilinkd LLC <rob@mobilinkd.com>
 // All rights reserved.
 
-#ifndef MOBILINKD__TNC__AFSK_MODULATOR_HPP_
-#define MOBILINKD__TNC__AFSK_MODULATOR_HPP_
+#pragma once
 
 #include <stddef.h>
 
 #include "PTT.hpp"
 #include "Log.h"
+#include "Modulator.hpp"
 
 #include "stm32l4xx_hal.h"
 #include "cmsis_os.h"
@@ -62,8 +62,8 @@ const int16_t sin_table[SIN_TABLE_LEN] = {
 };
 
 
-struct AFSKModulator {
-
+struct AFSKModulator : Modulator
+{
     static const size_t DAC_BUFFER_LEN = 44;
     static const size_t BIT_LEN = DAC_BUFFER_LEN / 2;
     static const size_t MARK_SKIP = 12;
@@ -75,7 +75,7 @@ struct AFSKModulator {
     PTT* ptt_;
     uint8_t twist_{50};
     uint16_t volume_{4096};
-    uint16_t buffer_[DAC_BUFFER_LEN];
+    std::array<uint16_t, DAC_BUFFER_LEN> buffer_;
 
     AFSKModulator(osMessageQId queue, PTT* ptt)
     : dacOutputQueueHandle_(queue), ptt_(ptt)
@@ -84,7 +84,13 @@ struct AFSKModulator {
             buffer_[i] = 2048;
     }
 
-    void set_volume(uint16_t v)
+   void init(const kiss::Hardware& hw);
+
+   void deinit() override
+   {
+   }
+
+   void set_gain(uint16_t v) override
     {
         v = std::max<uint16_t>(256, v);
         v = std::min<uint16_t>(4096, v);
@@ -103,18 +109,22 @@ struct AFSKModulator {
 
     void set_twist(uint8_t twist) {twist_ = twist;}
 
-    void send(bool bit) {
+    void send(bool bit) override
+    {
         switch (running_) {
         case -1:
+            ptt_->on();
+            #ifdef KISS_LOGGING
+                HAL_RCCEx_DisableLSCO();
+            #endif
+
             fill_first(bit);
             running_ = 0;
             break;
         case 0:
             fill_last(bit);
             running_ = 1;
-            ptt_->on();
-            HAL_TIM_Base_Start(&htim7);
-            HAL_DAC_Start_DMA(&hdac1, DAC_CHANNEL_1, (uint32_t*) buffer_, DAC_BUFFER_LEN, DAC_ALIGN_12B_R);
+            start_conversion();
             break;
         case 1:
             osMessagePut(dacOutputQueueHandle_, bit, osWaitForever);
@@ -122,8 +132,10 @@ struct AFSKModulator {
         }
     }
 
-    void fill(uint16_t* buffer, bool bit) {
-        for (size_t i = 0; i != BIT_LEN; i++) {
+    void fill(uint16_t* buffer, bool bit)
+    {
+        for (size_t i = 0; i != BIT_LEN; i++)
+        {
             int s = sin_table[pos_];
             s -= 2048;
             s *= volume_;
@@ -149,44 +161,81 @@ struct AFSKModulator {
         }
     }
 
-    void fill_first(bool bit) {
-        fill(buffer_, bit);
+    void fill_first(bool bit) override
+    {
+        fill(buffer_.data(), bit);
     }
 
-    void fill_last(bool bit) {
-        fill(buffer_ + BIT_LEN, bit);
+    void fill_last(bool bit) override
+    {
+        fill(buffer_.data() + BIT_LEN, bit);
     }
 
-    void empty() {
+    void empty() override
+    {
         switch (running_) {
         case 1:
             running_ = 0;
             break;
         case 0:
             running_ = -1;
-            HAL_DAC_Stop_DMA(&hdac1, DAC_CHANNEL_1);
-            HAL_TIM_Base_Stop(&htim7);
+            stop_conversion();
             ptt_->off();
             pos_ = 0;
+            #ifdef KISS_LOGGING
+                HAL_RCCEx_EnableLSCO(RCC_LSCOSOURCE_LSE);
+            #endif
             break;
         case -1:
             break;
         }
     }
 
-    void abort() {
+    void abort() override
+    {
         running_ = -1;
-        HAL_DAC_Stop_DMA(&hdac1, DAC_CHANNEL_1);
-        HAL_TIM_Base_Stop(&htim7);
+        stop_conversion();
         ptt_->off();
         pos_ = 0;
+        #ifdef KISS_LOGGING
+            HAL_RCCEx_EnableLSCO(RCC_LSCOSOURCE_LSE);
+        #endif
 
         // Drain the queue.
         while (osMessageGet(dacOutputQueueHandle_, 0).status == osEventMessage);
-    }
+   }
+
+   float bits_per_ms() const override
+   {
+       return 1.2f;
+   }
+
+private:
+   /**
+    * Configure the DAC for timer-based DMA conversion, start the timer,
+    * and start DMA to DAC.
+    */
+   void start_conversion()
+   {
+       DAC_ChannelConfTypeDef sConfig;
+
+       sConfig.DAC_SampleAndHold = DAC_SAMPLEANDHOLD_DISABLE;
+       sConfig.DAC_Trigger = DAC_TRIGGER_T7_TRGO;
+       sConfig.DAC_OutputBuffer = DAC_OUTPUTBUFFER_ENABLE;
+       sConfig.DAC_ConnectOnChipPeripheral = DAC_CHIPCONNECT_ENABLE;
+       sConfig.DAC_UserTrimming = DAC_TRIMMING_FACTORY;
+       if (HAL_DAC_ConfigChannel(&hdac1, &sConfig, DAC_CHANNEL_1) != HAL_OK)
+       {
+         CxxErrorHandler();
+       }
+
+       HAL_TIM_Base_Start(&htim7);
+       HAL_DAC_Start_DMA(
+           &hdac1, DAC_CHANNEL_1,
+           reinterpret_cast<uint32_t*>(buffer_.data()), buffer_.size(),
+           DAC_ALIGN_12B_R);
+   }
+
 };
 
 }} // mobilinkd::tnc
-
-
-#endif // MOBILINKD__TNC__AFSK_MODULATOR_HPP_
diff --git a/TNC/AFSKTestTone.cpp b/TNC/AFSKTestTone.cpp
index 16db756..483bdac 100644
--- a/TNC/AFSKTestTone.cpp
+++ b/TNC/AFSKTestTone.cpp
@@ -3,6 +3,7 @@
 
 #include "AFSKTestTone.hpp"
 #include "ModulatorTask.hpp"
+#include "Modulator.hpp"
 
 void startAfskToneTask(void const* arg)
 {
diff --git a/TNC/Afsk1200Demodulator.cpp b/TNC/Afsk1200Demodulator.cpp
new file mode 100644
index 0000000..a277063
--- /dev/null
+++ b/TNC/Afsk1200Demodulator.cpp
@@ -0,0 +1,227 @@
+// Copyright 2020 Rob Riggs <rob@mobilinkd.com>
+// All rights reserved.
+
+#include "Afsk1200Demodulator.hpp"
+#include "Goertzel.h"
+#include "AudioInput.hpp"
+#include "GPIO.hpp"
+
+namespace mobilinkd { namespace tnc {
+
+afsk1200::emphasis_filter_type Afsk1200Demodulator::filter_1;
+afsk1200::emphasis_filter_type Afsk1200Demodulator::filter_2;
+afsk1200::emphasis_filter_type Afsk1200Demodulator::filter_3;
+
+afsk1200::Demodulator Afsk1200Demodulator::demod1(26400, Afsk1200Demodulator::filter_1);
+afsk1200::Demodulator Afsk1200Demodulator::demod2(26400, Afsk1200Demodulator::filter_2);
+afsk1200::Demodulator Afsk1200Demodulator::demod3(26400, Afsk1200Demodulator::filter_3);
+
+hdlc::IoFrame* Afsk1200Demodulator::operator()(const q15_t* samples)
+{
+    hdlc::IoFrame* result = nullptr;
+
+    q15_t* filtered = demod_filter.filter(const_cast<q15_t* >(samples));
+
+    ++counter;
+
+#if 1
+    auto frame1 = demod1(filtered, ADC_BLOCK_SIZE);
+    if (frame1)
+    {
+        if (frame1->fcs() != last_fcs or counter > last_counter + 2)
+        {
+            last_fcs = frame1->fcs();
+            last_counter = counter;
+            result = frame1;
+        }
+        else
+        {
+            hdlc::release (frame1);
+        }
+    }
+#endif
+
+#if 1
+    auto frame2 = demod2(filtered, ADC_BLOCK_SIZE);
+    if (frame2)
+    {
+        if (frame2->fcs() != last_fcs or counter > last_counter + 2)
+        {
+            last_fcs = frame2->fcs();
+            last_counter = counter;
+            result = frame2;
+        }
+        else
+        {
+            hdlc::release(frame2);
+        }
+    }
+#endif
+
+#if 1
+    auto frame3 = demod3(filtered, ADC_BLOCK_SIZE);
+    if (frame3)
+    {
+        if (frame3->fcs() != last_fcs or counter > last_counter + 2)
+        {
+            last_fcs = frame3->fcs();
+            last_counter = counter;
+            result = frame3;
+        }
+        else
+        {
+            hdlc::release(frame3);
+        }
+    }
+#endif
+    locked_ = demod1.locked() or demod2.locked() or demod3.locked();
+    return result;
+}
+
+/*
+ * Return twist as a the difference in dB between mark and space.  The
+ * expected values are about 0dB for discriminator output and about 5.5dB
+ * for de-emphasized audio.
+ */
+float Afsk1200Demodulator::readTwist()
+{
+    DEBUG("enter Afsk1200Demodulator::readTwist");
+
+    float g1200 = 0.0f;
+    float g2200 = 0.0f;
+
+    GoertzelFilter<ADC_BLOCK_SIZE, SAMPLE_RATE> gf1200(1200.0, 0);
+    GoertzelFilter<ADC_BLOCK_SIZE, SAMPLE_RATE> gf2200(2200.0, 0);
+
+    const uint32_t AVG_SAMPLES = 20;
+
+    startADC(1817, ADC_BLOCK_SIZE);
+
+    for (uint32_t i = 0; i != AVG_SAMPLES; ++i)
+    {
+        uint32_t count = 0;
+        while (count < ADC_BLOCK_SIZE)
+        {
+            osEvent evt = osMessageGet(adcInputQueueHandle, osWaitForever);
+            if (evt.status != osEventMessage)
+                continue;
+
+            auto block = (audio::adc_pool_type::chunk_type*) evt.value.p;
+            uint16_t* data = (uint16_t*) block->buffer;
+            gf1200(data, ADC_BLOCK_SIZE);
+            gf2200(data, ADC_BLOCK_SIZE);
+
+            audio::adcPool.deallocate(block);
+
+            count += ADC_BLOCK_SIZE;
+        }
+
+        g1200 += (gf1200 / count);
+        g2200 += (gf2200 / count);
+
+        gf1200.reset();
+        gf2200.reset();
+    }
+
+    IDemodulator::stopADC();
+
+    g1200 = 10.0f * log10f(g1200 / AVG_SAMPLES);
+    g2200 = 10.0f * log10f(g2200 / AVG_SAMPLES);
+
+    auto result = g1200 - g2200;
+
+    INFO("Twist = %d / 100 (%d - %d)", int(result * 100), int(g1200),
+        int(g2200));
+
+    DEBUG("exit readTwist");
+    return result;
+}
+
+uint32_t Afsk1200Demodulator::readBatteryLevel()
+{
+    DEBUG("enter Afsk1200Demodulator::readBatteryLevel");
+
+    ADC_ChannelConfTypeDef sConfig;
+
+    sConfig.Channel = ADC_CHANNEL_VREFINT;
+    sConfig.Rank = ADC_REGULAR_RANK_1;
+    sConfig.SingleDiff = ADC_SINGLE_ENDED;
+    sConfig.SamplingTime = ADC_SAMPLETIME_92CYCLES_5;
+    sConfig.OffsetNumber = ADC_OFFSET_NONE;
+    sConfig.Offset = 0;
+    if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
+        CxxErrorHandler();
+
+    htim6.Init.Period = 48000;
+    if (HAL_TIM_Base_Init(&htim6) != HAL_OK) CxxErrorHandler();
+
+    if (HAL_TIM_Base_Start(&htim6) != HAL_OK)
+        CxxErrorHandler();
+
+    if (HAL_ADC_Start(&hadc1) != HAL_OK) CxxErrorHandler();
+    if (HAL_ADC_PollForConversion(&hadc1, 3) != HAL_OK) CxxErrorHandler();
+    auto vrefint = HAL_ADC_GetValue(&hadc1);
+    if (HAL_ADC_Stop(&hadc1) != HAL_OK) CxxErrorHandler();
+
+    // Disable battery charging while measuring battery voltage.
+    auto usb_ce = gpio::USB_CE::get();
+    gpio::USB_CE::on();
+
+    gpio::BAT_DIVIDER::off();
+    HAL_Delay(1);
+
+    sConfig.Channel = ADC_CHANNEL_15;
+    if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
+        CxxErrorHandler();
+
+    uint32_t vbat = 0;
+    if (HAL_ADC_Start(&hadc1) != HAL_OK) CxxErrorHandler();
+    for (size_t i = 0; i != 8; ++i)
+    {
+        if (HAL_ADC_PollForConversion(&hadc1, 1) != HAL_OK) CxxErrorHandler();
+        vbat += HAL_ADC_GetValue(&hadc1);
+    }
+
+    vbat /= 8;
+
+    if (HAL_ADC_Stop(&hadc1) != HAL_OK) CxxErrorHandler();
+    if (HAL_TIM_Base_Stop(&htim6) != HAL_OK)
+        CxxErrorHandler();
+
+    gpio::BAT_DIVIDER::on();
+
+    // Restore battery charging state.
+    if (!usb_ce) gpio::USB_CE::off();
+
+    INFO("Vref = %lu", vrefint);
+    INFO("Vbat = %lu (raw)", vbat);
+
+    // Order of operations is important to avoid underflow.
+    vbat *= 6600;
+    vbat /= (VREF + 1);
+
+    uint32_t vref = ((vrefint * 3300) + (VREF / 2)) / VREF;
+
+    INFO("Vref = %lumV", vref)
+    INFO("Vbat = %lumV", vbat);
+
+    DEBUG("exit Afsk1200Demodulator::readBatteryLevel");
+    return vbat;
+}
+
+const q15_t Afsk1200Demodulator::bpf_coeffs[FILTER_TAP_NUM] = {
+    4,     0,    -5,   -10,   -13,   -12,    -9,    -4,    -2,    -4,   -12,   -26,
+  -41,   -52,   -51,   -35,    -3,    39,    83,   117,   131,   118,    83,    36,
+   -6,   -32,   -30,    -3,    36,    67,    66,    19,   -74,  -199,  -323,  -408,
+ -421,  -344,  -187,    17,   218,   364,   417,   369,   247,   106,    14,    26,
+  166,   407,   676,   865,   866,   605,    68,  -675, -1484, -2171, -2547, -2471,
+-1895,  -882,   394,  1692,  2747,  3337,  3337,  2747,  1692,   394,  -882, -1895,
+-2471, -2547, -2171, -1484,  -675,    68,   605,   866,   865,   676,   407,   166,
+   26,    14,   106,   247,   369,   417,   364,   218,    17,  -187,  -344,  -421,
+ -408,  -323,  -199,   -74,    19,    66,    67,    36,    -3,   -30,   -32,    -6,
+   36,    83,   118,   131,   117,    83,    39,    -3,   -35,   -51,   -52,   -41,
+  -26,   -12,    -4,    -2,    -4,    -9,   -12,   -13,   -10,    -5,     0,     4,
+};
+
+}} // mobilinkd::tnc
+
diff --git a/TNC/Afsk1200Demodulator.hpp b/TNC/Afsk1200Demodulator.hpp
new file mode 100644
index 0000000..c78dee1
--- /dev/null
+++ b/TNC/Afsk1200Demodulator.hpp
@@ -0,0 +1,132 @@
+// Copyright 2020 Rob Riggs <rob@mobilinkd.com>
+// All rights reserved.
+
+#pragma once
+
+#include "Demodulator.hpp"
+#include "AfskDemodulator.hpp"
+#include "FirFilter.hpp"
+#include "FilterCoefficients.hpp"
+#include "KissHardware.hpp"
+#include "HdlcFrame.hpp"
+
+namespace mobilinkd { namespace tnc {
+
+struct Afsk1200Demodulator : IDemodulator
+{
+    /*
+     * Generated with Scipy Filter, 152 coefficients, 1100-2300Hz bandpass,
+     * Hann window, starting and ending 0 value coefficients removed.
+     *
+     * np.array(
+     *  firwin2(152,
+     *      [
+     *          0.0,
+     *          1000.0/(sample_rate/2),
+     *          1100.0/(sample_rate/2),
+     *          2350.0/(sample_rate/2),
+     *          2500.0/(sample_rate/2),
+     *          1.0
+     *      ],
+     *      [0,0,1,1,0,0],
+     *      antisymmetric = False,
+     *      window='hann') * 32768,
+     *  dtype=int)[10:-10]
+     */
+    static constexpr size_t FILTER_TAP_NUM = 132;
+    static constexpr uint32_t ADC_BLOCK_SIZE = afsk1200::ADC_BUFFER_SIZE;
+    static_assert(audio::ADC_BUFFER_SIZE >= ADC_BLOCK_SIZE);
+
+    static constexpr uint32_t SAMPLE_RATE = 26400;
+    static constexpr uint16_t VREF = 16383;
+
+    using audio_filter_t = Q15FirFilter<ADC_BLOCK_SIZE, FILTER_TAP_NUM>;
+
+    static const q15_t bpf_coeffs[FILTER_TAP_NUM];
+
+    static afsk1200::emphasis_filter_type filter_1;
+    static afsk1200::emphasis_filter_type filter_2;
+    static afsk1200::emphasis_filter_type filter_3;
+
+    static afsk1200::Demodulator demod1;
+    static afsk1200::Demodulator demod2;
+    static afsk1200::Demodulator demod3;
+
+    audio_filter_t demod_filter;
+    uint16_t last_fcs{0};
+    uint32_t last_counter{0};
+    uint32_t counter{0};
+    bool locked_{false};
+
+    virtual ~Afsk1200Demodulator() {}
+
+    void start() override
+    {
+        INFO("Setting 48MHz SysClock.");
+        SysClock48();
+
+        // rx_twist is 6dB for discriminator input and 0db for de-emphasized input.
+        auto twist = kiss::settings().rx_twist;
+
+        filter_1.init(*filter::fir::AfskFilters[twist + 3]);
+        filter_2.init(*filter::fir::AfskFilters[twist + 6]);
+        filter_3.init(*filter::fir::AfskFilters[twist + 9]);
+
+        last_fcs = 0;
+        last_counter = 0;
+        counter = 0;
+
+        demod_filter.init(bpf_coeffs);
+        passall(kiss::settings().options & KISS_OPTION_PASSALL);
+
+        hadc1.Init.OversamplingMode = ENABLE;
+        if (HAL_ADC_Init(&hadc1) != HAL_OK)
+        {
+            CxxErrorHandler();
+        }
+
+        ADC_ChannelConfTypeDef sConfig;
+
+        sConfig.Channel = AUDIO_IN;
+        sConfig.Rank = ADC_REGULAR_RANK_1;
+        sConfig.SingleDiff = ADC_SINGLE_ENDED;
+        sConfig.SamplingTime = ADC_SAMPLETIME_12CYCLES_5;
+        sConfig.OffsetNumber = ADC_OFFSET_NONE;
+        sConfig.Offset = 0;
+        if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
+            CxxErrorHandler();
+
+        startADC(1817, ADC_BLOCK_SIZE);
+    }
+
+    void stop() override
+    {
+        stopADC();
+        locked_ = false;
+    }
+
+    hdlc::IoFrame* operator()(const q15_t* samples) override;
+
+    float readTwist() override;
+
+    uint32_t readBatteryLevel() override;
+
+    bool locked() const override
+    {
+        return locked_;
+    }
+
+    size_t size() const override
+    {
+        return ADC_BLOCK_SIZE;
+    }
+
+    void passall(bool enabled) override
+    {
+        demod1.hdlc_decoder_.setPassall(enabled);
+        demod2.hdlc_decoder_.setPassall(enabled);
+        demod3.hdlc_decoder_.setPassall(enabled);
+    }
+};
+
+}} // mobilinkd::tnc
diff --git a/TNC/AfskDemodulator.cpp b/TNC/AfskDemodulator.cpp
index e520852..d4dba76 100644
--- a/TNC/AfskDemodulator.cpp
+++ b/TNC/AfskDemodulator.cpp
@@ -1,4 +1,4 @@
-// Copyright 2017-2019 Rob Riggs <rob@mobilinkd.com>
+// Copyright 2017-2020 Rob Riggs <rob@mobilinkd.com>
 // All rights reserved.
 
 #include "AfskDemodulator.hpp"
diff --git a/TNC/AfskDemodulator.hpp b/TNC/AfskDemodulator.hpp
index a3411c0..73e442a 100644
--- a/TNC/AfskDemodulator.hpp
+++ b/TNC/AfskDemodulator.hpp
@@ -31,7 +31,8 @@ const q15_t lpf_coeffs[] = {
    14,    18,    20,    19,    17,    14,    11,     8,     5,     3,     1,     0,
 };
 
-typedef FirFilter<audio::ADC_BUFFER_SIZE, 9> emphasis_filter_type;
+static constexpr uint32_t ADC_BUFFER_SIZE = 88;
+typedef FirFilter<ADC_BUFFER_SIZE, 9> emphasis_filter_type;
 
 struct Demodulator {
 
@@ -47,11 +48,11 @@ struct Demodulator {
     emphasis_filter_type& audio_filter_;
     libafsk::FixedDelayLine<40> delay_line_;
     DPLL pll_;
-    Q15FirFilter<audio::ADC_BUFFER_SIZE, LPF_FILTER_LEN> lpf_filter_;
+    Q15FirFilter<ADC_BUFFER_SIZE, LPF_FILTER_LEN> lpf_filter_;
     libafsk::NRZI nrzi_;
     hdlc::NewDecoder hdlc_decoder_;
     bool locked_;
-    q15_t buffer_[audio::ADC_BUFFER_SIZE];
+    q15_t buffer_[ADC_BUFFER_SIZE];
 
     Demodulator(size_t sample_rate, emphasis_filter_type& c)
     : sample_rate_(sample_rate)
diff --git a/TNC/AudioInput.cpp b/TNC/AudioInput.cpp
index 2bde7e5..d956795 100644
--- a/TNC/AudioInput.cpp
+++ b/TNC/AudioInput.cpp
@@ -2,18 +2,17 @@
 // All rights reserved.
 
 #include "AudioInput.hpp"
-#include "AfskDemodulator.hpp"
+#include "Afsk1200Demodulator.hpp"
+#include "Fsk9600Demodulator.hpp"
 #include "AudioLevel.hpp"
 #include "Log.h"
 #include "KissHardware.hpp"
 #include "GPIO.hpp"
 #include "HdlcFrame.hpp"
-#include "memory.hpp"
-#include "IirFilter.hpp"
-#include "FilterCoefficients.hpp"
 #include "PortInterface.hpp"
 #include "Goertzel.h"
 #include "DCD.h"
+#include "ModulatorTask.hpp"
 
 #include "arm_math.h"
 #include "stm32l4xx_hal.h"
@@ -28,18 +27,13 @@ extern osMessageQId ioEventQueueHandle;
 
 extern "C" void SystemClock_Config(void);
 
-// 1kB
-typedef mobilinkd::tnc::memory::Pool<
-    8, mobilinkd::tnc::audio::ADC_BUFFER_SIZE * 2> adc_pool_type;
-adc_pool_type adcPool;
-
 // DMA Conversion first half complete.
 extern "C" void HAL_ADC_ConvHalfCpltCallback(ADC_HandleTypeDef*) {
     using namespace mobilinkd::tnc::audio;
 
     auto block = adcPool.allocate();
     if (!block) return;
-    memmove(block->buffer, adc_buffer, ADC_BUFFER_SIZE * 2);
+    memmove(block->buffer, adc_buffer, dma_transfer_size);
     auto status = osMessagePut(adcInputQueueHandle, (uint32_t) block, 0);
     if (status != osOK) adcPool.deallocate(block);
 }
@@ -50,7 +44,7 @@ extern "C" void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef*) {
 
     auto block = adcPool.allocate();
     if (!block) return;
-    memmove(block->buffer, adc_buffer + DMA_TRANSFER_SIZE, ADC_BUFFER_SIZE * 2);
+    memmove(block->buffer, adc_buffer + half_buffer_size, dma_transfer_size);
     auto status = osMessagePut(adcInputQueueHandle, (uint32_t) block, 0);
     if (status != osOK) adcPool.deallocate(block);
 }
@@ -120,6 +114,7 @@ extern "C" void startAudioInputTask(void const*) {
         case UPDATE_SETTINGS:
             DEBUG("UPDATE_SETTINGS");
             setAudioInputLevels();
+            updateModulator();
             break;
         case IDLE:
             DEBUG("IDLE");
@@ -132,96 +127,47 @@ extern "C" void startAudioInputTask(void const*) {
 
 namespace mobilinkd { namespace tnc { namespace audio {
 
-/*
- * Generated with Scipy Filter, 152 coefficients, 1100-2300Hz bandpass,
- * Hann window, starting and ending 0 value coefficients removed.
- *
- * np.array(
- *  firwin2(152,
- *      [
- *          0.0,
- *          1000.0/(sample_rate/2),
- *          1100.0/(sample_rate/2),
- *          2350.0/(sample_rate/2),
- *          2500.0/(sample_rate/2),
- *          1.0
- *      ],
- *      [0,0,1,1,0,0],
- *      antisymmetric = False,
- *      window='hann') * 32768,
- *  dtype=int)[10:-10]
- */
-constexpr size_t FILTER_TAP_NUM = 132;
-const q15_t bpf_coeffs[] = {
-    4,     0,    -5,   -10,   -13,   -12,    -9,    -4,    -2,    -4,   -12,   -26,
-  -41,   -52,   -51,   -35,    -3,    39,    83,   117,   131,   118,    83,    36,
-   -6,   -32,   -30,    -3,    36,    67,    66,    19,   -74,  -199,  -323,  -408,
- -421,  -344,  -187,    17,   218,   364,   417,   369,   247,   106,    14,    26,
-  166,   407,   676,   865,   866,   605,    68,  -675, -1484, -2171, -2547, -2471,
--1895,  -882,   394,  1692,  2747,  3337,  3337,  2747,  1692,   394,  -882, -1895,
--2471, -2547, -2171, -1484,  -675,    68,   605,   866,   865,   676,   407,   166,
-   26,    14,   106,   247,   369,   417,   364,   218,    17,  -187,  -344,  -421,
- -408,  -323,  -199,   -74,    19,    66,    67,    36,    -3,   -30,   -32,    -6,
-   36,    83,   118,   131,   117,    83,    39,    -3,   -35,   -51,   -52,   -41,
-  -26,   -12,    -4,    -2,    -4,    -9,   -12,   -13,   -10,    -5,     0,     4,
-};
+uint32_t adc_buffer[ADC_BUFFER_SIZE];               // Two samples per element.
+volatile uint32_t adc_block_size = ADC_BUFFER_SIZE;          // Based on demodulator.
+volatile uint32_t dma_transfer_size = adc_block_size * 2;    // Transfer size in bytes.
+volatile uint32_t half_buffer_size = adc_block_size / 2;     // Transfer size in words / 2.
+adc_pool_type adcPool;
 
-uint32_t adc_buffer[ADC_BUFFER_SIZE];       // Two samples per element.
-
-typedef Q15FirFilter<ADC_BUFFER_SIZE, FILTER_TAP_NUM> audio_filter_type;
-
-audio_filter_type audio_filter;
-
-mobilinkd::tnc::afsk1200::emphasis_filter_type filter_1;
-mobilinkd::tnc::afsk1200::emphasis_filter_type filter_2;
-mobilinkd::tnc::afsk1200::emphasis_filter_type filter_3;
-
-mobilinkd::tnc::afsk1200::Demodulator& getDemod1(const TFirCoefficients<9>& f) __attribute__((noinline));
-mobilinkd::tnc::afsk1200::Demodulator& getDemod2(const TFirCoefficients<9>& f) __attribute__((noinline));
-mobilinkd::tnc::afsk1200::Demodulator& getDemod3(const TFirCoefficients<9>& f) __attribute__((noinline));
-
-mobilinkd::tnc::afsk1200::Demodulator& getDemod1(const TFirCoefficients<9>& f) {
-    filter_1.init(f);
-    static mobilinkd::tnc::afsk1200::Demodulator instance(26400, filter_1);
-    return instance;
-}
-
-mobilinkd::tnc::afsk1200::Demodulator& getDemod2(const TFirCoefficients<9>& f) {
-    filter_2.init(f);
-    static mobilinkd::tnc::afsk1200::Demodulator instance(26400, filter_2);
-    return instance;
-}
-
-mobilinkd::tnc::afsk1200::Demodulator& getDemod3(const TFirCoefficients<9>& f) {
-    filter_3.init(f);
-    static mobilinkd::tnc::afsk1200::Demodulator instance(26400, filter_3);
-    return instance;
+void set_adc_block_size(uint32_t block_size)
+{
+    adc_block_size = block_size;
+    dma_transfer_size = block_size * 2;
+    half_buffer_size = block_size / 2;
 }
 
 q15_t normalized[ADC_BUFFER_SIZE];
 
+
+IDemodulator* getDemodulator()
+{
+    static Afsk1200Demodulator afsk1200;
+    static Fsk9600Demodulator fsk9600;
+
+    switch (kiss::settings().modem_type)
+    {
+    case kiss::Hardware::ModemType::AFSK1200:
+        return &afsk1200;
+    case kiss::Hardware::ModemType::FSK9600:
+        return &fsk9600;
+    default:
+        ERROR("Invalid demodulator");
+        CxxErrorHandler();
+    }
+}
+
 void demodulatorTask() {
 
     DEBUG("enter demodulatorTask");
 
-    audio_filter.init(bpf_coeffs);
-
-    // rx_twist is 6dB for discriminator input and 0db for de-emphasized input.
-    auto twist = kiss::settings().rx_twist;
-
-    mobilinkd::tnc::afsk1200::Demodulator& demod1 = getDemod1(*filter::fir::AfskFilters[twist + 3]);
-    mobilinkd::tnc::afsk1200::Demodulator& demod2 = getDemod2(*filter::fir::AfskFilters[twist + 6]);
-    mobilinkd::tnc::afsk1200::Demodulator& demod3 = getDemod3(*filter::fir::AfskFilters[twist + 9]);
-
-    startADC(AUDIO_IN);
-
-    mobilinkd::tnc::hdlc::IoFrame* frame = 0;
-
-    uint16_t last_fcs = 0;
-    uint32_t last_counter = 0;
-    uint32_t counter = 0;
-
     bool dcd_status{false};
+    auto demodulator = getDemodulator();
+
+    demodulator->start();
 
     while (true) {
         osEvent peek = osMessagePeek(audioInputQueueHandle, 0);
@@ -232,71 +178,24 @@ void demodulatorTask() {
             continue;
         }
 
-        ++counter;
-
         auto block = (adc_pool_type::chunk_type*) evt.value.p;
         auto samples = (int16_t*) block->buffer;
 
-        arm_offset_q15(samples, 0 - virtual_ground, normalized, ADC_BUFFER_SIZE);
+        arm_offset_q15(samples, 0 - virtual_ground, normalized, demodulator->size());
         adcPool.deallocate(block);
-        q15_t* audio = audio_filter(normalized);
 
-#if 1
-        frame = demod1(audio, ADC_BUFFER_SIZE);
-        if (frame) {
-            if (frame->fcs() != last_fcs or counter > last_counter + 2) {
-                auto save_fcs = frame->fcs();
-                if (osMessagePut(ioEventQueueHandle, (uint32_t) frame, 1) == osOK) {
-                  last_fcs = save_fcs;
-                  last_counter = counter;
-                } else {
-                  hdlc::release(frame);
-                }
-            }
-            else {
+        auto frame = (*demodulator)(normalized);
+        if (frame)
+        {
+            frame->source(hdlc::IoFrame::RF_DATA);
+            if (osMessagePut(ioEventQueueHandle, (uint32_t) frame, 1) != osOK)
+            {
                 hdlc::release(frame);
             }
         }
-#endif
 
-#if 1
-        frame = demod2(audio, ADC_BUFFER_SIZE);
-        if (frame) {
-          if (frame->fcs() != last_fcs or counter > last_counter + 2) {
-              auto save_fcs = frame->fcs();
-              if (osMessagePut(ioEventQueueHandle, (uint32_t) frame, 1) == osOK) {
-                last_fcs = save_fcs;
-                last_counter = counter;
-              } else {
-                hdlc::release(frame);
-              }
-          }
-          else {
-              hdlc::release(frame);
-          }
-        }
-#endif
-
-#if 1
-        frame = demod3(audio, ADC_BUFFER_SIZE);
-        if (frame) {
-          if (frame->fcs() != last_fcs or counter > last_counter + 2) {
-              auto save_fcs = frame->fcs();
-              if (osMessagePut(ioEventQueueHandle, (uint32_t) frame, 1) == osOK) {
-                last_fcs = save_fcs;
-                last_counter = counter;
-              } else {
-                hdlc::release(frame);
-              }
-          }
-          else {
-              hdlc::release(frame);
-          }
-        }
-#endif
-        bool new_dcd_status = demod1.locked() or demod2.locked() or demod3.locked();
-        if (new_dcd_status xor dcd_status) {
-            dcd_status = new_dcd_status;
+        if (demodulator->locked() xor dcd_status) {
+            dcd_status = demodulator->locked();
             if (dcd_status) {
                 dcd_on();
             } else {
@@ -305,19 +204,22 @@ void demodulatorTask() {
         }
     }
 
-    stopADC();
+    demodulator->stop();
+
     dcd_off();
     DEBUG("exit demodulatorTask");
 }
 
 
-void streamLevels(uint32_t channel, uint8_t cmd) {
+void streamLevels(uint8_t cmd) {
 
     // Stream out Vpp, Vavg, Vmin, Vmax as four 16-bit values, left justified.
 
     uint8_t data[9];
     INFO("streamLevels: start");
-    startADC(channel);
+
+    auto demodulator = getDemodulator();
+    demodulator->start();
 
     while (true) {
         osEvent peek = osMessagePeek(audioInputQueueHandle, 0);
@@ -328,15 +230,15 @@ void streamLevels(uint32_t channel, uint8_t cmd) {
         uint16_t vmin = std::numeric_limits<uint16_t>::max();
         uint16_t vmax = std::numeric_limits<uint16_t>::min();
 
-        while (count < 2640) {
+        while (count < demodulator->size() * 30) {
             osEvent evt = osMessageGet(adcInputQueueHandle, osWaitForever);
             if (evt.status != osEventMessage) continue;
 
-            count += ADC_BUFFER_SIZE;
+            count += demodulator->size();
 
             auto block = (adc_pool_type::chunk_type*) evt.value.p;
             auto start =  (uint16_t*) block->buffer;
-            auto end = start + ADC_BUFFER_SIZE;
+            auto end = start + demodulator->size();
 
             vmin = std::min(vmin, *std::min_element(start, end));
             vmax = std::max(vmax, *std::max_element(start, end));
@@ -363,47 +265,52 @@ void streamLevels(uint32_t channel, uint8_t cmd) {
         ioport->write(data, 9, 6, 10);
     }
 
-    stopADC();
+    demodulator->stop();
     DEBUG("exit streamLevels");
 }
 
-levels_type readLevels(uint32_t channel, uint32_t samples) {
+levels_type readLevels(uint32_t)
+{
 
     DEBUG("enter readLevels");
 
     // Return Vpp, Vavg, Vmin, Vmax as four 16-bit values, right justified.
 
-    uint16_t count = 0;
+    uint32_t BLOCKS = 30;
     uint32_t accum = 0;
+    uint32_t iaccum = 0;
     uint16_t vmin = std::numeric_limits<uint16_t>::max();
     uint16_t vmax = std::numeric_limits<uint16_t>::min();
 
     INFO("readLevels: start");
-    startADC(channel);
-
-    while (count < samples) {
+    auto demodulator = getDemodulator();
+    demodulator->start();
 
+    for (uint32_t count = 0; count != BLOCKS; ++count)
+    {
         osEvent evt = osMessageGet(adcInputQueueHandle, osWaitForever);
         if (evt.status != osEventMessage) continue;
 
         auto block = (adc_pool_type::chunk_type*) evt.value.p;
         auto start =  (uint16_t*) block->buffer;
-        auto end = start + ADC_BUFFER_SIZE;
+        auto end = start + demodulator->size();
 
         vmin = std::min(vmin, *std::min_element(start, end));
         vmax = std::max(vmax, *std::max_element(start, end));
         accum = std::accumulate(start, end, accum);
 
+        iaccum += (accum / demodulator->size());
+
         adcPool.deallocate(block);
 
-        count += ADC_BUFFER_SIZE;
+        accum = 0;
     }
 
-    stopADC();
+    demodulator->stop();
 
     uint16_t pp = vmax - vmin;
-    uint16_t avg = accum / count;
-    DEBUG("exit readLevels");
+    uint16_t avg = iaccum / BLOCKS;
+    INFO("exit readLevels");
 
     return levels_type(pp, avg, vmin, vmax);
 }
@@ -421,56 +328,7 @@ constexpr uint32_t TWIST_SAMPLE_SIZE = 88;
  */
 float readTwist()
 {
-
-  DEBUG("enter readTwist");
-  constexpr uint32_t channel = AUDIO_IN;
-
-  float g1200 = 0.0f;
-  float g2200 = 0.0f;
-
-  GoertzelFilter<TWIST_SAMPLE_SIZE, SAMPLE_RATE> gf1200(1200.0, 0);
-  GoertzelFilter<TWIST_SAMPLE_SIZE, SAMPLE_RATE> gf2200(2200.0, 0);
-
-  const uint32_t AVG_SAMPLES = 20;
-
-  startADC(channel);
-
-  for (uint32_t i = 0; i != AVG_SAMPLES; ++i)
-  {
-    uint32_t count = 0;
-    while (count < TWIST_SAMPLE_SIZE)
-    {
-        osEvent evt = osMessageGet(adcInputQueueHandle, osWaitForever);
-        if (evt.status != osEventMessage) continue;
-
-        auto block = (adc_pool_type::chunk_type*) evt.value.p;
-        uint16_t* data =  (uint16_t*) block->buffer;
-        gf1200(data, ADC_BUFFER_SIZE);
-        gf2200(data, ADC_BUFFER_SIZE);
-
-        adcPool.deallocate(block);
-
-        count += ADC_BUFFER_SIZE;
-    }
-
-    g1200 += (gf1200 / count);
-    g2200 += (gf2200 / count);
-
-    gf1200.reset();
-    gf2200.reset();
-  }
-
-  stopADC();
-
-  g1200 = 10.0f * log10f(g1200 / AVG_SAMPLES);
-  g2200 = 10.0f * log10f(g2200 / AVG_SAMPLES);
-
-  auto result = g1200 - g2200;
-
-  INFO("Twist = %d / 100 (%d - %d)", int(result*100), int(g1200), int(g2200));
-
-  DEBUG("exit readTwist");
-  return result;
+    return getDemodulator()->readTwist();
 }
 
 /*
@@ -497,7 +355,6 @@ float readTwist()
 void pollInputTwist()
 {
     DEBUG("enter pollInputTwist");
-    constexpr uint32_t channel = AUDIO_IN;
 
     float g1200 = 0.0f;
     float g2200 = 0.0f;
@@ -507,7 +364,7 @@ void pollInputTwist()
 
     const uint32_t AVG_SAMPLES = 100;
 
-    startADC(channel);
+    IDemodulator::startADC(3029, TWIST_SAMPLE_SIZE);
 
     for (uint32_t i = 0; i != AVG_SAMPLES; ++i) {
 
@@ -534,7 +391,7 @@ void pollInputTwist()
       gf2200.reset();
     }
 
-    stopADC();
+    IDemodulator::stopADC();
 
     DEBUG("pollInputTwist: MARK=%d, SPACE=%d (x100)",
       int(g1200 * 100.0 / AVG_SAMPLES), int(g2200 * 100.0 / AVG_SAMPLES));
@@ -554,155 +411,9 @@ void pollInputTwist()
     DEBUG("exit pollInputTwist");
 }
 
-#if 0
-void streamAverageInputTwist()
-{
-    DEBUG("enter streamAverageInputTwist");
-
-    constexpr uint32_t channel = AUDIO_IN;
-
-    startADC(channel);
-
-    uint32_t acount = 0;
-    float g700 = 0.0f;
-    float g1200 = 0.0f;
-    float g1700 = 0.0f;
-    float g2200 = 0.0f;
-    float g2700 = 0.0f;
-
-    GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf700(700.0);
-    GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf1200(1200.0);
-    GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf1700(1700.0);
-    GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf2200(2200.0);
-    GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf2700(2700.0);
-
-    while (true) {
-      osEvent peek = osMessagePeek(audioInputQueueHandle, 0);
-      if (peek.status == osEventMessage) break;
-
-      acount++;
-      uint32_t count = 0;
-      while (count < TWIST_SAMPLE_SIZE) {
-
-          osEvent evt = osMessageGet(adcInputQueueHandle, osWaitForever);
-          if (evt.status != osEventMessage) continue;
-
-          count += ADC_BUFFER_SIZE;
-
-          auto block = (adc_pool_type::chunk_type*) evt.value.v;
-          uint16_t* data =  (uint16_t*) block->buffer;
-          gf700(data, ADC_BUFFER_SIZE);
-          gf1200(data, ADC_BUFFER_SIZE);
-          gf1700(data, ADC_BUFFER_SIZE);
-          gf2200(data, ADC_BUFFER_SIZE);
-          gf2700(data, ADC_BUFFER_SIZE);
-
-          adcPool.deallocate(block);
-      }
-
-      g700 += 10.0 * log10(gf700);
-      g1200 += 10.0 * log10(gf1200);
-      g1700 += 10.0 * log10(gf1700);
-      g2200 += 10.0 * log10(gf2200);
-      g2700 += 10.0 * log10(gf2700);
-
-      char* buffer = 0;
-      // @TODO: Make re-entrant printf work (or convert to fixed-point).
-      int len = asiprintf_r(
-        &buffer,
-        "_%f, %f, %f, %f, %f\r\n",
-        g700 / acount,
-        g1200 / acount,
-        g1700 / acount,
-        g2200 / acount,
-        g2700 / acount);
-
-      if (len > 0) {
-        buffer[0] = kiss::hardware::POLL_INPUT_TWIST;
-        ioport->write((uint8_t*)buffer, len - 1, 6, 10);
-        free(buffer);
-      }
-
-      gf700.reset();
-      gf1200.reset();
-      gf1700.reset();
-      gf2200.reset();
-      gf2700.reset();
-    }
-
-    stopADC();
-    DEBUG("exit streamAverageInputTwist");
-}
-
-void streamInstantInputTwist()
-{
-    DEBUG("enter streamInstantInputTwist");
-
-    constexpr uint32_t channel = AUDIO_IN;
-
-    startADC(channel);
-
-    GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf700(700.0);
-    GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf1200(1200.0);
-    GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf1700(1700.0);
-    GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf2200(2200.0);
-    GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf2700(2700.0);
-
-    while (true) {
-      osEvent peek = osMessagePeek(audioInputQueueHandle, 0);
-      if (peek.status == osEventMessage) break;
-
-      uint32_t count = 0;
-      while (count < TWIST_SAMPLE_SIZE) {
-
-          osEvent evt = osMessageGet(adcInputQueueHandle, osWaitForever);
-          if (evt.status != osEventMessage) continue;
-
-          count += ADC_BUFFER_SIZE;
-
-          auto block = (adc_pool_type::chunk_type*) evt.value.v;
-          uint16_t* data =  (uint16_t*) block->buffer;
-          gf700(data, ADC_BUFFER_SIZE);
-          gf1200(data, ADC_BUFFER_SIZE);
-          gf1700(data, ADC_BUFFER_SIZE);
-          gf2200(data, ADC_BUFFER_SIZE);
-          gf2700(data, ADC_BUFFER_SIZE);
-
-          adcPool.deallocate(block);
-      }
-
-      char* buffer = 0;
-      // @TODO: Make re-entrant printf work (or convert to fixed-point).
-      int len = asiprintf_r(
-        &buffer,
-        "_%f, %f, %f, %f, %f\r\n",
-        10.0 * log10(gf700),
-        10.0 * log10(gf1200),
-        10.0 * log10(gf1700),
-        10.0 * log10(gf2200),
-        10.0 * log10(gf2700));
-
-      if (len > 0) {
-        buffer[0] = kiss::hardware::POLL_INPUT_TWIST;
-        ioport->write((uint8_t*)buffer, len - 1, 6, 10);
-        free(buffer);
-      }
-
-      gf700.reset();
-      gf1200.reset();
-      gf1700.reset();
-      gf2200.reset();
-      gf2700.reset();
-    }
-
-    stopADC();
-    DEBUG("exit streamInstantInputTwist");
-}
-#endif
-
 void streamAmplifiedInputLevels() {
     DEBUG("enter streamAmplifiedInputLevels");
-    streamLevels(AUDIO_IN, kiss::hardware::POLL_INPUT_LEVEL);
+    streamLevels(kiss::hardware::POLL_INPUT_LEVEL);
     DEBUG("exit streamAmplifiedInputLevels");
 }
 
@@ -732,72 +443,9 @@ void pollAmplifiedInputLevel() {
     DEBUG("exit pollAmplifiedInputLevel");
 }
 
-void pollBatteryLevel() {
-    DEBUG("enter pollBatteryLevel");
-
-    ADC_ChannelConfTypeDef sConfig;
-
-    sConfig.Channel = ADC_CHANNEL_VREFINT;
-    sConfig.Rank = ADC_REGULAR_RANK_1;
-    sConfig.SingleDiff = ADC_SINGLE_ENDED;
-    sConfig.SamplingTime = ADC_SAMPLETIME_247CYCLES_5;
-    sConfig.OffsetNumber = ADC_OFFSET_NONE;
-    sConfig.Offset = 0;
-    if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
-        CxxErrorHandler();
-
-    htim6.Init.Period = 48000;
-    if (HAL_TIM_Base_Init(&htim6) != HAL_OK) CxxErrorHandler();
-
-    if (HAL_TIM_Base_Start(&htim6) != HAL_OK)
-        CxxErrorHandler();
-
-    if (HAL_ADC_Start(&hadc1) != HAL_OK) CxxErrorHandler();
-    if (HAL_ADC_PollForConversion(&hadc1, 3) != HAL_OK) CxxErrorHandler();
-    auto vrefint = HAL_ADC_GetValue(&hadc1);
-    if (HAL_ADC_Stop(&hadc1) != HAL_OK) CxxErrorHandler();
-
-    // Disable battery charging while measuring battery voltage.
-    auto usb_ce = gpio::USB_CE::get();
-    gpio::USB_CE::on();
-
-    gpio::BAT_DIVIDER::off();
-    HAL_Delay(1);
-
-    sConfig.Channel = ADC_CHANNEL_15;
-    if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
-        CxxErrorHandler();
-
-    uint32_t vbat = 0;
-    if (HAL_ADC_Start(&hadc1) != HAL_OK) CxxErrorHandler();
-    for (size_t i = 0; i != 8; ++i)
-    {
-        if (HAL_ADC_PollForConversion(&hadc1, 1) != HAL_OK) CxxErrorHandler();
-        vbat += HAL_ADC_GetValue(&hadc1);
-    }
-
-    vbat /= 8;
-
-    if (HAL_ADC_Stop(&hadc1) != HAL_OK) CxxErrorHandler();
-    if (HAL_TIM_Base_Stop(&htim6) != HAL_OK)
-        CxxErrorHandler();
-
-    htim6.Init.Period = 1817;
-    if (HAL_TIM_Base_Init(&htim6) != HAL_OK) CxxErrorHandler();
-
-    HAL_Delay(1);
-
-    gpio::BAT_DIVIDER::on();
-    if (!usb_ce) gpio::USB_CE::off();   // Restore battery charging state.
-
-    INFO("Vref = %lu", vrefint);
-    INFO("Vbat = %lu (raw)", vbat);
-
-    // Order of operations is important to avoid underflow.
-    vbat *= 6600;
-    vbat /= (vref + 1);
-
-    INFO("Vbat = %lumV", vbat);
+void pollBatteryLevel()
+{
+    auto vbat = getDemodulator()->readBatteryLevel();
 
     uint8_t data[3];
     data[0] = kiss::hardware::GET_BATTERY_LEVEL;
@@ -805,7 +453,6 @@ void pollBatteryLevel() {
     data[2] = (vbat & 0xFF);
 
     ioport->write(data, 3, 6, 10);
-    DEBUG("exit pollBatteryLevel");
 }
 
 #if 0
diff --git a/TNC/AudioInput.hpp b/TNC/AudioInput.hpp
index 7b186b3..0dcf990 100644
--- a/TNC/AudioInput.hpp
+++ b/TNC/AudioInput.hpp
@@ -4,6 +4,8 @@
 #ifndef MOBILINKD__TNC__AUDIO__INPUT_HPP_
 #define MOBILINKD__TNC__AUDIO__INPUT_HPP_
 
+#include "memory.hpp"
+
 #include "main.h"
 #include "stm32l4xx_hal.h"
 #include "cmsis_os.h"
@@ -83,10 +85,19 @@ enum AdcState {
     STREAM_INSTANT_TWIST_LEVEL
 };
 
-const size_t ADC_BUFFER_SIZE = 88;
-const size_t DMA_TRANSFER_SIZE = ADC_BUFFER_SIZE / 2;
+const size_t ADC_BUFFER_SIZE = 384;
 extern uint32_t adc_buffer[];       // Two int16_t samples per element.
+extern volatile uint32_t adc_block_size;
+extern volatile uint32_t dma_transfer_size;
+extern volatile uint32_t half_buffer_size;
 
+// 1kB
+typedef memory::Pool<8, ADC_BUFFER_SIZE * 2> adc_pool_type;
+extern adc_pool_type adcPool;
+
+void set_adc_block_size(uint32_t block_size);
+
+#if 0
 inline void stopADC() {
     if (HAL_ADC_Stop_DMA(&hadc1) != HAL_OK)
         CxxErrorHandler();
@@ -118,10 +129,11 @@ inline void restartADC() {
     if (HAL_ADC_Start_DMA(&hadc1, adc_buffer, ADC_BUFFER_SIZE * 2) != HAL_OK)
         CxxErrorHandler();
 }
+#endif
 
 /// Vpp, Vavg, Vmin, Vmax
 typedef std::tuple<uint16_t, uint16_t, uint16_t, uint16_t> levels_type;
-levels_type readLevels(uint32_t channel, uint32_t samples = 2640);
+levels_type readLevels(uint32_t channel);
 float readTwist();
 
 void demodulatorTask();
diff --git a/TNC/AudioLevel.cpp b/TNC/AudioLevel.cpp
index ea7f3f4..5c8ad92 100644
--- a/TNC/AudioLevel.cpp
+++ b/TNC/AudioLevel.cpp
@@ -5,6 +5,7 @@
 #include "AudioInput.hpp"
 #include "KissHardware.hpp"
 #include "ModulatorTask.hpp"
+#include "Modulator.hpp"
 #include "GPIO.hpp"
 #include "LEDIndicator.h"
 #include "ModulatorTask.hpp"
@@ -13,6 +14,8 @@
 #include "stm32l4xx_hal.h"
 
 #include <algorithm>
+#include <tuple>
+#include <array>
 #include <cstdlib>
 #include <cstdint>
 
@@ -199,7 +202,7 @@ void setAudioOutputLevel()
   } else {
       gpio::AUDIO_OUT_ATTEN::off();
   }
-  getModulator().set_volume(r);
+  getModulator().set_gain(r);
 }
 
 }}} // mobilinkd::tnc::audio
diff --git a/TNC/AudioLevel.hpp b/TNC/AudioLevel.hpp
index 1b0aeaa..6f7533d 100644
--- a/TNC/AudioLevel.hpp
+++ b/TNC/AudioLevel.hpp
@@ -28,7 +28,7 @@ void setAudioInputLevels();
 void setAudioOutputLevel();
 
 extern bool streamInputDCOffset;
-constexpr const uint16_t vref = 16383; // Must match ADC output (adjust when oversampling)
+constexpr const uint16_t vref = 4095; // Must match ADC output (adjust when oversampling)
 extern uint16_t virtual_ground;
 extern float i_vgnd;
 
diff --git a/TNC/DCD.cpp b/TNC/DCD.cpp
index f1b413e..9dc3733 100644
--- a/TNC/DCD.cpp
+++ b/TNC/DCD.cpp
@@ -3,6 +3,7 @@
 
 #include "DCD.h"
 #include "LEDIndicator.h"
+#include "GPIO.hpp"
 
 bool& dcd_status()
 {
diff --git a/TNC/Demodulator.cpp b/TNC/Demodulator.cpp
new file mode 100644
index 0000000..06311c4
--- /dev/null
+++ b/TNC/Demodulator.cpp
@@ -0,0 +1,52 @@
+// Copyright 2020 Rob Riggs <rob@mobilinkd.com>
+// All rights reserved.
+
+#include "Demodulator.hpp"
+
+namespace mobilinkd { namespace tnc {
+
+/**
+ * Start the ADC DMA transfer.  The block size is equal to the number of
+ * 32-bit elements in the buffer.  This is also equal to the number of
+ * 16-bit elements in each DMA "half complete" transfer.  Each DMA transfer
+ * results in block_size * 2 bytes being transferred.
+ *
+ * We must bear in mind that the DMA buffer size is expressed in DWORDs,
+ * the DMA transfer size is expressed in WORDs, and the memory copy operation
+ * in BYTEs.
+ *
+ * @param period
+ * @param block_size
+ */
+void IDemodulator::startADC(uint32_t period, uint32_t block_size)
+{
+    audio::set_adc_block_size(block_size);
+
+    htim6.Init.Period = period;
+    if (HAL_TIM_Base_Init(&htim6) != HAL_OK)
+    {
+        CxxErrorHandler();
+    }
+
+    if (HAL_TIM_Base_Start(&htim6) != HAL_OK)
+    {
+        CxxErrorHandler();
+    }
+
+    if (HAL_ADC_Start_DMA(&hadc1, audio::adc_buffer,
+        audio::dma_transfer_size) != HAL_OK)
+    {
+        CxxErrorHandler();
+    }
+}
+
+void IDemodulator::stopADC()
+{
+    if (HAL_ADC_Stop_DMA(&hadc1) != HAL_OK)
+        CxxErrorHandler();
+    if (HAL_TIM_Base_Stop(&htim6) != HAL_OK)
+        CxxErrorHandler();
+}
+
+
+}} // mobilinkd::tnc
diff --git a/TNC/Demodulator.hpp b/TNC/Demodulator.hpp
new file mode 100644
index 0000000..6a5a926
--- /dev/null
+++ b/TNC/Demodulator.hpp
@@ -0,0 +1,49 @@
+// Copyright 2020 Rob Riggs <rob@mobilinkd.com>
+// All rights reserved.
+
+#pragma once
+
+#include "HdlcFrame.hpp"
+#include "FirFilter.hpp"
+#include "AudioInput.hpp"
+
+#include <functional>
+
+#include <arm_math.h>
+
+namespace mobilinkd { namespace tnc {
+
+constexpr size_t FILTER_TAP_NUM = 132;
+
+using demod_filter_t = std::function<q15_t*(q15_t*, size_t)>;
+using demodulator_t = std::function<hdlc::IoFrame*(q15_t*)>;
+
+struct IDemodulator
+{
+    virtual void start() = 0;
+    virtual void stop() = 0;
+    virtual hdlc::IoFrame* operator()(const q15_t* samples) = 0;
+    virtual float readTwist() = 0;
+    virtual uint32_t readBatteryLevel() = 0;
+
+    virtual bool locked() const = 0;
+    virtual size_t size() const = 0;
+
+    /**
+     * Tell the demodulator to return all "passable" HDLC frames.  These
+     * are frames which consist of an even multiple of eight bits and are
+     * up to 330 bytes, but which do not have a valid checksum.
+     *
+     * @param enabled is true when enabled and false when disabled.  The
+     *  default state is disabled.
+     */
+    virtual void passall(bool enabled) = 0;
+
+    virtual ~IDemodulator() {}
+
+    static void startADC(uint32_t period, uint32_t block_size);
+
+    static void stopADC();
+};
+
+}} // mobilinkd::tnc
diff --git a/TNC/DigitalPLL.hpp b/TNC/DigitalPLL.hpp
index 802dd28..5f3b8b2 100644
--- a/TNC/DigitalPLL.hpp
+++ b/TNC/DigitalPLL.hpp
@@ -1,5 +1,7 @@
-#ifndef MOBILINKD__DIGITAL_PLL_H_
-#define MOBILINKD__DIGITAL_PLL_H_
+// Copyright 2015-2020 Mobilinkd LLC <rob@mobilinkd.com>
+// All rights reserved.
+
+#pragma once
 
 #include "Hysteresis.hpp"
 #include "IirFilter.hpp"
@@ -29,10 +31,10 @@ struct PLLResult {
 // scipy.signal:
 //      b, a = bessel(4, [80.0/(1200/2)], 'lowpass')
 //
-const std::array<float, 5> lock_b = {
+constexpr std::array<float, 5> lock_b = {
     1.077063e-03,4.308253e-03,6.462379e-03,4.308253e-03,1.077063e-03,
 };
-const std::array<float, 5> lock_a = {
+constexpr std::array<float, 5> lock_a = {
     1.000000e+00,-2.774567e+00,2.962960e+00,-1.437990e+00,2.668296e-01,
 };
 
@@ -41,7 +43,7 @@ const std::array<float, 5> lock_a = {
 //      loop_coeffs = firwin(9, [64.0/(1200/2)], width = None,
 //          pass_zero = True, scale = True, window='hann')
 //
-const std::array<float, 7> loop_coeffs = {
+constexpr std::array<float, 7> loop_coeffs = {
 //     0.08160962754214955, 0.25029850550446403, 0.3361837339067726, 0.2502985055044641, 0.08160962754214969
     3.196252e-02,1.204223e-01,2.176819e-01,2.598666e-01,2.176819e-01,1.204223e-01,3.196252e-02
 };
@@ -59,7 +61,7 @@ struct BaseDigitalPLL
 	float_type sps_; 			///< Samples per symbol
 	float_type limit_;			///< Samples per symbol / 2
 	libafsk::BaseHysteresis<float_type> lock_;
-    FirFilter<1, 7> loop_filter_{pll::loop_coeffs.begin()};
+    FirFilter<1, pll::loop_coeffs.size()> loop_filter_{pll::loop_coeffs.begin()};
     IirFilter<5> lock_filter_{pll::lock_b, pll::lock_a};
 
 	bool last_;
@@ -72,8 +74,8 @@ struct BaseDigitalPLL
 	BaseDigitalPLL(float_type sample_rate, float_type symbol_rate)
 	: sample_rate_(sample_rate), symbol_rate_(symbol_rate)
 	, sps_(sample_rate / symbol_rate)
-	, limit_(sps_ / float_type(2.0))
-    , lock_(sps_ * float_type(0.03), sps_ * float_type(0.15), 1, 0)
+    , limit_(sps_ / 2.0)
+    , lock_(sps_ * 0.03, sps_ * 0.15, 1, 0)
 	, last_(false), count_(0), sample_(false)
 	, jitter_(0.0), bits_(1)
 	{}
@@ -127,6 +129,3 @@ typedef BaseDigitalPLL<double> DigitalPLL;
 typedef BaseDigitalPLL<float> FastDigitalPLL;
 
 }} // mobilinkd::tnc
-
-#endif // MOBILINKD__DIGITAL_PLL_H_
-
diff --git a/TNC/FilterCoefficients.cpp b/TNC/FilterCoefficients.cpp
new file mode 100644
index 0000000..d335794
--- /dev/null
+++ b/TNC/FilterCoefficients.cpp
@@ -0,0 +1,35 @@
+// Copyright 2020 Rob Riggs <rob@mobilinkd.com>
+// All rights reserved.
+
+#include "FilterCoefficients.hpp"
+
+namespace mobilinkd { namespace tnc { namespace filter {
+
+namespace fir {
+
+const TFirCoefficients<9>* AfskFilters[19] = {
+  &dB_6,
+  &dB_5,
+  &dB_4,
+  &dB_3,
+  &dB_2,
+  &dB_1,
+  &dB0,
+  &dB1,
+  &dB2,
+  &dB3,
+  &dB4,
+  &dB5,
+  &dB6,
+  &dB7,
+  &dB8,
+  &dB9,
+  &dB10,
+  &dB11,
+  &dB12
+};
+
+} // fir
+
+
+}}} // mobilinkd::tnc::filter
diff --git a/TNC/FilterCoefficients.hpp b/TNC/FilterCoefficients.hpp
index 5711dfc..e24466a 100644
--- a/TNC/FilterCoefficients.hpp
+++ b/TNC/FilterCoefficients.hpp
@@ -1,8 +1,7 @@
-// Copyright 2015 Rob Riggs <rob@mobilinkd.com>
+// Copyright 2015-2020 Rob Riggs <rob@mobilinkd.com>
 // All rights reserved.
 
-#ifndef MOBILINKD__TNC__FILTER_COEFFICIENTS_HPP_
-#define MOBILINKD__TNC__FILTER_COEFFICIENTS_HPP_
+#pragma once
 
 #include "IirFilter.hpp"
 #include "FirFilter.hpp"
@@ -297,31 +296,9 @@ const TFirCoefficients<9> dB_6 = {
     }
 };
 
-const TFirCoefficients<9>* AfskFilters[] = {
-  &dB_6,
-  &dB_5,
-  &dB_4,
-  &dB_3,
-  &dB_2,
-  &dB_1,
-  &dB0,
-  &dB1,
-  &dB2,
-  &dB3,
-  &dB4,
-  &dB5,
-  &dB6,
-  &dB7,
-  &dB8,
-  &dB9,
-  &dB10,
-  &dB11,
-  &dB12
-};
+extern const TFirCoefficients<9>* AfskFilters[19];
 
 } // fir
 
 
 }}} // mobilinkd::tnc::filter
-
-#endif // MOBILINKD__TNC__FILTER_COEFFICIENTS_HPP_
diff --git a/TNC/FirFilter.hpp b/TNC/FirFilter.hpp
index b679ee7..8f64ae5 100644
--- a/TNC/FirFilter.hpp
+++ b/TNC/FirFilter.hpp
@@ -1,8 +1,7 @@
-// Copyright 2015-2019 Rob Riggs <rob@mobilinkd.com>
+// Copyright 2015-2020 Rob Riggs <rob@mobilinkd.com>
 // All rights reserved.
 
-#ifndef MOBILINKD__TNC__FIR_FILTER_H_
-#define MOBILINKD__TNC__FIR_FILTER_H_
+#pragma once
 
 #include <AudioLevel.hpp>
 
@@ -96,7 +95,6 @@ template <size_t BLOCK_SIZE, size_t FILTER_SIZE>
 struct Q15FirFilter {
     const q15_t* filter_taps{nullptr};
     q15_t filter_state[BLOCK_SIZE + FILTER_SIZE - 1];
-    q15_t filter_input[BLOCK_SIZE];
     q15_t filter_output[BLOCK_SIZE];
     q15_t vgnd_{0};
     q15_t i_vgnd_{0};
@@ -115,25 +113,16 @@ struct Q15FirFilter {
     {
         vgnd_ = audio::virtual_ground;
         filter_taps = taps;
-        arm_fir_init_q15(&instance, FILTER_SIZE, const_cast<q15_t*>(filter_taps), // WTF ARM?!?
+        arm_fir_init_q15(&instance, FILTER_SIZE,
+            const_cast<q15_t*>(filter_taps),
             filter_state, BLOCK_SIZE);
     }
 
-    // ADC input
-    q15_t* operator()(q15_t* input) // __attribute__((section(".bss2")))
+    q15_t* filter(const q15_t* input)
     {
-        arm_fir_fast_q15(&instance, input, filter_output, BLOCK_SIZE);
-        return filter_output;
-    }
-
-    // LPF input
-    q15_t* filter(q15_t* input) // __attribute__((section(".bss2")))
-    {
-        arm_fir_fast_q15(&instance, input, filter_output, BLOCK_SIZE);
+        arm_fir_fast_q15(&instance, const_cast<q15_t*>(input), filter_output, BLOCK_SIZE);
         return filter_output;
     }
 };
 
 }} // mobilinkd::tnc
-
-#endif // MOBILINKD__TNC__FIR_FILTER_H_
diff --git a/TNC/Fsk9600Demodulator.cpp b/TNC/Fsk9600Demodulator.cpp
new file mode 100644
index 0000000..187bdcf
--- /dev/null
+++ b/TNC/Fsk9600Demodulator.cpp
@@ -0,0 +1,382 @@
+// Copyright 2020 Rob Riggs <rob@mobilinkd.com>
+// All rights reserved.
+
+#include "Fsk9600Demodulator.hpp"
+#include "Goertzel.h"
+#include "AudioInput.hpp"
+#include "GPIO.hpp"
+#include "Log.h"
+
+namespace mobilinkd { namespace tnc {
+
+hdlc::IoFrame* Fsk9600Demodulator::operator()(const q15_t* samples)
+{
+    hdlc::IoFrame* result = nullptr;
+
+    auto filtered = demod_filter.filter(const_cast<q15_t* >(samples));
+
+    for (size_t i = 0; i != ADC_BLOCK_SIZE; ++i)
+    {
+        auto sample = filtered[i];
+
+        bool bit = sample >= 0;
+        auto pll = pll_(bit);
+
+        if (pll.sample)
+        {
+            locked_ = pll.locked;
+
+            // We will only ever get one frame because there are
+            // not enough bits in a block for more than one.
+            if (result) {
+                auto tmp = hdlc_decoder_(nrzi_.decode(lfsr_(bit)), locked_);
+                if (tmp) hdlc::release(tmp);
+            } else {
+                result = hdlc_decoder_(nrzi_.decode(lfsr_(bit)), locked_);
+#ifdef KISS_LOGGING
+                if (result) {
+                    INFO("samples = %ld, mean = %d, dev = %d",
+                        snr_.samples, int(snr_.mean), int(snr_.stdev()));
+                    INFO("SNR = %dmB", int(snr_.SNR() * 100.0f));
+                    snr_.reset();
+                }
+#endif
+            }
+
+#ifdef KISS_LOGGING
+            if (hdlc_decoder_.active())
+            {
+                if (!decoding_)
+                {
+                    snr_.reset();
+                    decoding_ = true;
+                }
+                snr_.capture(float(abs(sample)));
+            } else {
+                decoding_ = false;
+            }
+#endif
+        }
+
+    }
+    return result;
+}
+
+/*
+ * Return twist as a the difference in dB between mark and space.  The
+ * expected values are about 0dB for discriminator output and about 5.5dB
+ * for de-emphasized audio.
+ */
+float Fsk9600Demodulator::readTwist()
+{
+    DEBUG("enter Fsk9600Demodulator::readTwist");
+
+    float g120 = 0.0f;
+    float g4800 = 0.0f;
+
+    GoertzelFilter<ADC_BLOCK_SIZE, SAMPLE_RATE> gf120(120.0, 0);
+    GoertzelFilter<ADC_BLOCK_SIZE, SAMPLE_RATE> gf4800(4800.0, 0);
+
+    const uint32_t AVG_SAMPLES = 160;
+
+    startADC(416, ADC_BLOCK_SIZE);
+
+    for (uint32_t i = 0; i != AVG_SAMPLES; ++i)
+    {
+        uint32_t count = 0;
+        while (count < ADC_BLOCK_SIZE)
+        {
+            osEvent evt = osMessageGet(adcInputQueueHandle, osWaitForever);
+            if (evt.status != osEventMessage)
+                continue;
+
+            auto block = (audio::adc_pool_type::chunk_type*) evt.value.p;
+            uint16_t* data = (uint16_t*) block->buffer;
+            gf120(data, ADC_BLOCK_SIZE);
+            gf4800(data, ADC_BLOCK_SIZE);
+
+            audio::adcPool.deallocate(block);
+
+            count += ADC_BLOCK_SIZE;
+        }
+
+        g120 += (gf120 / count);
+        g4800 += (gf4800 / count);
+
+        gf120.reset();
+        gf4800.reset();
+    }
+
+    IDemodulator::stopADC();
+
+    g120 = 10.0f * log10f(g120 / AVG_SAMPLES);
+    g4800 = 10.0f * log10f(g4800 / AVG_SAMPLES);
+
+    auto result = g120 - g4800;
+
+    INFO("9600 Twist = %d / 100 (%d - %d)", int(result * 100), int(g120 * 100),
+        int(g4800 * 100));
+
+    DEBUG("exit Fsk9600Demodulator::readTwist");
+    return result;
+}
+
+uint32_t Fsk9600Demodulator::readBatteryLevel()
+{
+    DEBUG("enter Fsk9600Demodulator::readBatteryLevel");
+
+    ADC_ChannelConfTypeDef sConfig;
+
+    sConfig.Channel = ADC_CHANNEL_VREFINT;
+    sConfig.Rank = ADC_REGULAR_RANK_1;
+    sConfig.SingleDiff = ADC_SINGLE_ENDED;
+    sConfig.SamplingTime = ADC_SAMPLETIME_247CYCLES_5;
+    sConfig.OffsetNumber = ADC_OFFSET_NONE;
+    sConfig.Offset = 0;
+    if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
+        CxxErrorHandler();
+
+    htim6.Init.Period = 48000;
+    if (HAL_TIM_Base_Init(&htim6) != HAL_OK) CxxErrorHandler();
+
+    if (HAL_TIM_Base_Start(&htim6) != HAL_OK)
+        CxxErrorHandler();
+
+    if (HAL_ADC_Start(&hadc1) != HAL_OK) CxxErrorHandler();
+    if (HAL_ADC_PollForConversion(&hadc1, 3) != HAL_OK) CxxErrorHandler();
+    auto vrefint = HAL_ADC_GetValue(&hadc1);
+    if (HAL_ADC_Stop(&hadc1) != HAL_OK) CxxErrorHandler();
+
+    // Disable battery charging while measuring battery voltage.
+    auto usb_ce = gpio::USB_CE::get();
+    gpio::USB_CE::on();
+
+    gpio::BAT_DIVIDER::off();
+    HAL_Delay(1);
+
+    sConfig.Channel = ADC_CHANNEL_15;
+    if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
+        CxxErrorHandler();
+
+    uint32_t vbat = 0;
+    if (HAL_ADC_Start(&hadc1) != HAL_OK) CxxErrorHandler();
+    for (size_t i = 0; i != 8; ++i)
+    {
+        if (HAL_ADC_PollForConversion(&hadc1, 1) != HAL_OK) CxxErrorHandler();
+        vbat += HAL_ADC_GetValue(&hadc1);
+    }
+
+    vbat /= 8;
+
+    if (HAL_ADC_Stop(&hadc1) != HAL_OK) CxxErrorHandler();
+    if (HAL_TIM_Base_Stop(&htim6) != HAL_OK)
+        CxxErrorHandler();
+
+    gpio::BAT_DIVIDER::on();
+
+    // Restore battery charging state.
+    if (!usb_ce) gpio::USB_CE::off();
+
+    INFO("Vref = %lu", vrefint);
+    INFO("Vbat = %lu (raw)", vbat);
+
+    // Order of operations is important to avoid underflow.
+    vbat *= 6600;
+    vbat /= (VREF + 1);
+
+    uint32_t vref = ((vrefint * 3300) + (VREF / 2)) / VREF;
+
+    INFO("Vref = %lumV", vref)
+    INFO("Vbat = %lumV", vbat);
+
+    DEBUG("exit Fsk9600Demodulator::readBatteryLevel");
+    return vbat;
+}
+
+const Fsk9600Demodulator::bpf_bank_type Fsk9600Demodulator::bpf_bank = {{
+    // -3dB
+    {{
+            1,     0,     0,     0,     0,     0,    -1,    -1,    -1,    -1,     0,     0,
+            0,     0,     1,     2,     2,     3,     2,     2,     0,     0,    -2,    -3,
+           -4,    -4,    -3,    -1,     3,     9,    17,    27,    38,    48,    58,    65,
+           68,    67,    60,    46,    25,    -1,   -32,   -67,  -102,  -135,  -160,  -175,
+         -174,  -153,  -110,   -41,    54,   178,   327,   500,   692,   897,  1109,  1318,
+         1518,  1700,  1857,  1981,  2067,  2110,  2110,  2067,  1981,  1857,  1700,  1518,
+         1318,  1109,   897,   692,   500,   327,   178,    54,   -41,  -110,  -153,  -174,
+         -175,  -160,  -135,  -102,   -67,   -32,    -1,    25,    46,    60,    67,    68,
+           65,    58,    48,    38,    27,    17,     9,     3,    -1,    -3,    -4,    -4,
+           -3,    -2,     0,     0,     2,     2,     3,     2,     2,     1,     0,     0,
+            0,     0,    -1,    -1,    -1,    -1,     0,     0,     0,     0,     0,     1
+    }},
+    // -2dB
+    {{
+            1,     0,     0,     0,     0,    -1,    -1,    -1,    -1,    -1,    -1,    -1,
+            0,     0,     1,     1,     2,     2,     1,     0,     0,    -1,    -3,    -5,
+           -6,    -6,    -5,    -1,     3,    10,    20,    31,    42,    54,    64,    72,
+           75,    72,    63,    46,    21,   -11,   -49,   -92,  -135,  -175,  -208,  -229,
+         -233,  -216,  -173,  -102,     0,   131,   293,   481,   691,   915,  1147,  1378,
+         1598,  1799,  1971,  2108,  2203,  2251,  2251,  2203,  2108,  1971,  1799,  1598,
+         1378,  1147,   915,   691,   481,   293,   131,     0,  -102,  -173,  -216,  -233,
+         -229,  -208,  -175,  -135,   -92,   -49,   -11,    21,    46,    63,    72,    75,
+           72,    64,    54,    42,    31,    20,    10,     3,    -1,    -5,    -6,    -6,
+           -5,    -3,    -1,     0,     0,     1,     2,     2,     1,     1,     0,     0,
+           -1,    -1,    -1,    -1,    -1,    -1,    -1,     0,     0,     0,     0,     1
+    }},
+    // -1dB
+    {{
+            1,     0,     0,     0,     0,    -1,    -1,    -2,    -2,    -2,    -2,    -1,
+           -1,     0,     0,     0,     1,     1,     0,     0,    -1,    -3,    -5,    -7,
+           -8,    -8,    -6,    -2,     3,    11,    22,    34,    48,    60,    72,    80,
+           82,    78,    66,    45,    15,   -22,   -68,  -119,  -171,  -221,  -262,  -291,
+         -300,  -286,  -244,  -170,   -62,    79,   255,   460,   689,   936,  1191,  1445,
+         1688,  1909,  2100,  2251,  2356,  2410,  2410,  2356,  2251,  2100,  1909,  1688,
+         1445,  1191,   936,   689,   460,   255,    79,   -62,  -170,  -244,  -286,  -300,
+         -291,  -262,  -221,  -171,  -119,   -68,   -22,    15,    45,    66,    78,    82,
+           80,    72,    60,    48,    34,    22,    11,     3,    -2,    -6,    -8,    -8,
+           -7,    -5,    -3,    -1,     0,     0,     1,     1,     0,     0,     0,    -1,
+           -1,    -2,    -2,    -2,    -2,    -1,    -1,     0,     0,     0,     0,     1
+    }},
+    // 0dB
+    {{
+            1,     0,     0,     0,    -1,    -1,    -2,    -2,    -3,    -3,    -3,    -2,
+           -2,    -1,     0,     0,     0,     0,     0,    -1,    -3,    -5,    -7,    -9,
+          -10,   -10,    -7,    -3,     3,    13,    25,    39,    53,    68,    80,    88,
+           91,    85,    70,    45,     9,   -35,   -90,  -150,  -212,  -272,  -323,  -359,
+         -376,  -366,  -324,  -247,  -132,    21,   212,   436,   688,   959,  1240,  1520,
+         1789,  2034,  2244,  2412,  2528,  2587,  2587,  2528,  2412,  2244,  2034,  1789,
+         1520,  1240,   959,   688,   436,   212,    21,  -132,  -247,  -324,  -366,  -376,
+         -359,  -323,  -272,  -212,  -150,   -90,   -35,     9,    45,    70,    85,    91,
+           88,    80,    68,    53,    39,    25,    13,     3,    -3,    -7,   -10,   -10,
+           -9,    -7,    -5,    -3,    -1,     0,     0,     0,     0,     0,    -1,    -2,
+           -2,    -3,    -3,    -3,    -2,    -2,    -1,    -1,     0,     0,     0,     1
+    }},
+    // 1dB
+    {{
+            1,     1,     0,     0,    -1,    -2,    -2,    -3,    -3,    -4,    -4,    -3,
+           -3,    -2,    -1,    -1,    -1,    -1,    -1,    -3,    -5,    -7,    -9,   -11,
+          -12,   -12,    -9,    -4,     3,    14,    28,    43,    60,    76,    89,    98,
+          100,    93,    75,    45,     3,   -50,  -114,  -185,  -258,  -329,  -390,  -437,
+         -460,  -454,  -413,  -333,  -209,   -43,   164,   409,   686,   984,  1295,  1604,
+         1901,  2173,  2406,  2592,  2721,  2786,  2786,  2721,  2592,  2406,  2173,  1901,
+         1604,  1295,   984,   686,   409,   164,   -43,  -209,  -333,  -413,  -454,  -460,
+         -437,  -390,  -329,  -258,  -185,  -114,   -50,     3,    45,    75,    93,   100,
+           98,    89,    76,    60,    43,    28,    14,     3,    -4,    -9,   -12,   -12,
+          -11,    -9,    -7,    -5,    -3,    -1,    -1,    -1,    -1,    -1,    -2,    -3,
+           -3,    -4,    -4,    -3,    -3,    -2,    -2,    -1,     0,     0,     1,     1
+    }},
+    // 2dB
+    {{
+            1,     1,     0,     0,    -1,    -2,    -3,    -4,    -4,    -5,    -5,    -4,
+           -4,    -3,    -3,    -2,    -2,    -2,    -3,    -5,    -7,    -9,   -12,   -14,
+          -15,   -14,   -11,    -5,     3,    15,    31,    49,    67,    85,   100,   109,
+          110,   101,    80,    45,    -3,   -66,  -141,  -223,  -309,  -393,  -467,  -523,
+         -555,  -554,  -513,  -429,  -297,  -116,   110,   379,   684,  1013,  1356,  1699,
+         2028,  2329,  2588,  2794,  2937,  3010,  3010,  2937,  2794,  2588,  2329,  2028,
+         1699,  1356,  1013,   684,   379,   110,  -116,  -297,  -429,  -513,  -554,  -555,
+         -523,  -467,  -393,  -309,  -223,  -141,   -66,    -3,    45,    80,   101,   110,
+          109,   100,    85,    67,    49,    31,    15,     3,    -5,   -11,   -14,   -15,
+          -14,   -12,    -9,    -7,    -5,    -3,    -2,    -2,    -2,    -3,    -3,    -4,
+           -4,    -5,    -5,    -4,    -4,    -3,    -2,    -1,     0,     0,     1,     1
+    }},
+    // 3dB
+    {{
+            2,     1,     0,     0,    -1,    -3,    -4,    -4,    -5,    -6,    -6,    -5,
+           -5,    -4,    -4,    -3,    -3,    -4,    -5,    -7,    -9,   -12,   -15,   -17,
+          -18,   -17,   -13,    -6,     3,    17,    35,    55,    75,    95,   111,   121,
+          122,   111,    86,    45,   -12,   -85,  -171,  -267,  -367,  -465,  -552,  -620,
+         -661,  -665,  -626,  -537,  -395,  -199,    49,   346,   681,  1045,  1425,  1805,
+         2170,  2504,  2792,  3021,  3179,  3261,  3261,  3179,  3021,  2792,  2504,  2170,
+         1805,  1425,  1045,   681,   346,    49,  -199,  -395,  -537,  -626,  -665,  -661,
+         -620,  -552,  -465,  -367,  -267,  -171,   -85,   -12,    45,    86,   111,   122,
+          121,   111,    95,    75,    55,    35,    17,     3,    -6,   -13,   -17,   -18,
+          -17,   -15,   -12,    -9,    -7,    -5,    -4,    -3,    -3,    -4,    -4,    -5,
+           -5,    -6,    -6,    -5,    -4,    -4,    -3,    -1,     0,     0,     1,     2
+    }},
+    // 4dB
+    {{
+            2,     1,     0,    -1,    -2,    -3,    -4,    -5,    -6,    -7,    -7,    -7,
+           -6,    -6,    -5,    -5,    -5,    -6,    -7,    -9,   -12,   -15,   -18,   -20,
+          -21,   -20,   -16,    -8,     3,    19,    39,    61,    85,   107,   125,   135,
+          135,   122,    92,    44,   -21,  -105,  -205,  -316,  -432,  -546,  -648,  -729,
+         -780,  -791,  -752,  -658,  -505,  -291,   -18,   308,   679,  1082,  1502,  1924,
+         2330,  2701,  3021,  3275,  3452,  3542,  3542,  3452,  3275,  3021,  2701,  2330,
+         1924,  1502,  1082,   679,   308,   -18,  -291,  -505,  -658,  -752,  -791,  -780,
+         -729,  -648,  -546,  -432,  -316,  -205,  -105,   -21,    44,    92,   122,   135,
+          135,   125,   107,    85,    61,    39,    19,     3,    -8,   -16,   -20,   -21,
+          -20,   -18,   -15,   -12,    -9,    -7,    -6,    -5,    -5,    -5,    -6,    -6,
+           -7,    -7,    -7,    -6,    -5,    -4,    -3,    -2,    -1,     0,     1,     2
+    }},
+    // 5dB
+    {{
+            2,     1,     0,    -1,    -2,    -4,    -5,    -6,    -7,    -8,    -8,    -8,
+           -8,    -8,    -7,    -7,    -7,    -8,    -9,   -11,   -14,   -18,   -21,   -24,
+          -25,   -23,   -19,    -9,     3,    21,    44,    69,    95,   119,   139,   151,
+          150,   134,    99,    44,   -31,  -128,  -243,  -371,  -505,  -636,  -756,  -852,
+         -914,  -931,  -894,  -795,  -629,  -394,   -94,   265,   676,  1122,  1589,  2058,
+         2508,  2921,  3277,  3560,  3757,  3858,  3858,  3757,  3560,  3277,  2921,  2508,
+         2058,  1589,  1122,   676,   265,   -94,  -394,  -629,  -795,  -894,  -931,  -914,
+         -852,  -756,  -636,  -505,  -371,  -243,  -128,   -31,    44,    99,   134,   150,
+          151,   139,   119,    95,    69,    44,    21,     3,    -9,   -19,   -23,   -25,
+          -24,   -21,   -18,   -14,   -11,    -9,    -8,    -7,    -7,    -7,    -8,    -8,
+           -8,    -8,    -8,    -7,    -6,    -5,    -4,    -2,    -1,     0,     1,     2
+    }},
+    // 6dB
+    {{
+            2,     1,     0,    -1,    -3,    -4,    -6,    -7,    -9,   -10,   -10,   -10,
+          -10,    -9,    -9,    -9,    -9,   -10,   -12,   -14,   -18,   -21,   -25,   -28,
+          -29,   -27,   -22,   -11,     3,    24,    49,    77,   107,   134,   156,   168,
+          167,   148,   107,    44,   -43,  -154,  -286,  -432,  -586,  -738,  -876,  -989,
+        -1064, -1089, -1053,  -947,  -767,  -510,  -180,   218,   672,  1168,  1687,  2208,
+         2709,  3169,  3565,  3881,  4100,  4212,  4212,  4100,  3881,  3565,  3169,  2709,
+         2208,  1687,  1168,   672,   218,  -180,  -510,  -767,  -947, -1053, -1089, -1064,
+         -989,  -876,  -738,  -586,  -432,  -286,  -154,   -43,    44,   107,   148,   167,
+          168,   156,   134,   107,    77,    49,    24,     3,   -11,   -22,   -27,   -29,
+          -28,   -25,   -21,   -18,   -14,   -12,   -10,    -9,    -9,    -9,    -9,   -10,
+          -10,   -10,   -10,    -9,    -7,    -6,    -4,    -3,    -1,     0,     1,     2
+    }},
+    // 7dB
+    {{
+            3,     1,     0,    -1,    -3,    -5,    -7,    -9,   -10,   -11,   -12,   -12,
+          -12,   -11,   -11,   -11,   -11,   -12,   -14,   -17,   -21,   -26,   -30,   -33,
+          -34,   -32,   -25,   -13,     3,    27,    55,    87,   120,   150,   174,   188,
+          185,   163,   116,    43,   -56,  -183,  -334,  -501,  -678,  -852, -1012, -1143,
+        -1232, -1266, -1231, -1119,  -923,  -641,  -276,   164,   668,  1219,  1796,  2376,
+         2934,  3446,  3888,  4240,  4484,  4609,  4609,  4484,  4240,  3888,  3446,  2934,
+         2376,  1796,  1219,   668,   164,  -276,  -641,  -923, -1119, -1231, -1266, -1232,
+        -1143, -1012,  -852,  -678,  -501,  -334,  -183,   -56,    43,   116,   163,   185,
+          188,   174,   150,   120,    87,    55,    27,     3,   -13,   -25,   -32,   -34,
+          -33,   -30,   -26,   -21,   -17,   -14,   -12,   -11,   -11,   -11,   -11,   -12,
+          -12,   -12,   -11,   -10,    -9,    -7,    -5,    -3,    -1,     0,     1,     3
+    }},
+    // 8dB
+    {{
+            3,     1,     0,    -2,    -4,    -6,    -8,   -10,   -12,   -13,   -14,   -14,
+          -14,   -14,   -13,   -13,   -14,   -15,   -17,   -21,   -25,   -30,   -35,   -38,
+          -39,   -36,   -29,   -16,     3,    30,    62,    97,   134,   168,   195,   209,
+          206,   180,   127,    43,   -71,  -216,  -388,  -579,  -780,  -980, -1164, -1316,
+        -1421, -1464, -1431, -1311, -1097,  -787,  -383,   104,   664,  1277,  1919,  2565,
+         3187,  3758,  4251,  4643,  4916,  5055,  5055,  4916,  4643,  4251,  3758,  3187,
+         2565,  1919,  1277,   664,   104,  -383,  -787, -1097, -1311, -1431, -1464, -1421,
+        -1316, -1164,  -980,  -780,  -579,  -388,  -216,   -71,    43,   127,   180,   206,
+          209,   195,   168,   134,    97,    62,    30,     3,   -16,   -29,   -36,   -39,
+          -38,   -35,   -30,   -25,   -21,   -17,   -15,   -14,   -13,   -13,   -14,   -14,
+          -14,   -14,   -13,   -12,   -10,    -8,    -6,    -4,    -2,     0,     1,     3
+    }},
+    // 9dB
+    {{
+            3,     2,     0,    -2,    -4,    -7,    -9,   -12,   -14,   -15,   -16,   -17,
+          -17,   -16,   -16,   -16,   -17,   -18,   -21,   -25,   -30,   -35,   -40,   -44,
+          -45,   -42,   -33,   -18,     3,    33,    69,   109,   151,   189,   218,   234,
+          230,   199,   138,    42,   -88,  -253,  -448,  -666,  -895, -1123, -1334, -1510,
+        -1633, -1687, -1656, -1527, -1293,  -951,  -504,    37,   659,  1341,  2056,  2777,
+         3470,  4107,  4658,  5095,  5400,  5556,  5556,  5400,  5095,  4658,  4107,  3470,
+         2777,  2056,  1341,   659,    37,  -504,  -951, -1293, -1527, -1656, -1687, -1633,
+        -1510, -1334, -1123,  -895,  -666,  -448,  -253,   -88,    42,   138,   199,   230,
+          234,   218,   189,   151,   109,    69,    33,     3,   -18,   -33,   -42,   -45,
+          -44,   -40,   -35,   -30,   -25,   -21,   -18,   -17,   -16,   -16,   -16,   -17,
+          -17,   -16,   -15,   -14,   -12,    -9,    -7,    -4,    -2,     0,     2,     3
+    }}
+}};
+
+}} // mobilinkd::tnc
+
diff --git a/TNC/Fsk9600Demodulator.hpp b/TNC/Fsk9600Demodulator.hpp
new file mode 100644
index 0000000..3e914c2
--- /dev/null
+++ b/TNC/Fsk9600Demodulator.hpp
@@ -0,0 +1,112 @@
+// Copyright 2020 Rob Riggs <rob@mobilinkd.com>
+// All rights reserved.
+
+#pragma once
+
+#include "Demodulator.hpp"
+#include "AudioLevel.hpp"
+#include "AudioInput.hpp"
+#include "DigitalPLL.hpp"
+#include "NRZI.hpp"
+#include "HdlcDecoder.hpp"
+#include "KissHardware.hpp"
+#include "StandardDeviation.hpp"
+
+namespace mobilinkd { namespace tnc {
+
+struct Descrambler
+{
+    uint32_t state{0};
+
+    bool operator()(bool bit)
+    {
+        bool result = (bit ^ (state >> 16) ^ (state >> 11)) & 1;
+        state = ((state << 1) | (bit & 1)) & 0x1FFFF;
+        return result;
+    }
+};
+
+struct Fsk9600Demodulator : IDemodulator
+{
+    static constexpr size_t FILTER_TAP_NUM = 132;
+    static constexpr uint32_t ADC_BLOCK_SIZE = 384;
+    static_assert(audio::ADC_BUFFER_SIZE >= ADC_BLOCK_SIZE);
+
+    static constexpr uint32_t SAMPLE_RATE = 192000;
+    static constexpr uint16_t VREF = 4095;
+
+    using bpf_coeffs_type = std::array<int16_t, FILTER_TAP_NUM>;
+    using bpf_bank_type = std::array<bpf_coeffs_type, 13>;
+    using audio_filter_t = Q15FirFilter<ADC_BLOCK_SIZE, FILTER_TAP_NUM>;
+
+    static const bpf_bank_type bpf_bank;
+
+    audio_filter_t demod_filter;
+    BaseDigitalPLL<float> pll_{192000,9600};
+    bool locked_{false};
+    Descrambler lfsr_;
+    libafsk::NRZI nrzi_;
+    hdlc::NewDecoder hdlc_decoder_;
+    StandardDeviation snr_;
+    bool decoding_{false};
+
+    virtual ~Fsk9600Demodulator() {}
+
+    void start() override
+    {
+        SysClock80();
+
+        auto const& bpf_coeffs = bpf_bank[kiss::settings().rx_twist + 3];
+        const q15_t* bpf = bpf_coeffs.data();
+        demod_filter.init(bpf);
+        passall(kiss::settings().options & KISS_OPTION_PASSALL);
+
+        hadc1.Init.OversamplingMode = DISABLE;
+        if (HAL_ADC_Init(&hadc1) != HAL_OK)
+        {
+            CxxErrorHandler();
+        }
+
+        ADC_ChannelConfTypeDef sConfig;
+
+        sConfig.Channel = AUDIO_IN;
+        sConfig.Rank = ADC_REGULAR_RANK_1;
+        sConfig.SingleDiff = ADC_SINGLE_ENDED;
+        sConfig.SamplingTime = ADC_SAMPLETIME_247CYCLES_5;
+        sConfig.OffsetNumber = ADC_OFFSET_NONE;
+        sConfig.Offset = 0;
+        if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
+            CxxErrorHandler();
+
+        startADC(416, ADC_BLOCK_SIZE);
+    }
+
+    void stop() override
+    {
+        stopADC();
+        locked_ = false;
+    }
+
+    float readTwist() override;
+
+    uint32_t readBatteryLevel() override;
+
+    hdlc::IoFrame* operator()(const q15_t* samples) override;
+
+    bool locked() const override
+    {
+        return locked_;
+    }
+
+    size_t size() const override
+    {
+        return ADC_BLOCK_SIZE;
+    }
+
+    void passall(bool enabled) override
+    {
+        hdlc_decoder_.setPassall(enabled);
+    }
+};
+
+}} // mobilinkd::tnc
diff --git a/TNC/Fsk9600Modulator.cpp b/TNC/Fsk9600Modulator.cpp
new file mode 100644
index 0000000..2f2d7b7
--- /dev/null
+++ b/TNC/Fsk9600Modulator.cpp
@@ -0,0 +1,66 @@
+// Copyright 2020 Rob Riggs <rob@mobilinkd.com>
+// All rights reserved.
+
+#include "Fsk9600Modulator.hpp"
+
+namespace mobilinkd { namespace tnc {
+
+/*
+ * Cosine.
+const Fsk9600Modulator::cos_table_type Fsk9600Modulator::cos_table = {
+    2047,  2020,  1937,  1801,  1616,  1387,  1120,   822,   502,   169,
+    -169,  -502,  -822, -1120, -1387, -1616, -1801, -1937, -2020, -2048
+};
+*/
+
+/*
+ * Square wave -- filtered in hardware at 7200Hz
+const Fsk9600Modulator::cos_table_type Fsk9600Modulator::cos_table = {
+     2047,  2047,  2047,  2047,  2047,  2047,  2047,  2047,  2047,  2047,
+    -2048, -2048, -2048, -2048, -2048, -2048, -2048, -2048, -2048, -2048
+};
+*/
+
+// Gaussian
+const Fsk9600Modulator::cos_table_type Fsk9600Modulator::cos_table = {
+    2042,  2027,  1995,  1931,  1815,  1626,  1345,   968,   507,     0,
+    -507,  -968, -1345, -1626, -1815, -1931, -1995, -2027, -2042, -2048
+};
+
+void Fsk9600Modulator::init(const kiss::Hardware& hw)
+{
+    for (auto& x : buffer_) x = 2048;
+
+    (void) hw; // unused
+
+    state = State::STOPPED;
+    level = Level::HIGH;
+
+    SysClock80();
+
+    // Configure 80MHz clock for 192ksps.
+    htim7.Init.Period = 416;
+    if (HAL_TIM_Base_Init(&htim7) != HAL_OK)
+    {
+        ERROR("htim7 init failed");
+        CxxErrorHandler();
+    }
+
+    DAC_ChannelConfTypeDef sConfig;
+
+    sConfig.DAC_SampleAndHold = DAC_SAMPLEANDHOLD_DISABLE;
+    sConfig.DAC_Trigger = DAC_TRIGGER_NONE;
+    sConfig.DAC_OutputBuffer = DAC_OUTPUTBUFFER_ENABLE;
+    sConfig.DAC_ConnectOnChipPeripheral = DAC_CHIPCONNECT_ENABLE;
+    sConfig.DAC_UserTrimming = DAC_TRIMMING_FACTORY;
+    if (HAL_DAC_ConfigChannel(&hdac1, &sConfig, DAC_CHANNEL_1) != HAL_OK)
+    {
+      CxxErrorHandler();
+    }
+
+    if (HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, 2048) != HAL_OK) CxxErrorHandler();
+    if (HAL_DAC_Start(&hdac1, DAC_CHANNEL_1) != HAL_OK) CxxErrorHandler();
+}
+
+
+}} // mobilinkd::tnc
diff --git a/TNC/Fsk9600Modulator.hpp b/TNC/Fsk9600Modulator.hpp
new file mode 100644
index 0000000..5bc53d2
--- /dev/null
+++ b/TNC/Fsk9600Modulator.hpp
@@ -0,0 +1,229 @@
+// Copyright 2020 Rob Riggs <rob@mobilinkd.com>
+// All rights reserved.
+
+#pragma once
+
+#include "Modulator.hpp"
+
+#include <array>
+#include <algorithm>
+#include <cstdint>
+
+namespace mobilinkd { namespace tnc {
+
+struct Scrambler
+{
+    uint32_t state{0};
+
+    bool operator()(bool bit)
+    {
+        bool result = (bit ^ (state >> 16) ^ (state >> 11)) & 1;
+        state = ((state << 1) | result) & 0x1FFFF;
+        return result;
+    }
+};
+
+struct Fsk9600Modulator : Modulator
+{
+    static constexpr int8_t DAC_BUFFER_LEN = 40;
+    static constexpr int8_t BIT_LEN = DAC_BUFFER_LEN / 2;
+    static constexpr uint16_t VREF = 4095;
+
+    using cos_table_type = std::array<int16_t, Fsk9600Modulator::BIT_LEN>;
+    static const cos_table_type cos_table;
+
+    enum class Level { ZERO, HIGH, LOW };
+    enum class State { STOPPED, STARTING, RUNNING, STOPPING };
+
+    osMessageQId dacOutputQueueHandle_{0};
+    PTT* ptt_{nullptr};
+    uint16_t volume_{4096};
+    std::array<uint16_t, DAC_BUFFER_LEN> buffer_;
+    Level level{Level::HIGH};
+    State state{State::STOPPED};
+    Scrambler lfsr;
+
+    Fsk9600Modulator(osMessageQId queue, PTT* ptt)
+    : dacOutputQueueHandle_(queue), ptt_(ptt)
+    {}
+
+    ~Fsk9600Modulator() override {}
+
+    void init(const kiss::Hardware& hw) override;
+
+    void deinit() override
+    {
+        state = State::STOPPED;
+        HAL_DAC_Stop(&hdac1, DAC_CHANNEL_1);
+        HAL_TIM_Base_Stop(&htim7);
+        ptt_->off();
+    }
+
+    void set_gain(uint16_t level) override
+    {
+        auto v = std::max<uint16_t>(256, level);
+        v = std::min<uint16_t>(4096, v);
+        volume_ = v;
+    }
+
+    void set_ptt(PTT* ptt) override
+    {
+        if (state != State::STOPPED)
+        {
+            ERROR("PTT change while not stopped");
+            CxxErrorHandler();
+        }
+        ptt_ = ptt;
+        ptt_->off();
+    }
+
+    void send(bool bit) override
+    {
+        auto scrambled = lfsr(bit);
+
+        switch (state)
+        {
+        case State::STOPPING:
+        case State::STOPPED:
+            ptt_->on();
+            #ifdef KISS_LOGGING
+                HAL_RCCEx_DisableLSCO();
+            #endif
+
+            fill_first(scrambled);
+            state = State::STARTING;
+            break;
+        case State::STARTING:
+            fill_last(scrambled);
+            state = State::RUNNING;
+            start_conversion();
+            break;
+        case State::RUNNING:
+            osMessagePut(dacOutputQueueHandle_, scrambled, osWaitForever);
+            break;
+        }
+    }
+
+    // DAC DMA interrupt functions.
+
+    void fill_first(bool bit) override
+    {
+        fill(buffer_.data(), bit);
+    }
+
+    void fill_last(bool bit) override
+    {
+        fill(buffer_.data() + BIT_LEN, bit);
+    }
+
+    void empty() override
+    {
+        switch (state)
+        {
+        case State::STARTING:
+            // fall-through
+        case State::RUNNING:
+            state = State::STOPPING;
+            break;
+        case State::STOPPING:
+            state = State::STOPPED;
+            stop_conversion();
+            ptt_->off();
+            level = Level::HIGH;
+            #ifdef KISS_LOGGING
+                HAL_RCCEx_EnableLSCO(RCC_LSCOSOURCE_LSE);
+            #endif
+            break;
+        case State::STOPPED:
+            break;
+        }
+    }
+
+    void abort() override
+    {
+        state = State::STOPPED;
+        stop_conversion();
+        ptt_->off();
+        level = Level::HIGH;
+        #ifdef KISS_LOGGING
+            HAL_RCCEx_EnableLSCO(RCC_LSCOSOURCE_LSE);
+        #endif
+        // Drain the queue.
+        while (osMessageGet(dacOutputQueueHandle_, 0).status == osEventMessage);
+    }
+
+    float bits_per_ms() const override
+    {
+        return 9.6f;
+    }
+
+private:
+
+    /**
+     * Configure the DAC for timer-based DMA conversion, start the timer,
+     * and start DMA to DAC.
+     */
+    void start_conversion()
+    {
+        DAC_ChannelConfTypeDef sConfig;
+
+        sConfig.DAC_SampleAndHold = DAC_SAMPLEANDHOLD_DISABLE;
+        sConfig.DAC_Trigger = DAC_TRIGGER_T7_TRGO;
+        sConfig.DAC_OutputBuffer = DAC_OUTPUTBUFFER_ENABLE;
+        sConfig.DAC_ConnectOnChipPeripheral = DAC_CHIPCONNECT_ENABLE;
+        sConfig.DAC_UserTrimming = DAC_TRIMMING_FACTORY;
+        if (HAL_DAC_ConfigChannel(&hdac1, &sConfig, DAC_CHANNEL_1) != HAL_OK)
+        {
+          CxxErrorHandler();
+        }
+
+        HAL_TIM_Base_Start(&htim7);
+        HAL_DAC_Start_DMA(
+            &hdac1, DAC_CHANNEL_1,
+            reinterpret_cast<uint32_t*>(buffer_.data()), buffer_.size(),
+            DAC_ALIGN_12B_R);
+    }
+
+    uint16_t adjust_level(int32_t sample) const
+    {
+        sample *= volume_;
+        sample >>= 12;
+        sample += 2048;
+        return sample;
+    }
+
+    void fill(uint16_t* buffer, bool bit)
+    {
+        switch (level)
+        {
+        case Level::HIGH:
+            if (bit)
+            {
+                std::fill(buffer, buffer + BIT_LEN, adjust_level(2047));
+            }
+            else
+            {
+                std::transform(cos_table.begin(), cos_table.end(), buffer,
+                    [this](auto x){return adjust_level(x);});
+                level = Level::LOW;
+            }
+            break;
+        case Level::LOW:
+            if (bit)
+            {
+                std::transform(cos_table.begin(), cos_table.end(), buffer,
+                    [this](auto x){return adjust_level(-1 - x);});
+                level = Level::HIGH;
+            }
+            else
+            {
+                std::fill(buffer, buffer + BIT_LEN, adjust_level(-2048));
+            }
+            break;
+        default:
+            CxxErrorHandler();
+        }
+    }
+};
+
+}} // mobilinkd::tnc
diff --git a/TNC/Goertzel.h b/TNC/Goertzel.h
index 47e8b90..3ccefac 100644
--- a/TNC/Goertzel.h
+++ b/TNC/Goertzel.h
@@ -1,94 +1,102 @@
 // Copyright 2017 Rob Riggs <rob@mobilinkd.com>
 // All rights reserved.
 
-#ifndef MOBILINKD__TNC__GOERTZEL_FILTER_HPP_
-#define MOBILINKD__TNC__GOERTZEL_FILTER_HPP_
+#pragma once
 
 #include "AudioLevel.hpp"
 #include <arm_math.h>
 #include <complex>
 
-namespace mobilinkd { namespace tnc {
+namespace mobilinkd {
+namespace tnc {
 
 extern const float WINDOW[];
 
-template <uint32_t SAMPLES, uint32_t SAMPLE_RATE>
+template<uint32_t SAMPLES, uint32_t SAMPLE_RATE>
 class GoertzelFilter
 {
-	float filterFreq_;
-	int bin_;
-	float coeff_;
-  float d1{0.0f};
-  float d2{0.0f};
-  uint32_t count{0};
-  const float* window_;
-	
+    float filterFreq_;
+    int bin_;
+    float coeff_;
+    float d1 { 0.0f };
+    float d2 { 0.0f };
+    uint32_t count { 0 };
+    const float* window_;
+
 public:
-	GoertzelFilter(float filter_freq, const float* window = WINDOW)
-	: filterFreq_(filter_freq)
-	, bin_(0.5f + ((filter_freq * SAMPLES) / SAMPLE_RATE))
-	, coeff_(2.0f * cos((2.0f * M_PI * bin_) / float(SAMPLES)))
-  , window_(window)
-	{}
-	
-	void operator()(float* samples, uint32_t n) {
-
-	  for (size_t i = 0; i != n; ++i) {
-	      float w = window_ ? window_[count] : 1.0;
-	      float y  = w * samples[i] + coeff_ * d1 - d2;
-	      d2 = d1;
-	      d1 = y;
-	      ++count;
-	  }
-	}
-	
-  void operator()(uint16_t* samples, uint32_t n) {
-
-    for (uint32_t i = 0; i != n; ++i) {
-        float w = window_ ? window_[count] : 1.0;
-        float sample = (float(samples[i]) - audio::virtual_ground) * audio::i_vgnd;
-        float y = w * sample + coeff_ * d1 - d2;
-        d2 = d1;
-        d1 = y;
-        ++count;
+    GoertzelFilter(float filter_freq, const float* window = WINDOW)
+        : filterFreq_(filter_freq), bin_(
+            0.5f + ((filter_freq * SAMPLES) / SAMPLE_RATE)), coeff_(
+            2.0f * cos((2.0f * M_PI * bin_) / float(SAMPLES))), window_(window)
+    {
     }
-  }
 
-  operator float() const {
-    return d2 * d2 + d1 * d1 - coeff_ * d1 * d2;
-  }
+    void operator()(float* samples, uint32_t n)
+    {
 
-  void reset() {
-    d1 = 0.0f;
-    d2 = 0.0f;
-    count = 0;
-  }
+        for (size_t i = 0; i != n; ++i)
+        {
+            float w = window_ ? window_[count] : 1.0;
+            float y = w * samples[i] + coeff_ * d1 - d2;
+            d2 = d1;
+            d1 = y;
+            ++count;
+        }
+    }
+
+    void operator()(uint16_t* samples, uint32_t n)
+    {
+
+        for (uint32_t i = 0; i != n; ++i)
+        {
+            float w = window_ ? window_[count] : 1.0;
+            float sample = (float(samples[i]) - audio::virtual_ground)
+                * audio::i_vgnd;
+            float y = w * sample + coeff_ * d1 - d2;
+            d2 = d1;
+            d1 = y;
+            ++count;
+        }
+    }
+
+    operator float() const
+    {
+        return d2 * d2 + d1 * d1 - coeff_ * d1 * d2;
+    }
+
+    void reset()
+    {
+        d1 = 0.0f;
+        d2 = 0.0f;
+        count = 0;
+    }
 };
 
 #if 0
 template <uint32_t SAMPLES, uint32_t SAMPLE_RATE>
 class GoertzelFactory
 {
-  float window_[SAMPLES];
-  static void make_window(float* window)
-  {
-    for (size_t i = 0; i != SAMPLES; ++i) {
-      window[i] = 0.54f - 0.46f * cos(2.0f * M_PI * (float(i) / float(SAMPLE_RATE)));
+    float window_[SAMPLES];
+    static void make_window(float* window)
+    {
+        for (size_t i = 0; i != SAMPLES; ++i)
+        {
+            window[i] = 0.54f - 0.46f * cos(2.0f * M_PI * (float(i) / float(SAMPLE_RATE)));
+        }
     }
-  }
 
 public:
 
-  GoertzelFactory()
-  : window_()
-  {
-    make_window(window_);
-  }
+    GoertzelFactory()
+    : window_()
+    {
+        make_window(window_);
+    }
 
-  GoertzelFilter<SAMPLES, SAMPLE_RATE> make(float freq)
-  {
-    return GoertzelFilter<SAMPLES, SAMPLE_RATE>(freq, window_);
-  }
+    GoertzelFilter<SAMPLES, SAMPLE_RATE> make(float freq)
+    {
+        return GoertzelFilter<SAMPLES, SAMPLE_RATE>(freq, window_);
+    }
 };
 #endif
 
@@ -104,20 +112,22 @@ struct Goertzel
     float_type sin_;
     float_type cos_;
     float_type coeff_;
-    float_type q0{0.0}, q1{0.0}, q2{0.0};
+    float_type q0 { 0.0 }, q1 { 0.0 }, q2 { 0.0 };
     explicit Goertzel(float_type omega)
-    : sin_(sin(omega)), cos_(cos(omega)), coeff_(2.0 * cos_)
-    {}
+        : sin_(sin(omega)), cos_(cos(omega)), coeff_(2.0 * cos_)
+    {
+    }
 
     static type from_frequency(float_type frequency, float_type sample_rate)
     {
         return Goertzel(2.0 * PI * frequency / sample_rate);
     }
 
-    template <typename Container>
+    template<typename Container>
     complex_type operator()(const Container& data)
     {
-        for (auto& v : data) {
+        for (auto& v : data)
+        {
             q0 = coeff_ * q1 - q2 + v;
             q2 = q1;
             q1 = q0;
@@ -134,6 +144,6 @@ struct Goertzel
 
 typedef Goertzel<float> FloatGoertzel;
 
-}} // mobilinkd::tnc
+}
+} // mobilinkd::tnc
 
-#endif // MOBILINKD__TNC__GOERTZEL_FILTER_HPP_
diff --git a/TNC/HDLCEncoder.hpp b/TNC/HDLCEncoder.hpp
index ea8ccf8..43826ac 100644
--- a/TNC/HDLCEncoder.hpp
+++ b/TNC/HDLCEncoder.hpp
@@ -1,10 +1,10 @@
-// Copyright 2015 Mobilinkd LLC <rob@mobilinkd.com>
+// Copyright 2015-2020 Mobilinkd LLC <rob@mobilinkd.com>
 // All rights reserved.
 
-#ifndef INC_HDLCENCODER_HPP_
-#define INC_HDLCENCODER_HPP_
+#pragma once
 
-#include "AFSKModulator.hpp"
+#include "Modulator.hpp"
+#include "ModulatorTask.hpp"
 #include "HdlcFrame.hpp"
 #include "NRZI.hpp"
 #include "PTT.hpp"
@@ -25,8 +25,9 @@ using namespace mobilinkd::libafsk;
 
 struct Encoder {
 
-    static const uint8_t IDLE = 0x00;
-    static const uint8_t FLAG = 0x7E;
+    // static constexpr uint8_t IDLE = 0x00;
+    static constexpr uint8_t IDLE = 0x7E;
+    static constexpr uint8_t FLAG = 0x7E;
 
     enum class state_type {
         STATE_IDLE,
@@ -47,16 +48,16 @@ struct Encoder {
     NRZI nrzi_;
     uint16_t crc_;
     osMessageQId input_;
-    AFSKModulator* modulator_;
+    Modulator* modulator_;
     volatile bool running_;
     bool send_delay_;   // Avoid sending the preamble for back-to-back frames.
 
-    Encoder(osMessageQId input, AFSKModulator* output)
+    Encoder(osMessageQId input)
     : tx_delay_(kiss::settings().txdelay), tx_tail_(kiss::settings().txtail)
     , p_persist_(kiss::settings().ppersist), slot_time_(kiss::settings().slot)
     , duplex_(kiss::settings().duplex), state_(state_type::STATE_IDLE)
     , ones_(0), nrzi_(), crc_()
-    , input_(input), modulator_(output)
+    , input_(input), modulator_(&getModulator())
     , running_(false), send_delay_(true)
    {}
 
@@ -67,11 +68,17 @@ struct Encoder {
             state_ = state_type::STATE_IDLE;
             osEvent evt = osMessageGet(input_, osWaitForever);
             if (evt.status == osEventMessage) {
+                tx_delay_ = kiss::settings().txdelay;
+                tx_tail_ = kiss::settings().txtail;
+                p_persist_ = kiss::settings().ppersist;
+                slot_time_ = kiss::settings().slot;
+                duplex_ = kiss::settings().duplex;
                 auto frame = (IoFrame*) evt.value.p;
                 process(frame);
                 // See if we have back-to-back frames.
                 evt = osMessagePeek(input_, 0);
                 if (evt.status != osEventMessage) {
+                    send_raw(IDLE);
                     send_raw(IDLE);
                     send_delay_ = true;
                     if (!duplex_) {
@@ -95,6 +102,11 @@ struct Encoder {
     int p_persist() const { return p_persist_; }
     void p_persist(int value) { p_persist_ = value; }
 
+    void updateModulator()
+    {
+        modulator_ = &(getModulator());
+    }
+
     state_type status() const {return state_; }
     void stop() { running_ = false; }
 
@@ -184,6 +196,8 @@ struct Encoder {
             }
             send_delay();
             send_delay_ = false;
+        } else {
+            send_raw(FLAG);
         }
 
         for (auto c : *frame) send(c);
@@ -192,7 +206,9 @@ struct Encoder {
     }
 
     void send_delay() {
-        const size_t tmp = (tx_delay_ * 3) / 2;
+        const size_t tmp = tx_delay_ * 1.25 * modulator_->bits_per_ms();
+
+        INFO("Sending %u IDLE bytes", tmp);
         for (size_t i = 0; i != tmp; i++) {
             send_raw(IDLE);
         }
@@ -239,5 +255,3 @@ struct Encoder {
 };
 
 }}} // mobilinkd::tnc::hdlc
-
-#endif // INC_HDLCENCODER_HPP_
diff --git a/TNC/HdlcDecoder.cpp b/TNC/HdlcDecoder.cpp
index 789a11f..4191dc2 100644
--- a/TNC/HdlcDecoder.cpp
+++ b/TNC/HdlcDecoder.cpp
@@ -7,48 +7,6 @@
 
 namespace mobilinkd { namespace tnc { namespace hdlc {
 
-Decoder::Decoder(bool pass_all)
-: state_(SEARCH), ones_(0), buffer_(0), frame_(acquire())
-, bits_(0), passall_(pass_all), ready_(false)
-{}
-
-IoFrame* Decoder::operator()(bool bit, bool pll)
-{
-    IoFrame* result = nullptr;
-
-    // It appears that the ioFramePool may not get initialized in the proper
-    // order during some builds.
-    if (nullptr == frame_) frame_ = acquire();
-    if (nullptr == frame_) return result;
-
-    if (not pll) {
-        if ((state_ == FRAMING) and (frame_->size() >= 15) and passall_) {
-            frame_->parse_fcs();
-            if (passall_ or frame_->ok()) {
-                result = frame_;
-                ready_ = false;
-                frame_ = acquire();
-                return result;
-            }
-        }
-        reset();
-    } else {
-        if (process(bit)) {
-            ready_ = false;
-            frame_->parse_fcs();
-            if (passall_ or frame_->ok()) {
-                result = frame_;
-                frame_ = acquire();
-                if (frame_) start_hunt();
-                return result;
-            }
-            frame_->clear();
-        }
-    }
-
-    return result;
-}
-
 NewDecoder::optional_result_type NewDecoder::operator()(bool input, bool pll_lock)
 {
     optional_result_type result = nullptr;
@@ -56,9 +14,8 @@ NewDecoder::optional_result_type NewDecoder::operator()(bool input, bool pll_loc
     auto status = process(input, pll_lock);
     if (status)
     {
-//        INFO("Frame Status = %02x, size = %d, CRC = %04x",
-//            int(status), int(packet->size()), int(packet->crc()));
-        if (((status & STATUS_OK) || passall) && packet->size() > 10)
+        INFO("HDLC decode status = 0x%02x, bits = %d", int(status), int(report_bits));
+        if (can_pass(status) and packet->size() > 2)
         {
             result = packet;
             packet = nullptr;
@@ -74,12 +31,14 @@ uint8_t NewDecoder::process(bool input, bool pll_lock)
 {
     uint8_t result_code = 0;
 
+    if (state == State::IDLE and not pll_lock) return result_code;
+
     while (packet == nullptr) {
         packet = ioFramePool().acquire();
         if (!packet) osThreadYield();
     }
 
-    if (pll_lock) {
+    if (pll_lock or dcd != DCD::ON) {
         if (ones == 5) {
             if (input) {
                 // flag byte
@@ -92,6 +51,8 @@ uint8_t NewDecoder::process(bool input, bool pll_lock)
             }
         }
 
+        had_dcd |= pll_lock;
+
         buffer >>= 1;
         buffer |= (input * 128);
         bits += 1;                      // Free-running until Sync byte.
@@ -104,13 +65,36 @@ uint8_t NewDecoder::process(bool input, bool pll_lock)
         if (flag) {
             switch (buffer) {
             case 0x7E:
-                if (packet->size()) {
+                if (packet->size() > 2) {
+                    // We have started decoding a packet.
                     packet->parse_fcs();
-                    result_code = packet->ok() ? STATUS_OK : STATUS_CRC_ERROR;
+                    report_bits = bits;
+                    if (dcd == DCD::PARTIAL and not had_dcd) {
+                        // 120 (136) bits per AX.25 section 3.9.
+                        // Note we discard the flags.
+                        result_code = STATUS_NO_CARRIER;
+                    } else if (packet->ok()) {
+                        // Not compliant with AX.25 section 3.9.
+                        // We ignore byte alignment when FCS is OK.
+                        result_code = STATUS_OK;
+                    } else if (bits == 8) {
+                        // Otherwise, if there is a CRC error but we are on
+                        // an even byte boundary, flag a CRC error.  This is
+                        // used by the "pass all" rule.
+                        // Must be byte-aligned per AX.25 section 3.9.
+                        result_code = STATUS_CRC_ERROR;
+                    } else {
+                        // Extraneous bits mean we have a framing error.
+                        // We should not pass this frame up the stack.
+                        result_code = STATUS_FRAME_ERROR;
+                    }
+                } else {
+                    packet->clear();
                 }
                 state = State::SYNC;
                 flag = 0;
                 bits = 0;
+                had_dcd = false;
                 break;
             case 0xFE:
                 if (packet->size()) {
@@ -127,7 +111,8 @@ uint8_t NewDecoder::process(bool input, bool pll_lock)
             return result_code;
         }
 
-        switch (state) {
+        switch (state)
+        {
 
         case State::IDLE:
             break;
@@ -149,10 +134,33 @@ uint8_t NewDecoder::process(bool input, bool pll_lock)
         }
     } else {
         // PLL unlocked.
-        if (packet->size()) {
+        // Note the rules here are the same as above.
+        report_bits = bits;
+        had_dcd = false;
+        if (packet->size() > 2)
+        {
             packet->parse_fcs();
-            result_code = packet->ok() ? STATUS_OK | STATUS_NO_CARRIER : STATUS_NO_CARRIER;
+            if (packet->ok())
+            {
+                // Not compliant with AX.25 section 3.9.
+                // We ignore byte alignment when FCS is OK.
+                result_code = STATUS_OK;
+            }
+            else if (bits == 8)
+            {
+                // Must be byte-aligned per AX.25 section 3.9.
+                result_code = STATUS_CRC_ERROR;
+            }
+            else
+            {
+                result_code = STATUS_NO_CARRIER;
+            }
         }
+        else
+        {
+            packet->clear();
+        }
+
         if (state != State::IDLE) {
             buffer = 0;
             flag = 0;
diff --git a/TNC/HdlcDecoder.hpp b/TNC/HdlcDecoder.hpp
index ce6e746..aa971d3 100644
--- a/TNC/HdlcDecoder.hpp
+++ b/TNC/HdlcDecoder.hpp
@@ -1,8 +1,7 @@
-// Copyright 2015-2019 Mobilinkd LLC <rob@mobilinkd.com>
+// Copyright 2015-2020 Mobilinkd LLC <rob@mobilinkd.com>
 // All rights reserved.
 
-#ifndef MOBILINKD___HDLC_DECODER_HPP_
-#define MOBILINKD___HDLC_DECODER_HPP_
+#pragma once
 
 #include "HdlcFrame.hpp"
 
@@ -10,320 +9,6 @@
 
 namespace mobilinkd { namespace tnc { namespace hdlc {
 
-struct Decoder
-{
-    static const uint16_t FLAG = 0x7E00;
-    static const uint16_t ABORT = 0x7F;
-    static const uint16_t IDLE = 0xFF;
-
-    enum state {SEARCH, HUNT, FRAMING};
-
-    state state_;
-    int ones_;
-    uint16_t buffer_;
-    IoFrame* frame_;
-    int bits_;
-    bool passall_;
-    bool ready_;
-
-    Decoder(bool pass_all = false);
-
-    void reset() {
-        state_ = SEARCH;
-        ones_ = 0;
-        buffer_ = 0;
-        if (frame_) frame_->clear();
-        ready_ = false;
-    }
-
-    bool process(bool bit)
-    {
-        switch (state_)
-        {
-        case SEARCH:
-            search(bit);
-            break;
-        case HUNT:
-            hunt(bit);
-            break;
-        case FRAMING:
-            frame(bit);
-            break;
-        default:
-            reset();
-            break;
-        }
-
-        return ready_;
-    }
-
-    /**
-     * Process a bit.  If the bit results in a frame, a result set containing
-     * the frame, the FCS, and a flag indicating whether it is valid is
-     * returned.  If HDLC was contructed with passall=false, the flag returned
-     * is always true as only valid frames are returned.
-     *
-     * When PLL is passed, when true it indicates that the bit should be
-     * processed, and when false indicates that any frame in progress should
-     * be dropped and the state reset to SEARCH.
-     */
-    IoFrame* operator()(bool bit, bool pll);
-
-    void add_bit(bool bit)
-    {
-        const uint16_t BIT = 0x8000;
-
-        buffer_ >>= 1;
-        buffer_ |= (BIT * int(bit));
-        bits_ += 1;
-    }
-
-    char get_char()
-    {
-        char result = (buffer_ & 0xFF);
-
-        return result;
-    }
-
-    void consume_byte()
-    {
-        const uint16_t MASK = 0xFF00;
-
-        buffer_ &= MASK;
-        bits_ -= 8;
-    }
-
-    void consume_bit()
-    {
-        const uint16_t MASK = 0xFF00;
-
-        uint16_t tmp = (buffer_ & 0x7F);
-        tmp <<= 1;
-        buffer_ &= MASK;
-        buffer_ |= tmp;
-        bits_ -= 1;
-    }
-
-    void start_search()
-    {
-        state_ = SEARCH;
-    }
-
-    bool have_flag()
-    {
-        const uint16_t MASK = 0xFF00;
-
-        return (buffer_ & MASK) == FLAG;
-    }
-
-    void start_hunt()
-    {
-        state_ = HUNT;
-        bits_ = 0;
-        buffer_ = 0;
-    }
-
-    void search(bool bit)
-    {
-        add_bit(bit);
-
-        if (have_flag())
-        {
-            start_hunt();
-        }
-    }
-
-    bool have_frame_error()
-    {
-        switch (buffer_ & 0xFF00)
-        {
-        case 0xFF00:
-        case 0xFE00:
-        case 0xFC00:
-        case 0x7F00:
-        case 0x7E00:
-        case 0x3F00:
-            return true;
-        default:
-            return false;
-        }
-    }
-
-    bool have_bogon()
-    {
-        const uint16_t MASK = 0xFF00;
-
-        if  (bits_ != 8) return false;
-
-        const uint16_t test = (buffer_ & MASK);
-
-        switch (test)
-        {
-        case 0xFF00:
-        case 0xFE00:
-        case 0x7F00:
-            return true;
-        default:
-            return false;
-        }
-    }
-
-    void start_frame()
-    {
-        state_ = FRAMING;
-        frame_->clear();
-        ones_ = 0;
-        buffer_ &= 0xFF00;
-    }
-
-    void hunt(bool bit)
-    {
-        const uint16_t MASK = 0xFF00;
-
-        add_bit(bit);
-        buffer_ &= MASK;
-
-        if (bits_ != 8) return;
-
-        if (have_flag())  {
-            start_hunt();
-            return;
-        }
-
-        if (have_bogon()) {
-            start_search();
-            return;
-        }
-
-        if (not have_frame_error())  {
-            start_frame();
-            return;
-        }
-
-        start_search();
-    }
-
-    void frame(bool bit)
-    {
-        add_bit(bit);
-
-        if (ones_ < 5) {
-            if (buffer_ & 0x80) ones_ += 1;
-            else ones_ = 0;
-
-            if (bits_ == 16) {
-                if (frame_->push_back(get_char())) {
-                    consume_byte();
-                } else {
-                    // Allocation error.
-                    frame_->clear();
-                    start_search();
-                }
-            }
-
-            if (have_flag()) {
-                if (frame_->size() >= 15) {
-                    ready_ = true;
-                }
-            }
-        } else {
-            // 5 ones in a row means the next one should be 0 and be skipped.
-
-            if ((buffer_ & 0x80) == 0) {
-                ones_ = 0;
-                consume_bit();
-                return;
-            } else {
-
-                // Framing error.  Drop the frame.  If there is a FLAG
-                // in the buffer, go into HUNT otherwise SEARCH.
-                if (frame_->size() > 15) {
-                    ready_ = true;
-                    return;
-                }
-
-                if ((buffer_ >> (16 - bits_) & 0xFF) == 0x7E) {
-                    // Cannot call start_hunt() here because we need
-                    // to preserve buffer state.
-                    bits_ -= 8;
-                    state_ = HUNT;
-                    frame_->clear();
-                } else {
-                    start_search();
-                }
-            }
-        }
-    }
-
-    bool frame_end()
-    {
-        uint16_t tmp = (buffer_ >> (16 - bits_));
-        return (tmp & 0xFF) == FLAG;
-    }
-
-    bool frame_abort()
-    {
-        uint16_t tmp = (buffer_ >> (16 - bits_));
-        return (tmp & 0x7FFF) == 0x7FFF;
-    }
-
-    void abort_frame()
-    {
-        bits_ = 8;
-        buffer_ &= 0xFF00;
-        frame_->clear();
-    }
-
-    bool ready() const
-    {
-        return ready_;
-    }
-};
-
-#if 0
-
-Decoder::Decoder(bool pass_all)
-: state_(SEARCH), ones_(0), buffer_(0), frame_(acquire())
-, bits_(0), passall_(pass_all), ready_(false)
-{}
-
-IoFrame* Decoder::operator()(bool bit, bool pll)
-{
-    IoFrame* result = nullptr;
-
-    // It appears that the ioFramePool may not get initialized in the proper
-    // order during some builds.
-    if (nullptr == frame_) frame_ = acquire();
-
-    if (not pll) {
-        if ((state_ == FRAMING) and (frame_->size() > 15) and passall_) {
-            frame_->parse_fcs();
-            if (passall_ or frame_->ok()) {
-                result = frame_;
-                ready_ = false;
-                frame_ = acquire();
-                return result;
-            }
-        }
-        reset();
-    } else {
-        if (process(bit)) {
-            ready_ = false;
-            frame_->parse_fcs();
-            if (passall_ or frame_->ok()) {
-                result = frame_;
-                frame_ = acquire();
-                return result;
-            }
-            frame_->clear();
-        }
-    }
-
-    return result;
-}
-#endif
-
-
 struct NewDecoder
 {
     enum class State {IDLE, SYNC, RECEIVE};
@@ -338,27 +23,56 @@ struct NewDecoder
     static constexpr uint8_t STATUS_NO_CARRIER{0x10};
     static constexpr uint8_t STATUS_CRC_ERROR{0x20};
 
-    const uint16_t VALID_CRC = 0xf0b8;
+    static constexpr uint16_t VALID_CRC = 0xf0b8;
 
     State state{State::IDLE};
 
     uint8_t buffer{0};
 
     uint8_t bits{0};
+    uint8_t report_bits{0};
     uint8_t ones{0};
     bool flag{0};
+    bool had_dcd{false};
+
+    /**
+     * Tell the demodulator to return all "passable" HDLC frames.  These
+     * are frames which consist of an even multiple of eight bits and are
+     * up to 330 bytes, but which do not have a valid checksum.
+     */
     bool passall{false};
 
+    enum class DCD { ON, PARTIAL, OFF };
+
+    DCD dcd{DCD::PARTIAL};
+
     frame_type* packet{nullptr};
 
     NewDecoder(bool pass_all=false)
     : passall(pass_all)
     {}
 
+    bool can_pass(uint8_t status) const
+    {
+        return status == STATUS_OK or (passall and status == STATUS_CRC_ERROR);
+    }
+
     optional_result_type operator()(bool input, bool pll_lock);
     uint8_t process(bool input, bool pll_lock);
+    void setPassall(bool enabled)
+    {
+        passall = enabled;
+    }
+
+    void setDCD(DCD config)
+    {
+        dcd = config;
+    }
+
+    bool active() const
+    {
+        return state != State::IDLE;
+    }
 };
 
 }}} // mobilinkd::tnc::hdlc
-
-#endif // MOBILINKD___HDLC_DECODER_HPP_
diff --git a/TNC/HdlcFrame.hpp b/TNC/HdlcFrame.hpp
index d67be06..7b161fc 100644
--- a/TNC/HdlcFrame.hpp
+++ b/TNC/HdlcFrame.hpp
@@ -162,6 +162,8 @@ public:
 
     uint16_t size() const {return free_list_.size();}
 
+    static constexpr uint16_t capacity() { return SIZE; }
+
     frame_type* acquire() {
         frame_type* result = nullptr;
         auto x = taskENTER_CRITICAL_FROM_ISR();
diff --git a/TNC/IOEventTask.cpp b/TNC/IOEventTask.cpp
index 148fafc..597be66 100644
--- a/TNC/IOEventTask.cpp
+++ b/TNC/IOEventTask.cpp
@@ -12,6 +12,7 @@
 #include "Kiss.hpp"
 #include "KissHardware.hpp"
 #include "ModulatorTask.hpp"
+#include "Modulator.hpp"
 #include "UsbPort.hpp"
 #include "SerialPort.hpp"
 #include "NullPort.hpp"
@@ -151,6 +152,7 @@ void startIOEventTask(void const*)
                     hpcd_USB_FS.Instance->BCDR = 0;
                     HAL_PCD_MspDeInit(&hpcd_USB_FS);
                     HAL_GPIO_WritePin(USB_CE_GPIO_Port, USB_CE_Pin, GPIO_PIN_SET);
+//                    SysClock4();
                     if (ioport != getUsbPort())
                     {
                         break;
@@ -272,6 +274,7 @@ void startIOEventTask(void const*)
                 break;
             case CMD_USB_CONNECTED:
                 INFO("VBUS Detected");
+//                SysClock48();
                 MX_USB_DEVICE_Init();
                 HAL_PCD_MspInit(&hpcd_USB_FS);
                 hpcd_USB_FS.Instance->BCDR = 0;
@@ -372,6 +375,10 @@ void startIOEventTask(void const*)
         case IoFrame::FRAME_RETURN:
             hdlc::release(frame);
             break;
+        default:
+            ERROR("Unknown Frame Type");
+            hdlc::release(frame);
+            break;
         }
     }
 }
diff --git a/TNC/Kiss.cpp b/TNC/Kiss.cpp
index 72c84cc..eb96cd6 100644
--- a/TNC/Kiss.cpp
+++ b/TNC/Kiss.cpp
@@ -4,6 +4,7 @@
 #include <Log.h>
 #include "Kiss.hpp"
 #include "KissHardware.hpp"
+#include "ModulatorTask.hpp"
 
 // extern osMessageQId hdlcOutputQueueHandle;
 
@@ -26,16 +27,19 @@ void handle_frame(uint8_t frame_type, hdlc::IoFrame* frame) {
         DEBUG("FRAME_TX_DELAY");
         kiss::settings().txdelay = value;
         hdlc::release(frame);
+        updateModulator();
         break;
     case kiss::FRAME_P_PERSIST:
         DEBUG("FRAME_P_PERSIST");
         kiss::settings().ppersist = value;
         hdlc::release(frame);
+        updateModulator();
         break;
     case kiss::FRAME_SLOT_TIME:
         DEBUG("FRAME_SLOT_TIME");
         kiss::settings().slot = value;
         hdlc::release(frame);
+        updateModulator();
         break;
     case kiss::FRAME_TX_TAIL:
         DEBUG("FRAME_TX_TAIL");
@@ -46,6 +50,7 @@ void handle_frame(uint8_t frame_type, hdlc::IoFrame* frame) {
         DEBUG("FRAME_DUPLEX");
         kiss::settings().duplex = value;
         hdlc::release(frame);
+        updateModulator();
         break;
     case kiss::FRAME_HARDWARE:
         DEBUG("FRAME_HARDWARE");
diff --git a/TNC/Kiss.hpp b/TNC/Kiss.hpp
index eda3273..a07959c 100644
--- a/TNC/Kiss.hpp
+++ b/TNC/Kiss.hpp
@@ -46,9 +46,7 @@ struct slip_encoder
 
     slip_encoder()
     : packet_(0), size_(0), pos_(0), current_(0), shifting_(false)
-    {
-        set_current();
-    }
+    {}
 
     slip_encoder(const char* packet, size_t len)
     : packet_(packet), size_(len), pos_(0), current_(0), shifting_(false)
@@ -108,23 +106,23 @@ struct slip_encoder
 };
 
 /**
- * This is a forward iterator adapter that SLIP-encodes the data from the
+ * This is a input iterator adapter that SLIP-encodes the data from the
  * underlying frame iterator.  The only requirement is that the underlying
- * iterator meets the requirements of a forward iterator and that its value
+ * iterator meets the requirements of an input iterator and that its value
  * type is convertible to char.
  */
 struct slip_encoder2
 {
-    typedef std::forward_iterator_tag iterator_category;
+    typedef std::input_iterator_tag iterator_category;
     typedef size_t difference_type;
     typedef char value_type;
     typedef char& reference;
     typedef char* pointer;
 
-    static const uint8_t FEND = 0xC0;
-    static const uint8_t FESC = 0xDB;
-    static const uint8_t TFEND = 0xDC;
-    static const uint8_t TFESC = 0xDD;
+    static const char FEND = 0xC0;
+    static const char FESC = 0xDB;
+    static const char TFEND = 0xDC;
+    static const char TFESC = 0xDD;
 
     hdlc::IoFrame::iterator iter_;
     mutable bool shifting_;
@@ -161,7 +159,7 @@ struct slip_encoder2
      * @return the slip-encoded character.
      */
     char operator*() const {
-        uint8_t c = *iter_;
+        char c = *iter_;
 
         if (shifting_) {
             return c == FEND ? TFEND : TFESC;
@@ -171,7 +169,7 @@ struct slip_encoder2
             return FESC;
         }
 
-        return *iter_;
+        return c;
     }
 
     /**
@@ -192,7 +190,7 @@ struct slip_encoder2
      * @return a reference to *this.
      */
     slip_encoder2& operator++() {
-        uint8_t c = *iter_;
+        char c = *iter_;
         if (not shifting_ and ((c == FEND) or (c == FESC))) {
             shifting_ = true;
             return *this;
@@ -238,9 +236,7 @@ struct slip_decoder
 
     slip_decoder()
     : packet_(0), size_(0), pos_(0), current_(0)
-    {
-        set_current();
-    }
+    {}
 
     slip_decoder(const char* packet, size_t len)
     : packet_(packet), size_(len), pos_(0), current_(0)
diff --git a/TNC/KissHardware.cpp b/TNC/KissHardware.cpp
index 51da667..5d26539 100644
--- a/TNC/KissHardware.cpp
+++ b/TNC/KissHardware.cpp
@@ -6,9 +6,12 @@
 #include "AudioInput.hpp"
 #include "AudioLevel.hpp"
 #include "IOEventTask.h"
-#include <ModulatorTask.hpp>
+#include "ModulatorTask.hpp"
+#include "Modulator.hpp"
+#include "HDLCEncoder.hpp"
 
 #include <memory>
+#include <array>
 #include <cstdio>
 
 extern I2C_HandleTypeDef hi2c1;
@@ -26,9 +29,10 @@ int powerOffViaUSB(void)
 
 namespace mobilinkd { namespace tnc { namespace kiss {
 
-const char FIRMWARE_VERSION[] = "1.1.7";
+const char FIRMWARE_VERSION[] = "2.0.1";
 const char HARDWARE_VERSION[] = "Mobilinkd TNC3 2.1.1";
 
+
 Hardware& settings()
 {
     static Hardware instance __attribute__((section(".bss3")));
@@ -98,9 +102,7 @@ void Hardware::set_duplex(uint8_t value) {
     update_crc();
 }
 
-#if 1
-
- void reply8(uint8_t cmd, uint8_t result) {
+void reply8(uint8_t cmd, uint8_t result) {
     uint8_t data[2] { cmd, result };
     ioport->write(data, 2, 6, osWaitForever);
 }
@@ -110,7 +112,7 @@ void reply16(uint8_t cmd, uint16_t result) {
     ioport->write(data, 3, 6, osWaitForever);
 }
 
-inline void reply(uint8_t cmd, const uint8_t* data, uint16_t len) {
+void reply(uint8_t cmd, const uint8_t* data, uint16_t len) {
     uint8_t* buffer = static_cast<uint8_t*>(alloca(len + 1));
     buffer[0] = cmd;
     for (uint16_t i = 0; i != len; i++)
@@ -118,17 +120,33 @@ inline void reply(uint8_t cmd, const uint8_t* data, uint16_t len) {
     ioport->write(buffer, len + 1, 6, osWaitForever);
 }
 
-inline void reply_ext(uint8_t ext, uint8_t cmd, const uint8_t* data, uint16_t len) {
-    auto buffer = (uint8_t*) malloc(len + 2);
+template <size_t N>
+void reply_ext(const std::array<uint8_t, N>& cmd, const uint8_t* data, uint16_t len) {
+    auto buffer = static_cast<uint8_t*>(alloca(len + N));
     if (buffer == nullptr) return;
-    buffer[0] = ext;
-    buffer[1] = cmd;
+    std::copy(std::begin(cmd), std::end(cmd), buffer);
     for (uint16_t i = 0; i != len and data[i] != 0; i++)
-        buffer[i + 2] = data[i];
+        buffer[i + N] = data[i];
     ioport->write(buffer, len + 2, 6, osWaitForever);
     free(buffer);
 }
-#endif
+
+
+template <size_t N>
+inline void ext_reply(const std::array<uint8_t, N>& cmd, uint8_t result) {
+    std::array<uint8_t, 1 + N> buffer;
+    std::copy(std::begin(cmd), std::end(cmd), std::begin(buffer));
+    buffer[N] = result;
+    ioport->write(buffer.data(), N + 1, 6, osWaitForever);
+}
+
+template <size_t M, size_t N>
+void ext_reply(const std::array<uint8_t, M>& cmd, const std::array<uint8_t, N>& result) {
+    std::array<uint8_t, M + N> data;
+    std::copy(std::begin(cmd), std::end(cmd), std::begin(data));
+    std::copy(std::begin(result), std::end(result), std::begin(data) + M);
+    ioport->write(data.data(), M + N, 6, osWaitForever);
+}
 
 void Hardware::get_alias(uint8_t alias) {
     uint8_t result[14];
@@ -140,7 +158,7 @@ void Hardware::get_alias(uint8_t alias) {
     result[11] = aliases[alias].insert_id;
     result[12] = aliases[alias].preempt;
     result[13] = aliases[alias].hops;
-    reply_ext(hardware::EXTENDED_CMD, hardware::EXT_GET_ALIASES, result, 14);
+    reply_ext(hardware::EXT_GET_ALIASES, result, 14);
 }
 
 void Hardware::set_alias(const hdlc::IoFrame* frame) {
@@ -178,14 +196,13 @@ void Hardware::handle_request(hdlc::IoFrame* frame) {
 
     switch (command) {
 
-#if 1
     case hardware::SAVE:
     case hardware::SAVE_EEPROM_SETTINGS:
         update_crc();
         store();
         reply8(hardware::OK, hardware::SAVE_EEPROM_SETTINGS);
         break;
-#endif
+
     case hardware::POLL_INPUT_LEVEL:
         DEBUG("POLL_INPUT_VOLUME");
         reply8(hardware::POLL_INPUT_LEVEL, 0);
@@ -337,7 +354,7 @@ void Hardware::handle_request(hdlc::IoFrame* frame) {
         if (tx_twist < 0) tx_twist = 0;
         if (tx_twist > 100) tx_twist = 100;
         DEBUG("SET_OUTPUT_TWIST: %d", int(tx_twist));
-        getModulator().set_twist(uint8_t(tx_twist));
+        getModulator().init(*this);
         update_crc();
         [[fallthrough]];
     case hardware::GET_OUTPUT_TWIST:
@@ -411,6 +428,20 @@ void Hardware::handle_request(hdlc::IoFrame* frame) {
             options & KISS_OPTION_PTT_SIMPLEX ? 0 : 1);
         break;
 
+    case hardware::SET_PASSALL:
+        DEBUG("SET_PASSALL");
+        if (*it) {
+          options |= KISS_OPTION_PASSALL;
+        } else {
+          options &= ~KISS_OPTION_PASSALL;
+        }
+        update_crc();
+        [[fallthrough]];
+    case hardware::GET_PASSALL:
+        DEBUG("GET_PASSALL");
+        reply8(hardware::GET_PASSALL, options & KISS_OPTION_PASSALL ? 1 : 0);
+        break;
+
     case hardware::SET_USB_POWER_OFF:
         DEBUG("SET_USB_POWER_OFF");
         if (*it) {
@@ -459,11 +490,10 @@ void Hardware::handle_request(hdlc::IoFrame* frame) {
 
     case hardware::GET_ALL_VALUES:
         DEBUG("GET_ALL_VALUES");
+        // GET_API_VERSION must always come first.
         reply16(hardware::GET_API_VERSION, hardware::KISS_API_VERSION);
         osMessagePut(audioInputQueueHandle, audio::POLL_BATTERY_LEVEL,
             osWaitForever);
-        osMessagePut(audioInputQueueHandle, audio::POLL_TWIST_LEVEL,
-            osWaitForever);
         osMessagePut(audioInputQueueHandle, audio::IDLE,
             osWaitForever);
         reply(hardware::GET_FIRMWARE_VERSION, (uint8_t*) FIRMWARE_VERSION,
@@ -485,6 +515,7 @@ void Hardware::handle_request(hdlc::IoFrame* frame) {
         reply8(hardware::GET_DUPLEX, duplex);
         reply8(hardware::GET_PTT_CHANNEL,
             options & KISS_OPTION_PTT_SIMPLEX ? 0 : 1);
+        reply8(hardware::GET_PASSALL, options & KISS_OPTION_PASSALL ? 1 : 0);
         reply16(hardware::GET_CAPABILITIES,
             hardware::CAP_EEPROM_SAVE|hardware::CAP_BATTERY_LEVEL|
             hardware::CAP_ADJUST_INPUT|hardware::CAP_DFU_FIRMWARE);
@@ -493,44 +524,61 @@ void Hardware::handle_request(hdlc::IoFrame* frame) {
         reply8(hardware::GET_MIN_INPUT_TWIST, -3);  // Constants for this FW
         reply8(hardware::GET_MAX_INPUT_TWIST, 9);   // Constants for this FW
         reply(hardware::GET_MAC_ADDRESS, mac_address, sizeof(mac_address));
-        reply(hardware::GET_DATETIME, get_rtc_datetime(), 7);
+        ext_reply(hardware::EXT_GET_MODEM_TYPE, modem_type);
+        ext_reply(hardware::EXT_GET_MODEM_TYPES, supported_modem_types);
         if (*error_message) {
             reply(hardware::GET_ERROR_MSG, (uint8_t*) error_message, sizeof(error_message));
         }
-        break;
-
-    case hardware::EXTENDED_CMD:
-        handle_ext_request(frame);
+        // GET_DATETIME must always be last.  iOS config app depends on it.
+        reply(hardware::GET_DATETIME, get_rtc_datetime(), 7);
         break;
 
     default:
-        ERROR("Unknown hardware request");
+        if (command > 0xC0)
+        {
+            handle_ext_request(frame);
+        }
+        else
+        {
+            ERROR("Unknown hardware request");
+        }
     }
 }
 
-inline void ext_reply(uint8_t cmd, uint8_t result) {
-    uint8_t data[3] { hardware::EXTENDED_CMD, cmd, result };
-    ioport->write(data, 3, 6);
-}
-
 void Hardware::handle_ext_request(hdlc::IoFrame* frame) {
     auto it = frame->begin();
     ++it;
+    // Currently only supports 2-byte extended commands.
     uint8_t ext_command = *it++;
 
     switch (ext_command) {
-    case hardware::EXT_GET_MODEM_TYPE:
+
+    case hardware::EXT_SET_MODEM_TYPE[1]:
+        DEBUG("SET_MODEM_TYPE");
+        if ((*it == hardware::MODEM_TYPE_1200)
+            or (*it == hardware::MODEM_TYPE_9600))
+        {
+            modem_type = *it;
+            DEBUG(modem_type_lookup[modem_type]);
+            update_crc();
+        }
+        else
+        {
+            ERROR("Unsupported modem type");
+        }
+        osMessagePut(audioInputQueueHandle, audio::UPDATE_SETTINGS,
+            osWaitForever);
+        [[fallthrough]];
+    case hardware::EXT_GET_MODEM_TYPE[1]:
         DEBUG("EXT_GET_MODEM_TYPE");
-        ext_reply(hardware::EXT_GET_MODEM_TYPE, 1);
+        ext_reply(hardware::EXT_GET_MODEM_TYPE, modem_type);
         break;
-    case hardware::EXT_SET_MODEM_TYPE:
-        DEBUG("EXT_SET_MODEM_TYPE");
-        ext_reply(hardware::EXT_OK, hardware::EXT_SET_MODEM_TYPE);
-        break;
-    case hardware::EXT_GET_MODEM_TYPES:
+    case hardware::EXT_GET_MODEM_TYPES[1]:
         DEBUG("EXT_GET_MODEM_TYPES");
-        ext_reply(hardware::EXT_GET_MODEM_TYPES, 1);
+        ext_reply(hardware::EXT_GET_MODEM_TYPES, supported_modem_types);
         break;
+    default:
+        ERROR("Unknown extended hardware request");
     }
 }
 
diff --git a/TNC/KissHardware.hpp b/TNC/KissHardware.hpp
index 527aff9..0533d6b 100644
--- a/TNC/KissHardware.hpp
+++ b/TNC/KissHardware.hpp
@@ -1,8 +1,7 @@
-// Copyright 2015 Mobilinkd LLC <rob@mobilinkd.com>
+// Copyright 2015-2020 Mobilinkd LLC <rob@mobilinkd.com>
 // All rights reserved.
 
-#ifndef MOBILINKD__TNC__KISS_HARDWARE_HPP_
-#define MOBILINKD__TNC__KISS_HARDWARE_HPP_
+#pragma once
 
 #include "KissHardware.h"
 #include "Log.h"
@@ -35,7 +34,7 @@ namespace hardware {
  * The major version should be updated whenever non-backwards compatible
  * changes to the API are made.
  */
-constexpr const uint16_t KISS_API_VERSION = 0x0200;
+constexpr const uint16_t KISS_API_VERSION = 0x0201;
 
 constexpr const uint16_t CAP_DCD = 0x0100;
 constexpr const uint16_t CAP_SQUELCH = 0x0200;
@@ -106,6 +105,8 @@ constexpr const uint8_t GET_MAC_ADDRESS = 48;
 constexpr const uint8_t GET_DATETIME = 49;
 constexpr const uint8_t SET_DATETIME = 50;
 constexpr const uint8_t GET_ERROR_MSG = 51;
+constexpr const uint8_t GET_SNR = 52;
+constexpr const uint8_t GET_BER = 53;
 
 constexpr const uint8_t SET_BLUETOOTH_NAME = 65;
 constexpr const uint8_t GET_BLUETOOTH_NAME = 66;
@@ -126,6 +127,9 @@ constexpr const uint8_t GET_BT_POWER_OFF = 78;
 constexpr const uint8_t SET_PTT_CHANNEL = 79; // Which PTT line to use (currently 0 or 1,
 constexpr const uint8_t GET_PTT_CHANNEL = 80; // multiplex or simplex)
 
+constexpr const uint8_t SET_PASSALL = 81;   // Allow invalid CRC through when
+constexpr const uint8_t GET_PASSALL = 82;   // true (1).
+
 constexpr const uint8_t GET_MIN_OUTPUT_TWIST = 119;  ///< int8_t (may be negative).
 constexpr const uint8_t GET_MAX_OUTPUT_TWIST = 120;  ///< int8_t (may be negative).
 constexpr const uint8_t GET_MIN_INPUT_TWIST = 121;  ///< int8_t (may be negative).
@@ -137,34 +141,47 @@ constexpr const uint8_t GET_CAPABILITIES = 126;   ///< Send all capabilities.
 constexpr const uint8_t GET_ALL_VALUES = 127;     ///< Send all settings & versions.
 
 /**
- * Extended commands are two+ bytes in length.  They start at 80:00
- * and go through BF:FF (14 significant bits), then proceed to C0:00:00
- * through CF:FF:FF (20 more significant bits).
+ * Extended commands are two+ bytes in length.  They follow the model used
+ * by UTF-8 to extend the command set.  Extended commands start at C1:80
+ * and go through DF:BF (11 significant bits), then proceed to E0:80:80
+ * through EF:BF:BF (16 significant bits).
  *
- * If needed, the commands can be extended to 9 nibbles (D0 - DF),
- * 13 nibbles (E0-EF) and 17 nibbles (F0-FF).
+ * To avoid special KISS characters, we skip the following byte values in
+ * the first byte:
+ *
+ * - 0xC0 / FEND
+ * - 0xDB / FESC
+ * - 0xDC / TFEND
+ * - 0xDD / TFESC
  */
-constexpr const uint8_t EXTENDED_CMD = 128;
+constexpr const uint8_t EXTENDED_CMD = 0xC1;
 
-constexpr const uint8_t EXT_OK = 0;
-constexpr const uint8_t EXT_GET_MODEM_TYPE = 1;
-constexpr const uint8_t EXT_SET_MODEM_TYPE = 2;
-constexpr const uint8_t EXT_GET_MODEM_TYPES = 3;  ///< Return a list of supported modem types
+constexpr const uint8_t EXT_OK = 0x80;
 
-constexpr const uint8_t EXT_GET_ALIASES = 8;    ///< Number of aliases supported
-constexpr const uint8_t EXT_GET_ALIAS = 9;      ///< Alias number (uint8_t), 8 characters, 5 bytes (set, use, insert_id, preempt, hops)
-constexpr const uint8_t EXT_SET_ALIAS = 10;     ///< Alias number (uint8_t), 8 characters, 5 bytes (set, use, insert_id, preempt, hops)
+constexpr std::array<uint8_t, 2> EXT_GET_MODEM_TYPE = {0xC1, 0x81};
+constexpr std::array<uint8_t, 2> EXT_SET_MODEM_TYPE = {0xC1, 0x82};
+constexpr std::array<uint8_t, 2> EXT_GET_MODEM_TYPES = {0xC1, 0x83};    ///< Return a list of supported modem types
 
-constexpr const uint8_t EXT_GET_BEACONS = 12;   ///< Number of beacons supported
-constexpr const uint8_t EXT_GET_BEACON = 13;    ///< Beacon number (uint8_t), uint16_t interval in seconds, 3 NUL terminated strings (callsign, path, text)
-constexpr const uint8_t EXT_SET_BEACON = 14;    ///< Beacon number (uint8_t), uint16_t interval in seconds, 3 NUL terminated strings (callsign, path, text)
+constexpr std::array<uint8_t, 2> EXT_GET_ALIASES = {0xC1, 0x88};        ///< Number of aliases supported
+constexpr std::array<uint8_t, 2> EXT_GET_ALIAS = {0xC1, 0x89};          ///< Alias number (uint8_t), 8 characters, 5 bytes (set, use, insert_id, preempt, hops)
+constexpr std::array<uint8_t, 2> EXT_SET_ALIAS = {0xC1, 0x0A};          ///< Alias number (uint8_t), 8 characters, 5 bytes (set, use, insert_id, preempt, hops)
 
-constexpr const uint8_t MODEM_TYPE_1200 = 1;
-constexpr const uint8_t MODEM_TYPE_300 = 2;
-constexpr const uint8_t MODEM_TYPE_9600 = 3;
-constexpr const uint8_t MODEM_TYPE_PSK31 = 4;
-constexpr const uint8_t MODEM_TYPE_OFDM = 5;
-constexpr const uint8_t MODEM_TYPE_MFSK16 = 6;
+constexpr std::array<uint8_t, 2> EXT_GET_BEACON_SLOTS = {0xC1, 0x8C};   ///< Number of beacons supported
+constexpr std::array<uint8_t, 2> EXT_GET_BEACON = {0xC1, 0x8D};         ///< Beacon number (uint8_t), uint16_t interval in seconds, 3 NUL terminated strings (callsign, path, text)
+constexpr std::array<uint8_t, 2> EXT_SET_BEACON = {0xC1, 0x8E};         ///< Beacon number (uint8_t), uint16_t interval in seconds, 3 NUL terminated strings (callsign, path, text)
+
+
+/*
+ * Modem type values 0x00 - 0x7F are single-byte types.  Modem type values
+ * starting at 0xC0 are multi-byte values, and should follow the model used
+ * by UTF-8 to extend the modem types, should that ever be necessary.
+ */
+constexpr uint8_t MODEM_TYPE_1200 = 1;
+constexpr uint8_t MODEM_TYPE_300 = 2;
+constexpr uint8_t MODEM_TYPE_9600 = 3;
+constexpr uint8_t MODEM_TYPE_PSK31 = 4;
+constexpr uint8_t MODEM_TYPE_OFDM = 5;
+constexpr uint8_t MODEM_TYPE_MFSK16 = 6;
 
 // Boolean options.
 #define KISS_OPTION_CONN_TRACK      0x01
@@ -172,6 +189,7 @@ constexpr const uint8_t MODEM_TYPE_MFSK16 = 6;
 #define KISS_OPTION_VIN_POWER_ON    0x04  // Power on when plugged into USB
 #define KISS_OPTION_VIN_POWER_OFF   0x08  // Power off when unplugged from USB
 #define KISS_OPTION_PTT_SIMPLEX     0x10  // Simplex PTT (the default)
+#define KISS_OPTION_PASSALL         0x20  // Ignore invalid CRC.
 
 const char TOCALL[] = "APML30"; // Update for every feature change.
 
@@ -207,11 +225,24 @@ const size_t NUMBER_OF_BEACONS = 4;     // 680 bytes
  */
 struct Hardware
 {
+    static constexpr std::array<const char*, 4> modem_type_lookup = {
+        "NOT SET",
+        "AFSK1200",
+        "AFSK300",
+        "FSK9600",
+    };
+
+    // This must match the constants defined above.
     enum ModemType {
-        AFSK1200 = 1,
-        AFSK300,
-        FSK9600,
-        PSK31
+        AFSK1200 = hardware::MODEM_TYPE_1200,
+        AFSK300 = hardware::MODEM_TYPE_300,
+        FSK9600 = hardware::MODEM_TYPE_9600,
+        PSK31 = hardware::MODEM_TYPE_PSK31
+    };
+
+    static constexpr std::array<uint8_t, 2> supported_modem_types = {
+        hardware::MODEM_TYPE_1200,
+        hardware::MODEM_TYPE_9600
     };
 
     uint8_t txdelay;        ///< How long in 10mS units to wait for TX to settle before starting data
@@ -296,13 +327,13 @@ struct Hardware
         DEBUG("Slot Time: %d", (int)slot);
         DEBUG("TX Tail: %d", (int)txtail);
         DEBUG("Duplex: %d", (int)duplex);
-        DEBUG("Modem Type: %d", (int)modem_type);
+        DEBUG("Modem Type: %s", modem_type_lookup[modem_type]);
         DEBUG("TX Gain: %d", (int)output_gain);
         DEBUG("RX Gain: %d", (int)input_gain);
         DEBUG("TX Twist: %d", (int)tx_twist);
         DEBUG("RX Twist: %d", (int)rx_twist);
         DEBUG("Log Level: %d", (int)log_level);
-        DEBUG("Options: %d", (int)options);
+        DEBUG("Options: %04hx", options);
         DEBUG("MYCALL: %s", (char*) mycall);
         DEBUG("Dedupe time (secs): %d", (int)dedupe_seconds);
         DEBUG("Aliases:");
@@ -322,7 +353,7 @@ struct Hardware
             DEBUG(" text: %s", (char*)b.text);
             DEBUG(" frequency (secs): %d", (int)b.seconds);
         }
-        DEBUG("Checksum: %04x", checksum);
+        DEBUG("Checksum: %04hx", checksum);
     }
 
     bool load();
@@ -347,7 +378,6 @@ struct Hardware
 
 extern Hardware& settings();
 
-
 struct I2C_Storage
 {
     constexpr static const uint16_t i2c_address{EEPROM_ADDRESS};
@@ -374,6 +404,6 @@ void reply8(uint8_t cmd, uint8_t result) __attribute__((noinline));
 
 void reply16(uint8_t cmd, uint16_t result) __attribute__((noinline));
 
-}}} // mobilinkd::tnc::kiss
+void reply(uint8_t cmd, const uint8_t* data, uint16_t len) __attribute__((noinline));
 
-#endif // INMOBILINKD__TNC__KISS_HARDWARE_HPP_C_KISSHARDWARE_HPP_
+}}} // mobilinkd::tnc::kiss
diff --git a/TNC/Modulator.hpp b/TNC/Modulator.hpp
new file mode 100644
index 0000000..417e4f8
--- /dev/null
+++ b/TNC/Modulator.hpp
@@ -0,0 +1,132 @@
+// Copyright 2020 Rob Riggs <rob@mobilinkd.com>
+// All rights reserved.
+
+#pragma once
+
+#include "PTT.hpp"
+#include "KissHardware.hpp"
+
+#include "stm32l4xx_hal.h"
+#include "cmsis_os.h"
+
+#include <cstdint>
+
+extern osMessageQId hdlcOutputQueueHandle;
+extern osMessageQId dacOutputQueueHandle;
+extern TIM_HandleTypeDef htim7;
+extern DAC_HandleTypeDef hdac1;
+
+namespace mobilinkd { namespace tnc {
+
+/**
+ * The modulator has three distinct interfaces.  The configuration interface
+ * which is used to initialize the modulator, the bit sending interface used
+ * by the HDLC encoder to transmit the bits, and the modulation interface
+ * used by the DAC DMA engine to get the analog values output by the modem.
+ */
+struct Modulator
+{
+    virtual ~Modulator() {}
+
+    /**
+     * Implement all functionality required to configure the hardware and
+     * the modulator.  For example, configuring the timer used by the DAC
+     * should be done here.
+     */
+    virtual void init(const kiss::Hardware& hw) = 0;
+
+    /**
+     * Implement all functionality required to deactivate the hardware and
+     * the modulator.  For example, disabling the timer used by the DAC
+     * should be done here.
+     */
+    virtual void deinit() = 0;
+
+    virtual void set_gain(uint16_t level) = 0;
+
+    /**
+     * Set the PTT controller used by the modulator.  It is only legal
+     * to call this when the modulator is stopped.
+     *
+     * @param ptt
+     */
+    virtual void set_ptt(PTT* ptt) = 0;
+
+    /**
+     * Send a single bit.
+     *
+     * @param bit
+     */
+    virtual void send(bool bit) = 0;
+
+    /// The next three functions are called by the DAC DMA interrupt handler.
+
+    /**
+     * Fill the first half of the DAC DMA buffer.
+     *
+     * @warning This function is called in an interrupt context.
+     *
+     * @param bit
+     */
+    virtual void fill_first(bool bit) = 0;
+
+    /**
+     * Fill the second half of the DAC DMA buffer.
+     *
+     * @warning This function is called in an interrupt context.
+     *
+     * @param bit
+     */
+    virtual void fill_last(bool bit) = 0;
+
+    /**
+     * The DAC bit buffer is empty.  There are no more bits to process.
+     *
+     * @warning This function is called in an interrupt context.
+     */
+    virtual void empty() = 0;
+
+    virtual void abort() = 0;
+
+    virtual float bits_per_ms() const = 0;
+
+protected:
+
+    /**
+     * Stop the DMA conversion and the timer.  Configure DAC for no
+     * trigger and set the DAC level to exactly mid-level.
+     *
+     * @note The DAC is set to mid-level to ensure the audio coupling
+     *  capacitor is kept biased to ensure that there is no DC level
+     *  ramp when we start conversions.  This is bad for FSK.
+     */
+    void stop_conversion()
+    {
+        HAL_DAC_Stop_DMA(&hdac1, DAC_CHANNEL_1);
+        HAL_TIM_Base_Stop(&htim7);
+
+        DAC_ChannelConfTypeDef sConfig;
+
+        sConfig.DAC_SampleAndHold = DAC_SAMPLEANDHOLD_DISABLE;
+        sConfig.DAC_Trigger = DAC_TRIGGER_NONE;
+        sConfig.DAC_OutputBuffer = DAC_OUTPUTBUFFER_ENABLE;
+        sConfig.DAC_ConnectOnChipPeripheral = DAC_CHIPCONNECT_ENABLE;
+        sConfig.DAC_UserTrimming = DAC_TRIMMING_FACTORY;
+        if (HAL_DAC_ConfigChannel(&hdac1, &sConfig, DAC_CHANNEL_1) != HAL_OK)
+        {
+          CxxErrorHandler();
+        }
+
+        if (HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, 2048) != HAL_OK)
+        {
+            CxxErrorHandler();
+        }
+        if (HAL_DAC_Start(&hdac1, DAC_CHANNEL_1) != HAL_OK)
+        {
+            CxxErrorHandler();
+        }
+    }
+
+};
+
+}} // mobilinkd::tnc
diff --git a/TNC/ModulatorTask.cpp b/TNC/ModulatorTask.cpp
index 286a52f..6705a5e 100644
--- a/TNC/ModulatorTask.cpp
+++ b/TNC/ModulatorTask.cpp
@@ -2,13 +2,17 @@
 // All rights reserved.
 
 #include "ModulatorTask.hpp"
+#include "HDLCEncoder.hpp"
+#include "Modulator.hpp"
+#include "Fsk9600Modulator.hpp"
+#include "AFSKModulator.hpp"
 #include "KissHardware.hpp"
 #include "main.h"
 
 mobilinkd::tnc::SimplexPTT simplexPtt;
 mobilinkd::tnc::MultiplexPTT multiplexPtt;
 
-mobilinkd::tnc::AFSKModulator* modulator;
+mobilinkd::tnc::Modulator* modulator;
 mobilinkd::tnc::hdlc::Encoder* encoder;
 
 // DMA Conversion half complete.
@@ -34,13 +38,26 @@ extern "C" void HAL_DAC_DMAUnderrunCallbackCh1(DAC_HandleTypeDef*) {
     modulator->abort();
 }
 
-mobilinkd::tnc::AFSKModulator& getModulator() {
-    static mobilinkd::tnc::AFSKModulator instance(dacOutputQueueHandle, &simplexPtt);
-    return instance;
+mobilinkd::tnc::Modulator& getModulator()
+{
+    using namespace mobilinkd::tnc;
+
+    static AFSKModulator afsk1200modulator(dacOutputQueueHandle, &simplexPtt);
+    static Fsk9600Modulator fsk9600modulator(dacOutputQueueHandle, &simplexPtt);
+
+    switch (kiss::settings().modem_type)
+    {
+    case kiss::Hardware::ModemType::FSK9600:
+        return fsk9600modulator;
+    case kiss::Hardware::ModemType::AFSK1200:
+        return afsk1200modulator;
+    default:
+        CxxErrorHandler();
+    }
 }
 
 mobilinkd::tnc::hdlc::Encoder& getEncoder() {
-    static mobilinkd::tnc::hdlc::Encoder instance(hdlcOutputQueueHandle, &getModulator());
+    static mobilinkd::tnc::hdlc::Encoder instance(hdlcOutputQueueHandle);
     return instance;
 }
 
@@ -61,9 +78,19 @@ void updatePtt()
     using namespace mobilinkd::tnc::kiss;
 
     if (settings().options & KISS_OPTION_PTT_SIMPLEX)
-        modulator->set_ptt(&simplexPtt);
+        getModulator().set_ptt(&simplexPtt);
     else
-        modulator->set_ptt(&multiplexPtt);
+        getModulator().set_ptt(&multiplexPtt);
+}
+
+void updateModulator()
+{
+    using namespace mobilinkd::tnc::kiss;
+
+    modulator = &getModulator();
+    modulator->init(settings());
+    updatePtt();
+    encoder->updateModulator();
 }
 
 void startModulatorTask(void const*) {
@@ -78,7 +105,7 @@ void startModulatorTask(void const*) {
 
     updatePtt();
 
-    modulator->set_twist(settings().tx_twist);
+    getModulator().init(settings());
 
     encoder->tx_delay(settings().txdelay);
     encoder->p_persist(settings().ppersist);
diff --git a/TNC/ModulatorTask.hpp b/TNC/ModulatorTask.hpp
index 9f9ccfe..903602c 100644
--- a/TNC/ModulatorTask.hpp
+++ b/TNC/ModulatorTask.hpp
@@ -1,12 +1,8 @@
-// Copyright 2015 Mobilinkd LLC <rob@mobilinkd.com>
+// Copyright 2015-2020 Mobilinkd LLC <rob@mobilinkd.com>
 // All rights reserved.
 
+#pragma once
 
-#ifndef MOBILINKD__MODULATOR_TASK_HPP_
-#define MOBILINKD__MODULATOR_TASK_HPP_
-
-#include "HDLCEncoder.hpp"
-#include "AFSKModulator.hpp"
 #include "PTT.hpp"
 #include "cmsis_os.h"
 
@@ -14,13 +10,20 @@
 extern "C" {
 #endif
 
+namespace mobilinkd { namespace tnc {
+
+class Modulator;
+
+namespace hdlc {
+class Encoder;
+}
+
+}}
+
 extern mobilinkd::tnc::SimplexPTT simplexPtt;
 extern mobilinkd::tnc::MultiplexPTT multiplexPtt;
 
-extern mobilinkd::tnc::AFSKModulator* modulator;
-extern mobilinkd::tnc::hdlc::Encoder* encoder;
-
-mobilinkd::tnc::AFSKModulator& getModulator();
+mobilinkd::tnc::Modulator& getModulator();
 mobilinkd::tnc::hdlc::Encoder& getEncoder();
 
 void startModulatorTask(void const * argument);
@@ -30,9 +33,8 @@ enum class PTT {SIMPLEX, MULTIPLEX};
 void setPtt(PTT ptt);
 
 void updatePtt(void);
+void updateModulator(void);
 
 #ifdef __cplusplus
 }
 #endif
-
-#endif // MOBILINKD__MODULATOR_TASK_HPP_
diff --git a/TNC/SerialPort.cpp b/TNC/SerialPort.cpp
index 36685bd..b3a37d6 100644
--- a/TNC/SerialPort.cpp
+++ b/TNC/SerialPort.cpp
@@ -13,20 +13,28 @@
 #include "cmsis_os.h"
 
 #include <cstdlib>
+#include <cstdint>
 #include <cstring>
 #include <atomic>
 
 extern UART_HandleTypeDef huart3;
 extern osMessageQId ioEventQueueHandle;
 
-uint8_t rxBuffer;
-std::atomic<int> uart_error{HAL_UART_ERROR_NONE};
+std::atomic<uint32_t> uart_error{HAL_UART_ERROR_NONE};
 
 std::atomic<bool> txDoneFlag{true};
 
 uint8_t tmpBuffer[mobilinkd::tnc::TX_BUFFER_SIZE];
 uint8_t tmpBuffer2[mobilinkd::tnc::TX_BUFFER_SIZE];
 
+constexpr const int RX_BUFFER_SIZE = 127;
+unsigned char rxBuffer[RX_BUFFER_SIZE * 2];
+
+// 3 chunks of 128 bytes.  The first byte in each chunk is the length.
+typedef mobilinkd::tnc::memory::Pool<
+    3, RX_BUFFER_SIZE + 1> serial_pool_type;
+serial_pool_type serialPool;
+
 void log_frame(mobilinkd::tnc::hdlc::IoFrame* frame)
 {
     int pos = 0;
@@ -41,7 +49,56 @@ void log_frame(mobilinkd::tnc::hdlc::IoFrame* frame)
     DEBUG((char*)tmpBuffer2);
 }
 
-extern "C" void startSerialTask(void const* arg)
+// HAL does not have
+HAL_StatusTypeDef UART_DMAPauseReceive(UART_HandleTypeDef *huart)
+{
+  /* Process Locked */
+  __HAL_LOCK(huart);
+
+  if ((huart->RxState == HAL_UART_STATE_BUSY_RX) &&
+      (HAL_IS_BIT_SET(huart->Instance->CR3, USART_CR3_DMAR)))
+  {
+    /* Disable PE and ERR (Frame error, noise error, overrun error) interrupts */
+    CLEAR_BIT(huart->Instance->CR1, USART_CR1_PEIE);
+    CLEAR_BIT(huart->Instance->CR3, USART_CR3_EIE);
+
+    /* Disable the UART DMA Rx request */
+    CLEAR_BIT(huart->Instance->CR3, USART_CR3_DMAR);
+  }
+
+  /* Process Unlocked */
+  __HAL_UNLOCK(huart);
+
+  return HAL_OK;
+}
+
+HAL_StatusTypeDef UART_DMAResumeReceive(UART_HandleTypeDef *huart)
+{
+  /* Process Locked */
+  __HAL_LOCK(huart);
+
+  if (huart->RxState == HAL_UART_STATE_BUSY_RX)
+  {
+    /* Clear the Overrun flag before resuming the Rx transfer */
+    __HAL_UART_CLEAR_FLAG(huart, UART_CLEAR_OREF);
+
+    /* Reenable PE and ERR (Frame error, noise error, overrun error) interrupts */
+    SET_BIT(huart->Instance->CR1, USART_CR1_PEIE);
+    SET_BIT(huart->Instance->CR3, USART_CR3_EIE);
+
+    /* Enable the UART DMA Rx request */
+    SET_BIT(huart->Instance->CR3, USART_CR3_DMAR);
+  }
+
+  /* Process Unlocked */
+  __HAL_UNLOCK(huart);
+
+  return HAL_OK;
+}
+
+extern "C" void startSerialTask(void const* arg) __attribute__((optimize("-O1")));
+
+void startSerialTask(void const* arg)
 {
     using namespace mobilinkd::tnc;
 
@@ -58,90 +115,116 @@ extern "C" void startSerialTask(void const* arg)
 
     hdlc::IoFrame* frame = hdlc::acquire_wait();
 
-    HAL_UART_Receive_IT(&huart3, &rxBuffer, 1);
+    HAL_UART_Receive_DMA(&huart3, rxBuffer, RX_BUFFER_SIZE * 2);
+    __HAL_UART_ENABLE_IT(&huart3, UART_IT_IDLE);
 
     uint32_t last_sent_time = osKernelSysTick();
     uint32_t current_sent_time = 0;
+    bool paused = false;
 
     while (true) {
         osEvent evt = osMessageGet(serialPort->queue(), osWaitForever);
 
         if (evt.status != osEventMessage) {
-            HAL_UART_Receive_IT(&huart3, &rxBuffer, 1);
             continue;
         }
 
-        if (evt.value.v & 0x100) {
+        if (evt.value.v < FLASH_BASE) // Assumes FLASH_BASE < SRAM_BASE.
+        {
+            // Error received.
             hdlc::release(frame);
-            ERROR("UART Error: %08lx", evt.value.v);
+            ERROR("UART Error: %08lx", uart_error.load());
             uart_error.store(HAL_UART_ERROR_NONE);
             frame = hdlc::acquire_wait();
-            HAL_UART_Receive_IT(&huart3, &rxBuffer, 1);
+            HAL_UART_Receive_DMA(&huart3, rxBuffer, RX_BUFFER_SIZE * 2);
+            __HAL_UART_ENABLE_IT(&huart3, UART_IT_IDLE);
             continue;
         }
 
-        uint8_t c = evt.value.v;
-        switch (state) {
-        case WAIT_FBEGIN:
-            if (c == FEND) state = WAIT_FRAME_TYPE;
-            break;
-        case WAIT_FRAME_TYPE:
-            if (c == FEND) break;   // Still waiting for FRAME_TYPE.
-            frame->type(c);
-            state = WAIT_FEND;
-            break;
-        case WAIT_FEND:
-            switch (c) {
-            case FESC:
-                state = WAIT_ESCAPED;
+        auto block = (serial_pool_type::chunk_type*) evt.value.p;
+        auto data = static_cast<unsigned char*>(block->buffer);
+
+        uint8_t end = data[0] + 1;
+        for (uint8_t i = 1; i != end; ++i) {
+            uint8_t c = data[i];
+            switch (state) {
+            case WAIT_FBEGIN:
+                if (c == FEND) state = WAIT_FRAME_TYPE;
                 break;
-            case FEND:
-                frame->source(hdlc::IoFrame::SERIAL_DATA);
-                if (osMessagePut(ioEventQueueHandle, reinterpret_cast<uint32_t>(frame), osWaitForever) != osOK)
-                {
-                    hdlc::release(frame);
+            case WAIT_FRAME_TYPE:
+                if (c == FEND) break;   // Still waiting for FRAME_TYPE.
+                if (c < 8 or c == 0xFF) {
+                    frame->type(c);
+                    state = WAIT_FEND;
+                } else {
+                    WARN("Bad frame type");
+                    state = WAIT_FBEGIN;
                 }
-                current_sent_time = osKernelSysTick();
-                if (last_sent_time + 50 > current_sent_time) {
-                    uint32_t delay = (last_sent_time + 50) - current_sent_time;
-                    osDelay(delay);
-                }
-                last_sent_time = current_sent_time;
-                frame = hdlc::acquire_wait();
-                state = WAIT_FBEGIN;
                 break;
-            default:
-                if (not frame->push_back(c)) {
+            case WAIT_FEND:
+                switch (c) {
+                case FESC:
+                    state = WAIT_ESCAPED;
+                    break;
+                case FEND:
+                    frame->source(hdlc::IoFrame::SERIAL_DATA);
+                    if (osMessagePut(
+                        ioEventQueueHandle,
+                        reinterpret_cast<uint32_t>(frame),
+                        osWaitForever) != osOK)
+                    {
+                        WARN("Failed to send serial frame");
+                        hdlc::release(frame);
+                    }
+
+                    if (hdlc::ioFramePool().size() < (hdlc::ioFramePool().capacity() / 4))
+                    {
+                        UART_DMAPauseReceive(&huart3);
+                        WARN("Pausing UART RX");
+                        while (hdlc::ioFramePool().size() < (hdlc::ioFramePool().capacity() / 2))
+                        {
+                            osThreadYield();
+                        }
+                        UART_DMAResumeReceive(&huart3);
+                    }
+
+                    frame = hdlc::acquire_wait();
+                    state = WAIT_FBEGIN;
+                    break;
+                default:
+                    if (not frame->push_back(c)) {
+                        hdlc::release(frame);
+                        state = WAIT_FBEGIN;  // Drop frame;
+                        frame = hdlc::acquire_wait();
+                    }
+                }
+                break;
+            case WAIT_ESCAPED:
+                state = WAIT_FEND;
+                switch (c) {
+                case TFESC:
+                    if (not frame->push_back(FESC)) {
+                        hdlc::release(frame);
+                        state = WAIT_FBEGIN;  // Drop frame;
+                        frame = hdlc::acquire_wait();
+                    }
+                    break;
+                case TFEND:
+                    if (not frame->push_back(FEND)) {
+                        hdlc::release(frame);
+                        state = WAIT_FBEGIN;  // Drop frame;
+                        frame = hdlc::acquire_wait();
+                    }
+                    break;
+                default:
                     hdlc::release(frame);
                     state = WAIT_FBEGIN;  // Drop frame;
                     frame = hdlc::acquire_wait();
                 }
+                break;
             }
-            break;
-        case WAIT_ESCAPED:
-            state = WAIT_FEND;
-            switch (c) {
-            case TFESC:
-                if (not frame->push_back(FESC)) {
-                    hdlc::release(frame);
-                    state = WAIT_FBEGIN;  // Drop frame;
-                    frame = hdlc::acquire_wait();
-                }
-                break;
-            case TFEND:
-                if (not frame->push_back(FEND)) {
-                    hdlc::release(frame);
-                    state = WAIT_FBEGIN;  // Drop frame;
-                    frame = hdlc::acquire_wait();
-                }
-                break;
-            default:
-                hdlc::release(frame);
-                state = WAIT_FBEGIN;  // Drop frame;
-                frame = hdlc::acquire_wait();
-            }
-            break;
         }
+        serialPool.deallocate(block);
     }
 }
 
@@ -151,18 +234,73 @@ extern "C" void HAL_UART_TxCpltCallback(UART_HandleTypeDef*)
     txDoneFlag = true;
 }
 
-extern "C" void HAL_UART_RxCpltCallback(UART_HandleTypeDef *)
+extern "C" void HAL_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart)
 {
-    osMessagePut(mobilinkd::tnc::getSerialPort()->queue(), (uint32_t) rxBuffer, 0);
+    uint32_t len = (RX_BUFFER_SIZE * 2) - __HAL_DMA_GET_COUNTER(huart->hdmarx);
+    if (len == 0) return;
+    if (len > RX_BUFFER_SIZE) {
+        len = RX_BUFFER_SIZE; // wrapped.
+    }
 
-    HAL_UART_Receive_IT(&huart3, &rxBuffer, 1);
+    auto block = serialPool.allocate();
+    if (!block) return;
+    memmove(block->buffer + 1, rxBuffer, len);
+    block->buffer[0] = len;
+    auto status = osMessagePut(mobilinkd::tnc::getSerialPort()->queue(), (uint32_t) block, 0);
+    if (status != osOK) serialPool.deallocate(block);
 }
 
+extern "C" void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
+{
+    uint32_t hdmarx = __HAL_DMA_GET_COUNTER(huart->hdmarx);
+    uint32_t len = 0;
+    if (hdmarx > RX_BUFFER_SIZE) {
+        len = RX_BUFFER_SIZE;   // wrapped.
+    } else {
+        len = RX_BUFFER_SIZE - hdmarx;
+    }
+    if (len == 0) return;
+
+    auto block = serialPool.allocate();
+    if (!block) return;
+    memmove(block->buffer + 1, rxBuffer + RX_BUFFER_SIZE, len);
+    block->buffer[0] = len;
+    auto status = osMessagePut(mobilinkd::tnc::getSerialPort()->queue(), (uint32_t) block, 0);
+    if (status != osOK) serialPool.deallocate(block);
+}
+
+extern "C" void idleInterruptCallback(UART_HandleTypeDef* huart)
+{
+    uint32_t len = (RX_BUFFER_SIZE * 2) - __HAL_DMA_GET_COUNTER(huart->hdmarx);
+
+    if (len == 0) return;
+
+    auto block = serialPool.allocate();
+    if (!block) return;
+
+    HAL_UART_AbortReceive(huart);
+
+    if (len > RX_BUFFER_SIZE) {
+        // Second half
+        len = len - RX_BUFFER_SIZE;
+        memmove(block->buffer + 1, rxBuffer + RX_BUFFER_SIZE, len);
+    } else {
+        // First half
+        memmove(block->buffer + 1, rxBuffer, len);
+    }
+
+    block->buffer[0] = len;
+
+    HAL_UART_Receive_DMA(huart, rxBuffer, RX_BUFFER_SIZE * 2);
+
+    auto status = osMessagePut(mobilinkd::tnc::getSerialPort()->queue(), (uint32_t) block, 0);
+    if (status != osOK) serialPool.deallocate(block);
+
+}
 
 extern "C" void HAL_UART_ErrorCallback(UART_HandleTypeDef* huart)
 {
-    osMessagePut(mobilinkd::tnc::getSerialPort()->queue(),
-        (uint32_t) (huart->gState<<16) | huart->ErrorCode | 0x100, 0);
+    osMessagePut(mobilinkd::tnc::getSerialPort()->queue(), huart->ErrorCode, 0);
     uart_error.store((huart->gState<<16) | huart->ErrorCode);
     huart->ErrorCode = HAL_UART_ERROR_NONE;
     huart->gState = HAL_UART_STATE_READY;
diff --git a/TNC/StandardDeviation.hpp b/TNC/StandardDeviation.hpp
new file mode 100644
index 0000000..e014476
--- /dev/null
+++ b/TNC/StandardDeviation.hpp
@@ -0,0 +1,59 @@
+// Copyright 2020 Mobilinkd LLC <rob@mobilinkd.com>
+// All rights reserved.
+
+#pragma once
+
+#include "main.h"
+
+#include <arm_math.h>
+
+#include <cstdint>
+
+namespace mobilinkd {
+
+/**
+ * Compute a running standard deviation.
+ *
+ * Based on https://dsp.stackexchange.com/a/1187/36581
+ */
+struct StandardDeviation
+{
+    float mean{0.0};
+    float S{0.0};
+    uint32_t samples{0};
+
+    void reset()
+    {
+        mean = 0.0;
+        S = 0.0;
+        samples = 0;
+    }
+
+    void capture(float sample)
+    {
+        auto prev = mean;
+        samples += 1;
+        mean = mean + (sample - mean) / samples;
+        S = S + (sample - mean) * (sample - prev);
+    }
+
+    float variance() const
+    {
+        return samples == 0 ? -1.0 :  S / samples;
+    }
+
+    float stdev() const
+    {
+        float result = -1.0;
+        arm_sqrt_f32(variance(), &result);
+        return result;
+    }
+
+    // SNR in dB
+    float SNR() const
+    {
+        return 10.0 * log10(mean / stdev());
+    }
+};
+
+} // mobilinkd
diff --git a/TNC/UsbPort.cpp b/TNC/UsbPort.cpp
index b9620e6..f0bcc86 100644
--- a/TNC/UsbPort.cpp
+++ b/TNC/UsbPort.cpp
@@ -13,36 +13,27 @@
 extern "C" void TNC_Error_Handler(int dev, int err);
 
 extern osMessageQId ioEventQueueHandle;
+extern USBD_HandleTypeDef hUsbDeviceFS;
 
 extern "C" void cdc_receive(const uint8_t* buf, uint32_t len)
 {
     using namespace mobilinkd::tnc;
-    if (mobilinkd::tnc::getUsbPort()->queue() != 0)
+
+    // This is running in an interrupt handler.
+
+    UsbCdcRxBuffer_t* usbCdcRxBuffer = (UsbCdcRxBuffer_t*)(buf);
+    usbCdcRxBuffer->size = len;
+
+    if (getUsbPort()->queue() != 0)
     {
-        if (len == 1)
+        // This should always succeed.
+        if (osMessagePut(getUsbPort()->queue(), (uint32_t) buf, 0) == osOK)
         {
-            // Send single byte via queue directly.  Linux seems to do
-            // this for ttyACM ports.
-            osMessagePut(getUsbPort()->queue(), *buf, osWaitForever);
             return;
         }
-
-        auto frame = hdlc::acquire();
-        if (frame)
-        {
-            for (uint32_t i = 0; i != len; ++i)
-            {
-                frame->push_back(*buf++);
-            }
-            frame->source(hdlc::IoFrame::SERIAL_DATA);
-            if (osMessagePut(mobilinkd::tnc::getUsbPort()->queue(),
-                (uint32_t) frame,
-                osWaitForever) != osOK)
-            {
-                mobilinkd::tnc::hdlc::release(frame);
-            }
-        }
     }
+    ERROR("USB packet dropped");
+    USBD_CDC_ReceivePacket(&hUsbDeviceFS); // re-enable receive.
 }
 
 extern "C" void startCDCTask(void const* arg)
@@ -116,7 +107,7 @@ void UsbPort::add_char(uint8_t c)
 
 void UsbPort::run()
 {
-    frame_ = hdlc::acquire();
+    if (frame_ == nullptr) frame_ = hdlc::acquire();
 
     while (true) {
         osEvent evt = osMessageGet(queue(), osWaitForever);
@@ -124,27 +115,24 @@ void UsbPort::run()
             continue;
         }
 
-        uint32_t c = evt.value.v;
+        auto usbCdcRxBuffer = (UsbCdcRxBuffer_t*) evt.value.p;
 
-        if (c < 0x100) // Assume single byte transfer.
-        {
-            add_char(c);
-            continue;
+        // Handle ping-pong buffers.
+        if (usbCdcRxBuffer == usbCdcRxBuffer_1) {
+            USBD_CDC_SetRxBuffer(&hUsbDeviceFS, usbCdcRxBuffer_2->buffer);
+        } else {
+            USBD_CDC_SetRxBuffer(&hUsbDeviceFS, usbCdcRxBuffer_1->buffer);
         }
+        USBD_CDC_ReceivePacket(&hUsbDeviceFS);
 
-        auto input = static_cast<hdlc::IoFrame*>(evt.value.p);
+        INFO("USB p %lu", usbCdcRxBuffer->size);
 
-        if (!isOpen()) {
-            hdlc::release(input);
-            continue;
+        if (isOpen()) {
+            for (uint32_t i = 0; i != usbCdcRxBuffer->size; ++i) {
+                add_char(usbCdcRxBuffer->buffer[i]);
+            }
         }
-
-        for (uint8_t c : *input) {
-            add_char(c);
-        }
-        hdlc::release(input);
     }
-
 }
 
 void UsbPort::init()
diff --git a/firmware.ioc b/firmware.ioc
index a6d724a..12ee479 100644
--- a/firmware.ioc
+++ b/firmware.ioc
@@ -99,7 +99,7 @@ FREERTOS.configUSE_TIMERS=1
 File.Version=6
 I2C1.I2C_Speed_Mode=I2C_Fast_Plus
 I2C1.IPParameters=I2C_Speed_Mode,Timing
-I2C1.Timing=0x20000209
+I2C1.Timing=0x00000107
 KeepUserPlacement=true
 Mcu.Family=STM32L4
 Mcu.IP0=ADC1
@@ -475,10 +475,11 @@ RCC.HCLKFreq_Value=48000000
 RCC.HSE_VALUE=8000000
 RCC.HSI48_VALUE=48000000
 RCC.HSI_VALUE=16000000
-RCC.I2C1Freq_Value=48000000
+RCC.I2C1CLockSelection=RCC_I2C1CLKSOURCE_HSI
+RCC.I2C1Freq_Value=16000000
 RCC.I2C2Freq_Value=48000000
 RCC.I2C3Freq_Value=48000000
-RCC.IPParameters=ADCFreq_Value,AHBFreq_Value,APB1Freq_Value,APB1TimFreq_Value,APB2Freq_Value,APB2TimFreq_Value,CortexFreq_Value,FCLKCortexFreq_Value,FamilyName,HCLKFreq_Value,HSE_VALUE,HSI48_VALUE,HSI_VALUE,I2C1Freq_Value,I2C2Freq_Value,I2C3Freq_Value,LCDFreq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPUART1Freq_Value,LSCOPinFreq_Value,LSCOSource1,LSE_Drive_Capability,LSI_VALUE,MCO1PinFreq_Value,MSI_VALUE,PLLN,PLLPoutputFreq_Value,PLLQoutputFreq_Value,PLLRCLKFreq_Value,PLLSAI1N,PLLSAI1PoutputFreq_Value,PLLSAI1QoutputFreq_Value,PLLSAI1RoutputFreq_Value,PREFETCH_ENABLE,PWRFreq_Value,RCC_MCO1Source,RNGFreq_Value,RTCClockSelection,RTCFreq_Value,SAI1Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,USART1Freq_Value,USART2Freq_Value,USART3Freq_Value,USBFreq_Value,VCOInputFreq_Value,VCOOutputFreq_Value,VCOSAI1OutputFreq_Value
+RCC.IPParameters=ADCFreq_Value,AHBFreq_Value,APB1Freq_Value,APB1TimFreq_Value,APB2Freq_Value,APB2TimFreq_Value,CortexFreq_Value,FCLKCortexFreq_Value,FamilyName,HCLKFreq_Value,HSE_VALUE,HSI48_VALUE,HSI_VALUE,I2C1CLockSelection,I2C1Freq_Value,I2C2Freq_Value,I2C3Freq_Value,LCDFreq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPUART1Freq_Value,LSCOPinFreq_Value,LSCOSource1,LSE_Drive_Capability,LSI_VALUE,MCO1PinFreq_Value,MSI_VALUE,PLLN,PLLPoutputFreq_Value,PLLQoutputFreq_Value,PLLRCLKFreq_Value,PLLSAI1N,PLLSAI1PoutputFreq_Value,PLLSAI1QoutputFreq_Value,PLLSAI1RoutputFreq_Value,PREFETCH_ENABLE,PWRFreq_Value,RCC_MCO1Source,RNGFreq_Value,RTCClockSelection,RTCFreq_Value,SAI1Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,USART1Freq_Value,USART2Freq_Value,USART3CLockSelection,USART3Freq_Value,USBFreq_Value,VCOInputFreq_Value,VCOOutputFreq_Value,VCOSAI1OutputFreq_Value
 RCC.LCDFreq_Value=32768
 RCC.LPTIM1Freq_Value=48000000
 RCC.LPTIM2Freq_Value=48000000
@@ -509,7 +510,8 @@ RCC.SYSCLKFreq_VALUE=48000000
 RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK
 RCC.USART1Freq_Value=48000000
 RCC.USART2Freq_Value=48000000
-RCC.USART3Freq_Value=48000000
+RCC.USART3CLockSelection=RCC_USART3CLKSOURCE_HSI
+RCC.USART3Freq_Value=16000000
 RCC.USBFreq_Value=48000000
 RCC.VCOInputFreq_Value=4000000
 RCC.VCOOutputFreq_Value=96000000
diff --git a/stlink-tnc3.cfg b/stlink-tnc3.cfg
index 2c95e4f..19fd821 100644
--- a/stlink-tnc3.cfg
+++ b/stlink-tnc3.cfg
@@ -9,5 +9,4 @@ source [find target/stm32l4x.cfg]
 
 reset_config srst_only
 itm port 0 on
-tpiu config internal swv uart off 48000000
-
+tpiu config internal swv uart off 80000000