diff --git a/.cproject b/.cproject
index b0d130b..15d53f5 100644
--- a/.cproject
+++ b/.cproject
@@ -17,16 +17,16 @@
 				<configuration artifactExtension="elf" artifactName="${ProjName}" 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="rm -rf" description="" id="fr.ac6.managedbuild.config.gnu.cross.exe.debug.785246917" name="Debug" parent="fr.ac6.managedbuild.config.gnu.cross.exe.debug" postannouncebuildStep="Generating hex and Printing size information:" postbuildStep="arm-none-eabi-objcopy -O ihex &quot;${BuildArtifactFileBaseName}.elf&quot; &quot;${BuildArtifactFileBaseName}.hex&quot; &amp;&amp; arm-none-eabi-size &quot;${BuildArtifactFileName}&quot;">
 					<folderInfo id="fr.ac6.managedbuild.config.gnu.cross.exe.debug.785246917." name="/" resourcePath="">
 						<toolChain id="fr.ac6.managedbuild.toolchain.gnu.cross.exe.debug.2013979193" name="Ac6 STM32 MCU GCC" superClass="fr.ac6.managedbuild.toolchain.gnu.cross.exe.debug">
-							<option id="fr.ac6.managedbuild.option.gnu.cross.prefix.382830450" name="Prefix" superClass="fr.ac6.managedbuild.option.gnu.cross.prefix" useByScannerDiscovery="false" value="arm-none-eabi-" valueType="string"/>
-							<option id="fr.ac6.managedbuild.option.gnu.cross.mcu.66405555" name="Mcu" superClass="fr.ac6.managedbuild.option.gnu.cross.mcu" useByScannerDiscovery="false" value="STM32F103C8Tx" valueType="string"/>
-							<option id="fr.ac6.managedbuild.option.gnu.cross.board.1729153140" name="Board" superClass="fr.ac6.managedbuild.option.gnu.cross.board" useByScannerDiscovery="false" value="F103C8T6_DIGI_USB" valueType="string"/>
-							<option id="fr.ac6.managedbuild.option.gnu.cross.core.1083960614" name="Core" superClass="fr.ac6.managedbuild.option.gnu.cross.core" useByScannerDiscovery="false" valueType="stringList">
+							<option id="fr.ac6.managedbuild.option.gnu.cross.prefix.382830450" name="Prefix" superClass="fr.ac6.managedbuild.option.gnu.cross.prefix" value="arm-none-eabi-" valueType="string"/>
+							<option id="fr.ac6.managedbuild.option.gnu.cross.mcu.66405555" name="Mcu" superClass="fr.ac6.managedbuild.option.gnu.cross.mcu" value="STM32F103C8Tx" valueType="string"/>
+							<option id="fr.ac6.managedbuild.option.gnu.cross.board.1729153140" name="Board" superClass="fr.ac6.managedbuild.option.gnu.cross.board" value="F103C8T6_DIGI_USB" valueType="string"/>
+							<option id="fr.ac6.managedbuild.option.gnu.cross.core.1083960614" name="Core" superClass="fr.ac6.managedbuild.option.gnu.cross.core" valueType="stringList">
 								<listOptionValue builtIn="false" value="ARM Cortex-M3"/>
 								<listOptionValue builtIn="false" value="CM3"/>
 							</option>
-							<option id="fr.ac6.managedbuild.option.gnu.cross.instructionSet.1656935294" name="Instruction Set" superClass="fr.ac6.managedbuild.option.gnu.cross.instructionSet" useByScannerDiscovery="false" value="fr.ac6.managedbuild.option.gnu.cross.instructionSet.thumbII" valueType="enumerated"/>
-							<option id="fr.ac6.managedbuild.option.gnu.cross.fpu.1493691602" name="Floating point hardware" superClass="fr.ac6.managedbuild.option.gnu.cross.fpu" useByScannerDiscovery="false" value="fr.ac6.managedbuild.option.gnu.cross.fpu.no" valueType="enumerated"/>
-							<option id="fr.ac6.managedbuild.option.gnu.cross.floatabi.2025609880" name="Floating-point ABI" superClass="fr.ac6.managedbuild.option.gnu.cross.floatabi" useByScannerDiscovery="false" value="fr.ac6.managedbuild.option.gnu.cross.floatabi.soft" valueType="enumerated"/>
+							<option id="fr.ac6.managedbuild.option.gnu.cross.instructionSet.1656935294" name="Instruction Set" superClass="fr.ac6.managedbuild.option.gnu.cross.instructionSet" value="fr.ac6.managedbuild.option.gnu.cross.instructionSet.thumbII" valueType="enumerated"/>
+							<option id="fr.ac6.managedbuild.option.gnu.cross.fpu.1493691602" name="Floating point hardware" superClass="fr.ac6.managedbuild.option.gnu.cross.fpu" value="fr.ac6.managedbuild.option.gnu.cross.fpu.no" valueType="enumerated"/>
+							<option id="fr.ac6.managedbuild.option.gnu.cross.floatabi.2025609880" name="Floating-point ABI" superClass="fr.ac6.managedbuild.option.gnu.cross.floatabi" value="fr.ac6.managedbuild.option.gnu.cross.floatabi.soft" valueType="enumerated"/>
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="fr.ac6.managedbuild.targetPlatform.gnu.cross.2146232636" isAbstract="false" osList="all" superClass="fr.ac6.managedbuild.targetPlatform.gnu.cross"/>
 							<builder buildPath="${workspace_loc:/F103C8T6_DIGI_USB}/Debug" id="fr.ac6.managedbuild.builder.gnu.cross.458407908" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="fr.ac6.managedbuild.builder.gnu.cross">
 								<outputEntries>
@@ -52,7 +52,7 @@
 									<listOptionValue builtIn="false" value="STM32F103xB"/>
 								</option>
 								<option id="fr.ac6.managedbuild.gnu.c.compiler.option.misc.other.1107445133" superClass="fr.ac6.managedbuild.gnu.c.compiler.option.misc.other" useByScannerDiscovery="false" value="-fmessage-length=0" valueType="string"/>
-								<option id="gnu.c.compiler.option.dialect.std.2115782942" superClass="gnu.c.compiler.option.dialect.std" useByScannerDiscovery="true" value="gnu.c.compiler.dialect.default" valueType="enumerated"/>
+								<option id="gnu.c.compiler.option.dialect.std.1039445628" superClass="gnu.c.compiler.option.dialect.std" useByScannerDiscovery="true" value="gnu.c.compiler.dialect.default" valueType="enumerated"/>
 								<inputType id="fr.ac6.managedbuild.tool.gnu.cross.c.compiler.input.c.1385058365" superClass="fr.ac6.managedbuild.tool.gnu.cross.c.compiler.input.c"/>
 								<inputType id="fr.ac6.managedbuild.tool.gnu.cross.c.compiler.input.s.901665218" superClass="fr.ac6.managedbuild.tool.gnu.cross.c.compiler.input.s"/>
 							</tool>
@@ -77,10 +77,10 @@
 								<inputType id="fr.ac6.managedbuild.tool.gnu.cross.cpp.compiler.input.s.314001530" superClass="fr.ac6.managedbuild.tool.gnu.cross.cpp.compiler.input.s"/>
 							</tool>
 							<tool id="fr.ac6.managedbuild.tool.gnu.cross.c.linker.949266977" name="MCU GCC Linker" superClass="fr.ac6.managedbuild.tool.gnu.cross.c.linker">
-								<option id="fr.ac6.managedbuild.tool.gnu.cross.c.linker.script.100896041" name="Linker Script (-T)" superClass="fr.ac6.managedbuild.tool.gnu.cross.c.linker.script" useByScannerDiscovery="false" value="../STM32F103C8Tx_FLASH.ld" valueType="string"/>
-								<option id="gnu.c.link.option.libs.1305202629" name="Libraries (-l)" superClass="gnu.c.link.option.libs" useByScannerDiscovery="false"/>
-								<option id="gnu.c.link.option.paths.1292687187" name="Library search path (-L)" superClass="gnu.c.link.option.paths" useByScannerDiscovery="false"/>
-								<option id="gnu.c.link.option.ldflags.2131178816" name="Linker flags" superClass="gnu.c.link.option.ldflags" useByScannerDiscovery="false" value="-specs=nosys.specs -specs=nano.specs" valueType="string"/>
+								<option id="fr.ac6.managedbuild.tool.gnu.cross.c.linker.script.100896041" name="Linker Script (-T)" superClass="fr.ac6.managedbuild.tool.gnu.cross.c.linker.script" value="../STM32F103C8Tx_FLASH.ld" valueType="string"/>
+								<option id="gnu.c.link.option.libs.1305202629" name="Libraries (-l)" superClass="gnu.c.link.option.libs"/>
+								<option id="gnu.c.link.option.paths.1292687187" name="Library search path (-L)" superClass="gnu.c.link.option.paths"/>
+								<option id="gnu.c.link.option.ldflags.2131178816" name="Linker flags" superClass="gnu.c.link.option.ldflags" value="-specs=nosys.specs -specs=nano.specs" valueType="string"/>
 								<option id="gnu.c.link.option.other.623842724" name="Other options (-Xlinker [option])" superClass="gnu.c.link.option.other" useByScannerDiscovery="false"/>
 								<inputType id="cdt.managedbuild.tool.gnu.c.linker.input.751662654" superClass="cdt.managedbuild.tool.gnu.c.linker.input">
 									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
@@ -100,7 +100,7 @@
 							</tool>
 							<tool id="fr.ac6.managedbuild.tool.gnu.archiver.1989184041" name="MCU GCC Archiver" superClass="fr.ac6.managedbuild.tool.gnu.archiver"/>
 							<tool id="fr.ac6.managedbuild.tool.gnu.cross.assembler.1762903833" name="MCU GCC Assembler" superClass="fr.ac6.managedbuild.tool.gnu.cross.assembler">
-								<option id="gnu.both.asm.option.include.paths.1278058431" name="Include paths (-I)" superClass="gnu.both.asm.option.include.paths" useByScannerDiscovery="false"/>
+								<option id="gnu.both.asm.option.include.paths.1278058431" name="Include paths (-I)" superClass="gnu.both.asm.option.include.paths"/>
 								<inputType id="cdt.managedbuild.tool.gnu.assembler.input.632714311" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
 								<inputType id="fr.ac6.managedbuild.tool.gnu.cross.assembler.input.1298773058" superClass="fr.ac6.managedbuild.tool.gnu.cross.assembler.input"/>
 							</tool>
@@ -160,8 +160,6 @@
 									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32F1xx/Include"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
-									<listOptionValue builtIn="false" value="/vp-digi/lwfec"/>
-									<listOptionValue builtIn="false" value="../lwfec"/>
 								</option>
 								<option id="gnu.c.compiler.option.preprocessor.def.symbols.600695662" name="Defined symbols (-D)" superClass="gnu.c.compiler.option.preprocessor.def.symbols" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
@@ -235,6 +233,12 @@
 	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
 		<project id="F103C8T6_DIGI_USB.fr.ac6.managedbuild.target.gnu.cross.exe.1488070975" name="Executable" projectType="fr.ac6.managedbuild.target.gnu.cross.exe"/>
 	</storageModule>
+	<storageModule moduleId="scannerConfiguration">
+		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		<scannerConfigBuildInfo instanceId="fr.ac6.managedbuild.config.gnu.cross.exe.debug.785246917;fr.ac6.managedbuild.config.gnu.cross.exe.debug.785246917.;fr.ac6.managedbuild.tool.gnu.cross.c.compiler.736856211;fr.ac6.managedbuild.tool.gnu.cross.c.compiler.input.c.1385058365">
+			<autodiscovery enabled="false" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+	</storageModule>
 	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
 	<storageModule moduleId="refreshScope" versionNumber="2">
 		<configuration configurationName="Debug">
@@ -243,13 +247,5 @@
 		<configuration configurationName="Release"/>
 	</storageModule>
 	<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets"/>
-	<storageModule moduleId="scannerConfiguration">
-		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-		<scannerConfigBuildInfo instanceId="fr.ac6.managedbuild.config.gnu.cross.exe.debug.785246917;fr.ac6.managedbuild.config.gnu.cross.exe.debug.785246917.;fr.ac6.managedbuild.tool.gnu.cross.c.compiler.736856211;fr.ac6.managedbuild.tool.gnu.cross.c.compiler.input.c.1385058365">
-			<autodiscovery enabled="false" problemReportingEnabled="true" selectedProfileId=""/>
-		</scannerConfigBuildInfo>
-		<scannerConfigBuildInfo instanceId="fr.ac6.managedbuild.config.gnu.cross.exe.release.2040258153;fr.ac6.managedbuild.config.gnu.cross.exe.release.2040258153.;fr.ac6.managedbuild.tool.gnu.cross.c.compiler.736856211;fr.ac6.managedbuild.tool.gnu.cross.c.compiler.input.c.1385058365">
-			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-		</scannerConfigBuildInfo>
-	</storageModule>
+	<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
 </cproject>
diff --git a/Inc/ax25.h b/Inc/ax25.h
index 45ea226..0cca850 100644
--- a/Inc/ax25.h
+++ b/Inc/ax25.h
@@ -26,6 +26,10 @@ enum Ax25RxStage
 	RX_STAGE_IDLE = 0,
 	RX_STAGE_FLAG,
 	RX_STAGE_FRAME,
+#ifdef ENABLE_FX25
+	RX_STAGE_FX25_TAG,
+	RX_STAGE_FX25_FRAME,
+#endif
 };
 
 struct Ax25ProtoConfig
@@ -35,6 +39,7 @@ struct Ax25ProtoConfig
 	uint16_t quietTime; //Quiet time in ms
 	uint8_t allowNonAprs : 1; //allow non-APRS packets
 	uint8_t fx25 : 1; //enable FX.25 (AX.25 + FEC)
+	uint8_t fx25Tx : 1; //enable TX in FX.25
 };
 
 extern struct Ax25ProtoConfig Ax25Config;
@@ -71,9 +76,10 @@ void Ax25ClearReceivedFrameBitmap(void);
  * @param **dst Pointer to internal buffer
  * @param *size Actual frame size
  * @param *signalLevel Frame signal level (RMS)
+ * @param *fixed Count of fixed bytes (FX.25)
  * @return True if frame was read, false if no more frames to read
  */
-bool Ax25ReadNextRxFrame(uint8_t **dst, uint16_t *size, uint16_t *signalLevel);
+bool Ax25ReadNextRxFrame(uint8_t **dst, uint16_t *size, uint16_t *signalLevel, uint8_t *fixed);
 
 /**
  * @brief Get current RX stage
diff --git a/Inc/drivers/modem.h b/Inc/drivers/modem.h
index 2530f33..358a050 100644
--- a/Inc/drivers/modem.h
+++ b/Inc/drivers/modem.h
@@ -31,6 +31,11 @@ enum ModemType
 	MODEM_1200_V23,
 	MODEM_300,
 	MODEM_9600,
+
+#ifdef ENABLE_PSK
+	MODEM_BPSK_1200,
+	MODEM_QPSK_1200,
+#endif
 };
 
 enum ModemTxTestMode
@@ -53,12 +58,14 @@ extern struct ModemDemodConfig ModemConfig;
 
 enum ModemPrefilter
 {
+	PREFILTER_NONE = 0,
 	PREFILTER_PREEMPHASIS,
 	PREFILTER_DEEMPHASIS,
 	PREFILTER_FLAT,
-	PREFILTER_NONE,
 };
 
+void pushSample(float s);
+
 /**
  * @brief Get current modem baudrate
  * @return Baudrate
diff --git a/Inc/fx25.h b/Inc/fx25.h
index 21497ea..d747c3f 100644
--- a/Inc/fx25.h
+++ b/Inc/fx25.h
@@ -4,6 +4,7 @@
 #ifdef ENABLE_FX25
 
 #include <stdint.h>
+#include <stdbool.h>
 
 #define FX25_MAX_BLOCK_SIZE 255
 
@@ -16,15 +17,39 @@ struct Fx25Mode
 
 extern const struct Fx25Mode Fx25ModeList[11];
 
+/**
+ * @brief Get FX.25 mode for given correlation tag
+ * @param tag FX.25 correlation tag
+ * @return FX.25 mode structure pointer or NULL if not a FX.25 tag
+ */
+const struct Fx25Mode* Fx25GetModeForTag(uint64_t tag);
+
 /**
  * @brief Get FX.25 mode for given payload size
  * @param size Payload size including flags and CRC
  * @return FX.25 mode structure pointer or NULL if standard AX.25 must be used
  */
-const struct Fx25Mode* Fx25GetMode(uint16_t size);
+const struct Fx25Mode* Fx25GetModeForSize(uint16_t size);
 
-void Fx25AddParity(uint8_t *buffer, const struct Fx25Mode *mode);
+/**
+ * @brief Encode AX.25 message in FX.25
+ * @param *buffer AX.25 message (bit-stuffed, with CRC and padding)
+ * @param *mode FX.25 mode
+ */
+void Fx25Encode(uint8_t *buffer, const struct Fx25Mode *mode);
 
+/**
+ * @brief Decode/fix FX.25 packet
+ * @param *buffer Input buffer
+ * @param *mode FX.25 mode
+ * @param *fixed Number of bytes fixed
+ * @return True if message is valid, false if uncorrectable
+ */
+bool Fx25Decode(uint8_t *buffer, const struct Fx25Mode *mode, uint8_t *fixed);
+
+/**
+ * @brief Initialize FX.25 module
+ */
 void Fx25Init(void);
 
 #endif
diff --git a/Src/ax25.c b/Src/ax25.c
index 1d467bd..9b9df7a 100644
--- a/Src/ax25.c
+++ b/Src/ax25.c
@@ -19,10 +19,9 @@ along with VP-Digi.  If not, see <http://www.gnu.org/licenses/>.
 #include <stdlib.h>
 #include "drivers/modem.h"
 #include "common.h"
-#include "drivers/systick.h"
 #include <stdbool.h>
-#include "digipeater.h"
-#include "fx25.h"
+#include <string.h>
+#include "drivers/systick.h"
 
 struct Ax25ProtoConfig Ax25Config;
 
@@ -35,8 +34,10 @@ struct Ax25ProtoConfig Ax25Config;
 //frames that are too long are sent as standard AX.25 frames
 //Reed-Solomon library needs a bit of memory and the frame buffer must be smaller
 //otherwise we run out of RAM
-#define FRAME_MAX_SIZE (280) //single frame max length
+#define FRAME_MAX_SIZE (265) //single frame max length
+#include "fx25.h"
 #endif
+
 #define FRAME_MAX_COUNT (10) //max count of frames in buffer
 #define FRAME_BUFFER_SIZE (FRAME_MAX_COUNT * FRAME_MAX_SIZE) //circular frame buffer length
 
@@ -50,6 +51,7 @@ struct FrameHandle
 	uint16_t start;
 	uint16_t size;
 	uint16_t signalLevel;
+	uint8_t corrected;
 #ifdef ENABLE_FX25
 	struct Fx25Mode *fx25Mode;
 #endif
@@ -125,6 +127,11 @@ struct RxState
 	uint8_t rawData; //raw data being currently received
 	enum Ax25RxStage rx; //current RX stage
 	uint8_t frameReceived; //frame received flag
+#ifdef ENABLE_FX25
+	struct Fx25Mode *fx25Mode;
+	uint64_t tag; //received correlation tag
+	uint8_t tagBit;
+#endif
 };
 
 static volatile struct RxState rxState[MODEM_MAX_DEMODULATOR_COUNT];
@@ -186,13 +193,23 @@ void Ax25TxKiss(uint8_t *buf, uint16_t len)
 			if(end == len) //no frame end marker found
 				return;
 			Ax25WriteTxFrame(&buf[i + 2], end - (i + 2)); //skip modem number and send frame
-			DigiStoreDeDupe(&buf[i + 2], end - (i + 2));
+			//DigiStoreDeDupe(&buf[i + 2], end - (i + 2));
 			i = end; //move pointer to the next byte if there are more consecutive frames
 		}
 	}
 }
 
+static void removeLastFrameFromRxBuffer(void)
+{
+	if(rxFrameHead == 0)
+		rxFrameHead = FRAME_MAX_COUNT - 1;
+	else
+		rxFrameHead--;
+	rxFrameBufferFull = false;
+}
+
 #ifdef ENABLE_FX25
+
 static void *writeFx25Frame(uint8_t *data, uint16_t size)
 {
 	//first calculate how big the frame can be
@@ -200,7 +217,7 @@ static void *writeFx25Frame(uint8_t *data, uint16_t size)
 	//bitstuffing occurs after 5 consecutive ones, so in worst scenario
 	//bits inserted by bitstuffing can occupy up to frame size / 5 additional bytes
 	//also add 1 in case there is a remainder when dividing
-	const struct Fx25Mode *fx25Mode = fx25Mode = Fx25GetMode(size + 4 + (size / 5) + 1);
+	const struct Fx25Mode *fx25Mode = fx25Mode = Fx25GetModeForSize(size + 4 + (size / 5) + 1);
 	uint16_t requiredSize = size;
 	if(NULL != fx25Mode)
 		requiredSize = fx25Mode->K + fx25Mode->T;
@@ -217,6 +234,8 @@ static void *writeFx25Frame(uint8_t *data, uint16_t size)
 	txFrame[txFrameHead].start = txBufferHead;
 	txFrame[txFrameHead].fx25Mode = (struct Fx25Mode*)fx25Mode;
 
+	memset(txFx25Buffer, 0, sizeof(txFx25Buffer));
+
 	uint16_t index = 0;
 	//header flag
 	txFx25Buffer[index++] = 0x7E;
@@ -233,7 +252,7 @@ static void *writeFx25Frame(uint8_t *data, uint16_t size)
 			bits++;
 			if(i < size) //frame data
 			{
-				if(data[i] >> k)
+				if((data[i] >> k) & 1)
 				{
 					calculateCRC(1, &crc);
 					bitstuff++;
@@ -253,7 +272,7 @@ static void *writeFx25Frame(uint8_t *data, uint16_t size)
 				else
 					c = (crc >> 8) ^ 0xFF;
 
-				if(c >> k)
+				if((c >> k) & 1)
 				{
 					bitstuff++;
 					txFx25Buffer[index] |= 0x80;
@@ -291,7 +310,7 @@ static void *writeFx25Frame(uint8_t *data, uint16_t size)
 			txFx25Buffer[index] >>= 1;
 			bits++;
 
-			if(0x7E >> k)
+			if((0x7E >> k) & 1)
 			{
 				txFx25Buffer[index] |= 0x80;
 			}
@@ -304,7 +323,7 @@ static void *writeFx25Frame(uint8_t *data, uint16_t size)
 		}
 	}
 
-	Fx25AddParity(txFx25Buffer, fx25Mode);
+	Fx25Encode(txFx25Buffer, fx25Mode);
 
 	for(uint16_t i = 0; i < (fx25Mode->K + fx25Mode->T); i++)
 	{
@@ -319,6 +338,94 @@ static void *writeFx25Frame(uint8_t *data, uint16_t size)
 		txFrameBufferFull = true;
 	return ret;
 }
+
+
+static struct FrameHandle* parseFx25Frame(uint8_t *frame, uint16_t size, uint16_t *crc)
+{
+	struct FrameHandle *h = &rxFrame[rxFrameHead];
+	uint16_t initialRxBufferHead = rxBufferHead;
+	if(!rxFrameBufferFull)
+	{
+		rxFrame[rxFrameHead++].start = rxBufferHead;
+		rxFrameHead %= FRAME_MAX_COUNT;
+		if(rxFrameHead == txFrameHead)
+			rxFrameBufferFull = true;
+	}
+	else
+		return NULL;
+
+	uint16_t i = 0; //input data index
+	uint16_t k = 0; //output data size
+	while(frame[i] == 0x7E)
+		i++;
+
+	uint8_t bitstuff = 0;
+	uint8_t outBit = 0;
+	for(; i < size; i++)
+	{
+		for(uint8_t b = 0; b < 8; b++)
+		{
+			if(frame[i] & (1 << b))
+			{
+				rxBuffer[rxBufferHead] >>= 1;
+				rxBuffer[rxBufferHead] |= 0x80;
+				bitstuff++;
+			}
+			else
+			{
+				if(bitstuff == 5) //zero after 5 ones, normal bitstuffing
+				{
+					bitstuff = 0;
+					continue;
+				}
+				else if(bitstuff == 6) //zero after 6 ones, this is a flag
+				{
+					goto endParseFx25Frame;
+				}
+				else if(bitstuff >= 7) //zero after 7 ones, illegal byte
+				{
+					removeLastFrameFromRxBuffer();
+					return NULL;
+				}
+				bitstuff = 0;
+				rxBuffer[rxBufferHead] >>= 1;
+			}
+			outBit++;
+			if(outBit == 8)
+			{
+				k++;
+				rxBufferHead++;
+				rxBufferHead %= FRAME_BUFFER_SIZE;
+				outBit = 0;
+			}
+		}
+	}
+
+endParseFx25Frame:
+	*crc = 0xFFFF;
+	i = initialRxBufferHead;
+	for(uint16_t j = 0; j < (k - 2); j++)
+	{
+		for(uint8_t b = 0; b < 8; b++)
+			calculateCRC((rxBuffer[i] >> b) & 1, crc);
+
+		i++;
+		i %= FRAME_BUFFER_SIZE;
+	}
+
+	*crc ^= 0xFFFF;
+	if((rxBuffer[i] == (*crc & 0xFF) )
+		&& (rxBuffer[(i + 1) % FRAME_BUFFER_SIZE] == ((*crc >> 8) & 0xFF))) //check CRC
+	{
+		h->size = k - 2;
+		return h;
+	}
+	else
+	{
+		removeLastFrameFromRxBuffer();
+		return NULL;
+	}
+}
 #endif
 
 void *Ax25WriteTxFrame(uint8_t *data, uint16_t size)
@@ -330,7 +437,7 @@ void *Ax25WriteTxFrame(uint8_t *data, uint16_t size)
 		return NULL;
 
 #ifdef ENABLE_FX25
-	if(Ax25Config.fx25)
+	if(Ax25Config.fx25 && Ax25Config.fx25Tx)
 	{
 		void *ret = writeFx25Frame(data, size);
 		if(ret)
@@ -367,7 +474,7 @@ void *Ax25WriteTxFrame(uint8_t *data, uint16_t size)
 }
 
 
-bool Ax25ReadNextRxFrame(uint8_t **dst, uint16_t *size, uint16_t *signalLevel)
+bool Ax25ReadNextRxFrame(uint8_t **dst, uint16_t *size, uint16_t *signalLevel, uint8_t *fixed)
 {
 	if((rxFrameHead == rxFrameTail) && !rxFrameBufferFull)
 		return false;
@@ -381,8 +488,12 @@ bool Ax25ReadNextRxFrame(uint8_t **dst, uint16_t *size, uint16_t *signalLevel)
 
 	*signalLevel = rxFrame[rxFrameTail].signalLevel;
 	*size = rxFrame[rxFrameTail].size;
+#ifdef ENABLE_FX25
+	*fixed = rxFrame[rxFrameTail].corrected;
+#endif
 
 	rxFrameBufferFull = false;
+
 	rxFrameTail++;
 	rxFrameTail %= FRAME_MAX_COUNT;
 	return true;
@@ -393,7 +504,6 @@ enum Ax25RxStage Ax25GetRxStage(uint8_t modem)
 	return rxState[modem].rx;
 }
 
-
 void Ax25BitParse(uint8_t bit, uint8_t modem)
 {
 	if(lastCrc != 0) //there was a frame received
@@ -412,96 +522,184 @@ void Ax25BitParse(uint8_t bit, uint8_t modem)
 
 	}
 
-
 	struct RxState *rx = (struct RxState*)&(rxState[modem]);
 
 	rx->rawData <<= 1; //store incoming bit
 	rx->rawData |= (bit > 0);
 
+#ifdef ENABLE_FX25
+	rx->tag >>= 1;
+	if(bit)
+		rx->tag |= 0x8000000000000000;
+	rx->tagBit++;
 
-	if(rx->rawData == 0x7E) //HDLC flag received
+
+	if((rx->rx == RX_STAGE_FX25_TAG) || (rx->rx == RX_STAGE_FX25_FRAME))
 	{
-		if(rx->rx == RX_STAGE_FRAME) //if we are in frame, this is the end of the frame
+		if((rx->rx == RX_STAGE_FX25_TAG) && (rx->tagBit == 64))
 		{
-    		if((rx->frameIdx > 15)) //correct frame must be at least 16 bytes long
-    		{
-        		uint16_t i = 0;
-				for(; i < rx->frameIdx - 2; i++) //look for path end bit
-        		{
-        			if(rx->frame[i] & 1)
-        				break;
-        		}
-
-				//if non-APRS frames are not allowed, check if this frame has control=0x03 and PID=0xF0
-
-				if(Ax25Config.allowNonAprs || (((rx->frame[i + 1] == 0x03) && (rx->frame[i + 2] == 0xf0))))
+			if(Ax25Config.fx25)
+			{
+				rx->fx25Mode = (struct Fx25Mode*)Fx25GetModeForTag(rx->tag);
+				if(Ax25Config.fx25 && (rx->fx25Mode != NULL))
 				{
-					if((rx->frame[rx->frameIdx - 2] == ((rx->crc & 0xFF) ^ 0xFF)) && (rx->frame[rx->frameIdx - 1] == (((rx->crc >> 8) & 0xFF) ^ 0xFF))) //check CRC
+					rx->rx = RX_STAGE_FX25_FRAME;
+					rx->frameIdx = 0;
+					rx->receivedBitIdx = 0;
+					rx->receivedByte = 0;
+					return;
+				}
+			}
+			rx->rx = RX_STAGE_FRAME;
+		}
+	}
+	else
+	{
+			if(rx->rawData != 0x7E)
+			{
+				if(Ax25Config.fx25)
+				{
+					if((rx->rx == RX_STAGE_FLAG) && (rx->tagBit == 8))
+						rx->rx = RX_STAGE_FX25_TAG;
+				}
+				else
+					rx->rx = RX_STAGE_FRAME;
+			}
+			else
+				rx->tagBit = 0;
+#endif
+
+		if(rx->rawData == 0x7E) //HDLC flag received
+		{
+			if(rx->rx == RX_STAGE_FRAME) //if we are in frame, this is the end of the frame
+			{
+				if((rx->frameIdx > 15)) //correct frame must be at least 16 bytes long
+				{
+					uint16_t i = 0;
+					for(; i < rx->frameIdx - 2; i++) //look for path end bit
 					{
-						rx->frameReceived = 1;
-						rx->frameIdx -= 2; //remove CRC
-						if(rx->crc != lastCrc) //the other decoder has not received this frame yet, so store it in main frame buffer
+						if(rx->frame[i] & 1)
+							break;
+					}
+
+					//if non-APRS frames are not allowed, check if this frame has control=0x03 and PID=0xF0
+					if(Ax25Config.allowNonAprs || (((rx->frame[i + 1] == 0x03) && (rx->frame[i + 2] == 0xF0))))
+					{
+						rx->crc = 0xFFFF;
+						for(i = 0; i < rx->frameIdx - 2; i++)
 						{
-							lastCrc = rx->crc; //store CRC of this frame
-
-							if(!rxFrameBufferFull) //if enough space, store the frame
+							for(uint8_t k = 0; k < 8; k++)
 							{
-								rxFrame[rxFrameHead].start = rxBufferHead;
-								rxFrame[rxFrameHead].signalLevel = ModemGetRMS(modem);
-								rxFrame[rxFrameHead++].size = rx->frameIdx;
-								rxFrameHead %= FRAME_MAX_COUNT;
-								if(rxFrameHead == txFrameHead)
-									rxFrameBufferFull = true;
+								calculateCRC((rx->frame[i] >> k) & 1, &(rx->crc));
+							}
+						}
+						rx->crc ^= 0xFFFF;
+						if((rx->frame[rx->frameIdx - 2] == (rx->crc & 0xFF)) && (rx->frame[rx->frameIdx - 1] == ((rx->crc >> 8) & 0xFF))) //check CRC
+						{
+							rx->frameReceived = 1;
+							rx->frameIdx -= 2; //remove CRC
+							if(rx->crc != lastCrc) //the other decoder has not received this frame yet, so store it in main frame buffer
+							{
+								lastCrc = rx->crc; //store CRC of this frame
 
-								for(uint16_t i = 0; i < rx->frameIdx; i++)
+								if(!rxFrameBufferFull) //if enough space, store the frame
 								{
-									rxBuffer[rxBufferHead++] = rx->frame[i];
-									rxBufferHead %= FRAME_BUFFER_SIZE;
-								}
+									rxFrame[rxFrameHead].start = rxBufferHead;
+									rxFrame[rxFrameHead].signalLevel = ModemGetRMS(modem);
+									rxFrame[rxBufferHead].corrected = 0;
+#ifdef ENABLE_FX25
+									rxFrame[rxBufferHead].fx25Mode = NULL;
+#endif
+									rxFrame[rxFrameHead++].size = rx->frameIdx;
+									rxFrameHead %= FRAME_MAX_COUNT;
+									if(rxFrameHead == txFrameHead)
+										rxFrameBufferFull = true;
 
+									for(uint16_t i = 0; i < rx->frameIdx; i++)
+									{
+										rxBuffer[rxBufferHead++] = rx->frame[i];
+										rxBufferHead %= FRAME_BUFFER_SIZE;
+									}
+								}
 							}
 						}
 					}
 				}
-
-    		}
-
+			}
+			rx->rx = RX_STAGE_FLAG;
+			ModemClearRMS(modem);
+			rx->receivedByte = 0;
+			rx->receivedBitIdx = 0;
+			rx->frameIdx = 0;
+			return;
 		}
-		rx->rx = RX_STAGE_FLAG;
-		ModemClearRMS(modem);
-		rx->receivedByte = 0;
-		rx->receivedBitIdx = 0;
-		rx->frameIdx = 0;
-		rx->crc = 0xFFFF;
-		return;
+
+#ifdef ENABLE_FX25
 	}
 
 
-	if((rx->rawData & 0x7F) == 0x7F) //received 7 consecutive ones, this is an error (sometimes called "escape byte")
+	if((rx->rx != RX_STAGE_FX25_FRAME) && (rx->rx != RX_STAGE_FX25_TAG))
 	{
-		rx->rx = RX_STAGE_FLAG;
-		ModemClearRMS(modem);
-		rx->receivedByte = 0;
-		rx->receivedBitIdx = 0;
-		rx->frameIdx = 0;
-		rx->crc = 0xFFFF;
-		return;
+#else
+	{
+		//this condition must not be checked when FX.25 is enabled
+		//because FX.25 parity bytes and tags contain >= 7 consecutive ones
+		if((rx->rawData & 0x7F) == 0x7F) //received 7 consecutive ones, this is an error (sometimes called "escape byte")
+		{
+			rx->rx = RX_STAGE_FLAG;
+			ModemClearRMS(modem);
+			rx->receivedByte = 0;
+			rx->receivedBitIdx = 0;
+			rx->frameIdx = 0;
+			rx->crc = 0xFFFF;
+			return;
+		}
+#endif
+		if((rx->rawData & 0x3F) == 0x3E) //dismiss bit 0 added by bit stuffing
+			return;
 	}
 
-
-
-	if(rx->rx == RX_STAGE_IDLE) //not in a frame, don't go further
-		return;
-
-
-	if((rx->rawData & 0x3F) == 0x3E) //dismiss bit 0 added by bit stuffing
-		return;
-
 	if(rx->rawData & 0x01) //received bit 1
 		rx->receivedByte |= 0x80; //store it
 
 	if(++rx->receivedBitIdx >= 8) //received full byte
 	{
+#ifdef ENABLE_FX25
+		//end of FX.25 reception, that is received full block
+		if((rx->fx25Mode != NULL) && (rx->frameIdx == (rx->fx25Mode->K + rx->fx25Mode->T)))
+		{
+			uint8_t fixed = 0;
+			if(Fx25Decode(rx->frame, rx->fx25Mode, &fixed))
+			{
+				uint16_t crc;
+				struct FrameHandle *h = parseFx25Frame(rx->frame, rx->frameIdx, &crc);
+				if(h != NULL)
+				{
+					rx->frameReceived = 1;
+					h->corrected = fixed;
+					//FX.25 (RS) decoding is not reentrant/interrupt safe
+					//use only one modem when FX.25 is enabled
+					// if(crc != lastCrc)
+					// {
+						h->signalLevel = ModemGetRMS(modem);
+						h->fx25Mode = rx->fx25Mode;
+						lastCrc = crc;
+					// }
+					// else
+					// 	removeLastFrameFromRxBuffer();
+				}
+			}
+			rx->rx = RX_STAGE_FX25_TAG;
+			rx->tagBit = 0;
+			ModemClearRMS(modem);
+			rx->receivedByte = 0;
+			rx->receivedBitIdx = 0;
+			rx->frameIdx = 0;
+			return;
+		}
+#else
+		rx->rx = RX_STAGE_FRAME;
+#endif
 		if(rx->frameIdx > FRAME_MAX_SIZE) //frame is too long
 		{
 			rx->rx = RX_STAGE_IDLE;
@@ -512,14 +710,6 @@ void Ax25BitParse(uint8_t bit, uint8_t modem)
 			rx->crc = 0xFFFF;
 			return;
 		}
-		if(rx->frameIdx >= 2) //more than 2 bytes received, calculate CRC
-		{
-			for(uint8_t i = 0; i < 8; i++)
-			{
-				calculateCRC((rx->frame[rx->frameIdx - 2] >> i) & 1, &(rx->crc));
-			}
-		}
-		rx->rx = RX_STAGE_FRAME;
 		rx->frame[rx->frameIdx++] = rx->receivedByte; //store received byte
 		rx->receivedByte = 0;
 		rx->receivedBitIdx = 0;
@@ -544,12 +734,14 @@ uint8_t Ax25GetTxBit(void)
 			else
 			{
 				txDelayElapsed = 0;
+#ifdef ENABLE_FX25
 				if(NULL != txFrame[txFrameTail].fx25Mode)
 				{
 					txStage = TX_STAGE_CORRELATION_TAG;
 					txTagByteIdx = 0;
 				}
 				else
+#endif
 					txStage = TX_STAGE_HEADER_FLAGS;
 			}
 		}
@@ -561,6 +753,8 @@ transmitTag:
 				txByte = (txFrame[txFrameTail].fx25Mode->tag >> (8 * txTagByteIdx)) & 0xFF;
 			else
 				txStage = TX_STAGE_DATA;
+
+			txTagByteIdx++;
 		}
 #endif
 		if(txStage == TX_STAGE_HEADER_FLAGS) //transmitting initial flags
@@ -689,7 +883,11 @@ transmitTail:
 
 	uint8_t txBit = 0;
 	//transmitting normal data or CRC in AX.25 mode
-	if((NULL != txFrame[txFrameTail].fx25Mode) || (txStage == TX_STAGE_DATA) || (txStage == TX_STAGE_CRC))
+	if(
+#ifdef ENABLE_FX25
+		(NULL == txFrame[txFrameTail].fx25Mode) &&
+#endif
+		((txStage == TX_STAGE_DATA) || (txStage == TX_STAGE_CRC)))
 	{
 		if(txBitstuff == 5) //5 consecutive ones transmitted
 		{
@@ -730,16 +928,16 @@ transmitTail:
  */
 void Ax25TransmitBuffer(void)
 {
-	if(txInitStage == TX_INIT_WAITING)
-		return;
-	if(txInitStage == TX_INIT_TRANSMITTING)
-		return;
+	 if(txInitStage == TX_INIT_WAITING)
+	 	return;
+	 if(txInitStage == TX_INIT_TRANSMITTING)
+	 	return;
 
-	if((txFrameHead != txFrameTail) || txFrameBufferFull)
-	{
-		txQuiet = (SysTickGet() + (Ax25Config.quietTime / SYSTICK_INTERVAL) + Random(0, 200 / SYSTICK_INTERVAL)); //calculate required delay
-		txInitStage = TX_INIT_WAITING;
-	}
+	 if((txFrameHead != txFrameTail) || txFrameBufferFull)
+	 {
+	 	txQuiet = (SysTickGet() + (Ax25Config.quietTime / SYSTICK_INTERVAL) + Random(0, 200 / SYSTICK_INTERVAL)); //calculate required delay
+	 	txInitStage = TX_INIT_WAITING;
+	 }
 }
 
 
@@ -748,7 +946,7 @@ void Ax25TransmitBuffer(void)
  * @brief Start transmission immediately
  * @warning Transmission should be initialized using Ax25_transmitBuffer
  */
-static void transmitStart(void)
+/*static */void transmitStart(void)
 {
 	txCrc = 0xFFFF; //initial CRC value
 	txStage = TX_STAGE_PREAMBLE;
@@ -765,37 +963,37 @@ static void transmitStart(void)
  */
 void Ax25TransmitCheck(void)
 {
-	if(txInitStage == TX_INIT_OFF) //TX not initialized at all, nothing to transmit
-		return;
-	if(txInitStage == TX_INIT_TRANSMITTING) //already transmitting
-		return;
+	 if(txInitStage == TX_INIT_OFF) //TX not initialized at all, nothing to transmit
+	 	return;
+	 if(txInitStage == TX_INIT_TRANSMITTING) //already transmitting
+	 	return;
 
-	if(ModemIsTxTestOngoing()) //TX test is enabled, wait for now
-		return;
+	 if(ModemIsTxTestOngoing()) //TX test is enabled, wait for now
+	 	return;
 
-	if(txQuiet < SysTickGet()) //quit time has elapsed
-	{
-		if(!ModemDcdState()) //channel is free
-		{
-			txInitStage = TX_INIT_TRANSMITTING; //transmit right now
-			txRetries = 0;
-			transmitStart();
-		}
-		else //channel is busy
-		{
-			if(txRetries == MAX_TRANSMIT_RETRY_COUNT) //timeout
-			{
-				txInitStage = TX_INIT_TRANSMITTING; //transmit right now
-				txRetries = 0;
-				transmitStart();
-			}
-			else //still trying
-			{
-				txQuiet = SysTickGet() + Random(100 / SYSTICK_INTERVAL, 500 / SYSTICK_INTERVAL); //try again after some random time
-				txRetries++;
-			}
-		}
-	}
+	 if(txQuiet < SysTickGet()) //quit time has elapsed
+	 {
+	 	if(!ModemDcdState()) //channel is free
+	 	{
+	 		txInitStage = TX_INIT_TRANSMITTING; //transmit right now
+	 		txRetries = 0;
+	 		transmitStart();
+	 	}
+	 	else //channel is busy
+	 	{
+	 		if(txRetries == MAX_TRANSMIT_RETRY_COUNT) //timeout
+	 		{
+	 			txInitStage = TX_INIT_TRANSMITTING; //transmit right now
+	 			txRetries = 0;
+	 			transmitStart();
+	 		}
+	 		else //still trying
+	 		{
+	 			txQuiet = SysTickGet() + Random(100 / SYSTICK_INTERVAL, 500 / SYSTICK_INTERVAL); //try again after some random time
+	 			txRetries++;
+	 		}
+	 	}
+	 }
 }
 
 void Ax25Init(void)
diff --git a/Src/common.c b/Src/common.c
index c7d075b..3f9dae0 100644
--- a/Src/common.c
+++ b/Src/common.c
@@ -30,7 +30,11 @@ struct _GeneralConfig GeneralConfig =
 		.kissMonitor = 0,
 };
 
-const char versionString[] = "VP-Digi v. 1.3.0\r\nThe open-source standalone APRS digipeater controller and KISS TNC\r\n";
+const char versionString[] = "VP-Digi v. 2.0.0\r\nThe open-source standalone APRS digipeater controller and KISS TNC\r\n"
+#ifdef ENABLE_FX25
+		"with FX.25 support compiled-in\r\n"
+#endif
+		;
 
 static uint64_t pow10i(uint16_t exp)
 {
diff --git a/Src/config.c b/Src/config.c
index 1839b18..7d48fa3 100644
--- a/Src/config.c
+++ b/Src/config.c
@@ -94,7 +94,8 @@ along with VP-DigiConfig.  If not, see <http://www.gnu.org/licenses/>.
 #define CONFIG_KISSMONITOR 1206
 #define CONFIG_DEST 1208
 #define CONFIG_ALLOWNONAPRS 1214
-#define CONFIG_XXX 1216 //next address (not used)
+#define CONFIG_FX25 1216
+#define CONFIG_XXX 1218 //next address (not used)
 
 
 /**
@@ -262,6 +263,7 @@ void ConfigWrite(void)
 	write(CONFIG_PWM_FLAT, ModemConfig.usePWM | (ModemConfig.flatAudioIn << 1));
 	write(CONFIG_KISSMONITOR, GeneralConfig.kissMonitor);
 	write(CONFIG_ALLOWNONAPRS, Ax25Config.allowNonAprs);
+	write(CONFIG_FX25, Ax25Config.fx25 | (Ax25Config.fx25Tx << 1));
 
 	write(CONFIG_FLAG, CONFIG_FLAG_WRITTEN);
 
@@ -350,6 +352,9 @@ uint8_t ConfigRead(void)
 	ModemConfig.flatAudioIn = (t & 2) > 0;
 	GeneralConfig.kissMonitor = (read(CONFIG_KISSMONITOR) == 1);
 	Ax25Config.allowNonAprs = (read(CONFIG_ALLOWNONAPRS) == 1);
+	t = (uint8_t)read(CONFIG_FX25);
+	Ax25Config.fx25 = t & 1;
+	Ax25Config.fx25Tx = (t & 2) > 0;
 
 	return 1;
 }
diff --git a/Src/drivers/modem.c b/Src/drivers/modem.c
index 006b7a3..327c556 100644
--- a/Src/drivers/modem.c
+++ b/Src/drivers/modem.c
@@ -15,13 +15,14 @@ You should have received a copy of the GNU General Public License
 along with VP-Digi.  If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "drivers/modem.h"
 #include "drivers/systick.h"
+#include "drivers/modem.h"
 #include "ax25.h"
-#include "stm32f1xx.h"
 #include <math.h>
 #include <stdlib.h>
 #include "common.h"
+#include <string.h>
+#include "stm32f1xx.h"
 
 
 /*
@@ -53,10 +54,10 @@ along with VP-Digi.  If not, see <http://www.gnu.org/licenses/>.
 #define DCD300_INC 5
 #define DCD300_PLLTUNE 0
 
-#define N1200 8 //samples per symbol
-#define N9600 4
-#define N300 16
-#define NMAX 16 //keep this value equal to the biggest Nx
+#define N1200 8 //samples per symbol @ fs=9600, oversampling = 38400 Hz
+#define N9600 4 //fs=38400, oversampling = 153600 Hz
+#define N300 32 //fs=9600, oversampling = 38400 Hz
+#define NMAX 32 //keep this value equal to the biggest Nx
 
 #define PLL1200_STEP (((uint64_t)1 << 32) / N1200) //PLL tick increment value
 #define PLL9600_STEP (((uint64_t)1 << 32) / N9600)
@@ -69,8 +70,9 @@ along with VP-Digi.  If not, see <http://www.gnu.org/licenses/>.
 #define PLL300_LOCKED_TUNE 0.74f
 #define PLL300_NOT_LOCKED_TUNE 0.50f
 
-#define AGC9600_ATTACK 0.08f
-#define AGC9600_DECAY 0.0008f
+//for 9600 modem AGC, but not used
+// #define AGC9600_ATTACK 0.08f //0.08
+// #define AGC9600_DECAY 0.0008f
 
 #define DAC_SINE_SIZE 32 //DAC sine table size
 
@@ -86,6 +88,7 @@ static uint16_t dacSine[DAC_SINE_SIZE]; //sine samples for DAC
 static uint8_t dacSineIdx; //current sine sample index
 static uint16_t samples[4]; //very raw received samples, filled directly by DMA
 static uint8_t currentSymbol; //current symbol for NRZI encoding
+static uint8_t scrambledSymbol; //current symbol after scrambling
 static float markFreq; //mark frequency
 static float spaceFreq; //space frequency
 static float baudRate; //baudrate
@@ -94,12 +97,12 @@ static uint8_t spaceStep; //space timer step
 static uint16_t baudRateStep; //baudrate timer step
 static int16_t coeffHiI[NMAX], coeffLoI[NMAX], coeffHiQ[NMAX], coeffLoQ[NMAX]; //correlator IQ coefficients
 static uint8_t dcd = 0; //multiplexed DCD state from both demodulators
-static uint32_t lfsr = 0; //LFSR for 9600 Bd
+static uint32_t lfsr = 0xFFFFF; //LFSR for 9600 Bd
 
 /**
  * @brief BPF filter with 2200 Hz tone 6 dB preemphasis (it actually attenuates 1200 Hz tone by 6 dB)
  */
-static int16_t bpf1200[8] =
+static const int16_t bpf1200[8] =
 {
       728,
     -13418,
@@ -114,7 +117,7 @@ static int16_t bpf1200[8] =
 /**
  * @brief BPF filter with 2200 Hz tone 6 dB deemphasis
  */
-static int16_t bpf1200Inv[8] =
+static const int16_t bpf1200Inv[8] =
 {
         -10513,
         -10854,
@@ -126,29 +129,23 @@ static int16_t bpf1200Inv[8] =
          -879
 };
 
-//fs=4800, rectangular, fc1=1400, fc2=2000, 0 dB @ 1600 Hz and 1800 Hz, N = 15, gain 65536
-static int16_t bpf300[15] =
+//fs=9600, rectangular, fc1=1500, fc2=1900, 0 dB @ 1600 Hz and 1800 Hz, N = 15, gain 65536
+static const int16_t bpf300[15] =
 {
-	-2327, 3557, 1048, -9284, 12210, -3989, -9849, 17268, -9849, -3989, 12210, -9284, 1048, 3557, -2327,
+	186, 8887, 8184, -1662, -10171, -8509, 386, 5394, 386, -8509, -10171, -1662, 8184, 8887, 186,
 };
 
 #define BPF_MAX_TAPS 15
 
-//fs=4800 Hz, raised cosine, fc=300 Hz (BR=600 Bd), beta=0.8, N=14, gain=65536
-static int16_t lpf300[14] =
+//fs=9600 Hz, raised cosine, fc=300 Hz (BR=600 Bd), beta=0.8, N=14, gain=65536
+static const int16_t lpf300[14] =
 {
-	741, 1834, 3216, 4756, 6268, 7547, 8402, 8402, 7547, 6268, 4756, 3216, 1834, 741,
+	4385, 4515, 4627, 4720, 4793, 4846, 4878, 4878, 4846, 4793, 4720, 4627, 4515, 4385,
 };
 
-#ifdef EXPERIMENTAL_LPF1200
-//fs=9600 Hz, raised cosine, fc=600Hz (BR=1200 Bd), beta=0.8, N=14, gain=65536
-static const int16_t lpf1200[14] =
-{
-	741, 1834, 3216, 4756, 6268, 7547, 8402, 8402, 7547, 6268, 4756, 3216, 1834, 741,
-};
-#define LPF_MAX_TAPS 14
-#else
-static int16_t lpf1200[15] =
+//I don't remember what are this filter parameters,
+//but it seems to be the best among all I have tested
+static const int16_t lpf1200[15] =
 {
         -6128,
         -5974,
@@ -167,14 +164,11 @@ static int16_t lpf1200[15] =
         -6128
 };
 
-#define LPF_MAX_TAPS 15
-
-#endif
-
 //fs=38400 Hz, Gaussian, fc=4800 Hz (9600 Bd), N=9, gain=65536
-//seems like there is no difference between N=9 and any higher order
+//seems like there is almost no difference between N=9 and any higher order
 static int16_t lpf9600[9] = {497, 2360, 7178, 13992, 17478, 13992, 7178, 2360, 497};
 
+#define LPF_MAX_TAPS 15
 
 #define FILTER_MAX_TAPS ((LPF_MAX_TAPS > BPF_MAX_TAPS) ? LPF_MAX_TAPS : BPF_MAX_TAPS)
 
@@ -265,10 +259,8 @@ uint8_t ModemIsTxTestOngoing(void)
 
 void ModemClearRMS(uint8_t modem)
 {
-
 	demodState[modem].RMSenergy = 0;
 	demodState[modem].RMSsampleCount = 0;
-
 }
 
 uint16_t ModemGetRMS(uint8_t modem)
@@ -287,16 +279,26 @@ enum ModemPrefilter ModemGetFilterType(uint8_t modem)
  */
 static void setDcd(uint8_t state)
 {
-	if(state)
-	{
-		GPIOC->BSRR = GPIO_BSRR_BR13;
-		GPIOB->BSRR = GPIO_BSRR_BS5;
-	}
-	else
-	{
-		GPIOC->BSRR = GPIO_BSRR_BS13;
-		GPIOB->BSRR = GPIO_BSRR_BR5;
-	}
+	 if(state)
+	 {
+	 	GPIOC->BSRR = GPIO_BSRR_BR13;
+	 	GPIOB->BSRR = GPIO_BSRR_BS5;
+	 }
+	 else
+	 {
+	 	GPIOC->BSRR = GPIO_BSRR_BS13;
+	 	GPIOB->BSRR = GPIO_BSRR_BR5;
+	 }
+}
+
+static inline uint8_t descramble(uint8_t in)
+{
+	//G3RUH descrambling (x^17+x^12+1)
+	uint8_t bit = ((lfsr & 0x10000) > 0) ^ ((lfsr & 0x800) > 0) ^ (in > 0);
+
+	lfsr <<= 1;
+	lfsr |= in;
+	return bit;
 }
 
 static inline uint8_t scramble(uint8_t in)
@@ -311,14 +313,13 @@ static inline uint8_t scramble(uint8_t in)
 
 /**
  * @brief ISR for demodulator
- * Called at 9600 Hz by DMA
  */
 void DMA1_Channel2_IRQHandler(void) __attribute__ ((interrupt));
 void DMA1_Channel2_IRQHandler(void)
 {
-	if(DMA1->ISR & DMA_ISR_TCIF2)
-	{
-		DMA1->IFCR |= DMA_IFCR_CTCIF2;
+	 if(DMA1->ISR & DMA_ISR_TCIF2)
+	 {
+	 	DMA1->IFCR |= DMA_IFCR_CTCIF2;
 
 		int32_t sample = ((samples[0] + samples[1] + samples[2] + samples[3]) >> 1) - 4095; //calculate input sample (decimation)
 
@@ -345,83 +346,95 @@ void DMA1_Channel2_IRQHandler(void)
 	}
 }
 
-
-
 /**
  * @brief ISR for pushing DAC samples
  */
-void TIM1_UP_IRQHandler(void) __attribute__ ((interrupt));
-void TIM1_UP_IRQHandler(void)
+ void TIM1_UP_IRQHandler(void) __attribute__ ((interrupt));
+ void TIM1_UP_IRQHandler(void)
+ {
+ 	TIM1->SR &= ~TIM_SR_UIF;
+
+ 	int32_t sample = 0;
+
+ 	if(ModemConfig.modem == MODEM_9600)
+ 	{
+ 		if(ModemConfig.usePWM)
+ 			sample = scrambledSymbol ? 89 : 0;
+ 		else
+ 			sample = scrambledSymbol ? 15 : 0;
+
+ 		sample = filter(&demodState[0].lpf, sample);
+ 		if(sample < 0)
+ 			sample = 0;
+ 		else if(sample > 15)
+ 			sample = 15;
+ 	}
+ 	else
+ 	{
+ 		sample = dacSine[dacSineIdx];
+ 		dacSineIdx++;
+ 		dacSineIdx &= (DAC_SINE_SIZE - 1);
+ 	}
+
+ 	if(ModemConfig.usePWM)
+ 	{
+ 		TIM4->CCR1 = sample;
+ 	}
+ 	else
+ 	{
+ 		GPIOB->ODR &= ~0xF000; //zero 4 oldest bits
+ 		GPIOB->ODR |= (sample << 12); //write sample to 4 oldest bits
+ 	}
+
+ }
+
+void txBit()
 {
-	TIM1->SR &= ~TIM_SR_UIF;
-
-	int32_t sample = 0;
-
-	if(ModemConfig.modem == MODEM_9600)
+	if(Ax25GetTxBit() == 0) //get next bit and check if it's 0
 	{
-		if(ModemConfig.usePWM)
-			sample = currentSymbol ? 90 : 0;
-		else
-			sample = currentSymbol ? 15 : 1;
-
-		sample = filter(&demodState[0].lpf, sample);
-	}
-	else
-	{
-		sample = dacSine[dacSineIdx];
-		dacSineIdx++;
-		dacSineIdx &= (DAC_SINE_SIZE - 1);
-	}
-
-	if(ModemConfig.usePWM)
-	{
-		TIM4->CCR1 = sample;
-	}
-	else
-	{
-		GPIOB->ODR &= ~0xF000; //zero 4 oldest bits
-		GPIOB->ODR |= (sample << 12); //write sample to 4 oldest bits
-	}
+		currentSymbol ^= 1; //change symbol - NRZI encoding
+	}
+	//if 1, no symbol change
 
+	scrambledSymbol = scramble(currentSymbol);
 }
 
-
 /**
  * @brief ISR for baudrate generator timer. NRZI encoding is done here.
  */
-void TIM3_IRQHandler(void) __attribute__ ((interrupt));
-void TIM3_IRQHandler(void)
-{
-	TIM3->SR &= ~TIM_SR_UIF;
+ void TIM3_IRQHandler(void) __attribute__ ((interrupt));
+ void TIM3_IRQHandler(void)
+ {
+ 	TIM3->SR &= ~TIM_SR_UIF;
 
-	if(txTestState == TEST_DISABLED) //transmitting normal data
-	{
-		if(Ax25GetTxBit() == 0) //get next bit and check if it's 0
-		{
-			currentSymbol ^= 1; //change symbol - NRZI encoding
-		}
-		//if 1, no symbol change
-	}
-	else //transmit test mode
-	{
-		currentSymbol ^= 1; //change symbol
-	}
+ 	if(txTestState == TEST_DISABLED) //transmitting normal data
+ 	{
+ 		if(Ax25GetTxBit() == 0) //get next bit and check if it's 0
+ 		{
+ 			currentSymbol ^= 1; //change symbol - NRZI encoding
+ 		}
+ 		//if 1, no symbol change
+ 	}
+ 	else //transmit test mode
+ 	{
+ 		currentSymbol ^= 1; //change symbol
+ 	}
 
-	TIM1->CNT = 0;
+ 	TIM1->CNT = 0;
 
-	if(ModemConfig.modem == MODEM_9600)
-	{
-		currentSymbol = scramble(currentSymbol);
-	}
-	else
-	{
-		if(currentSymbol) //current symbol is space
-			TIM1->ARR = spaceStep;
-		else //mark
-			TIM1->ARR = markStep;
-	}
+ 	if(ModemConfig.modem == MODEM_9600)
+ 	{
+ 		scrambledSymbol = scramble(currentSymbol);
+ 	}
+ 	else
+ 	{
+ 		if(currentSymbol) //current symbol is space
+ 			TIM1->ARR = spaceStep;
+ 		else //mark
+ 			TIM1->ARR = markStep;
+ 	}
 
-}
+ }
 
 
 /**
@@ -468,6 +481,7 @@ static int32_t demodulate(int16_t sample, struct DemodState *dem)
 	}
 
 
+
 	//DCD using PLL
 	//PLL is running nominally at 1200 Hz (= baudrate)
 	//PLL timer is counting up and eventually overflows to a minimal negative value
@@ -480,11 +494,11 @@ static int32_t demodulate(int16_t sample, struct DemodState *dem)
 	//when configured properly, it's generally immune to noise, as the detected tone changes much faster than 1200 baud
 	//it's also important to set some maximum value for DCD counter, otherwise the DCD is "sticky"
 
-	dem->dcdPll = (signed)((unsigned)(dem->dcdPll) + ((unsigned)dem->pll)); //keep PLL ticking at the frequency equal to baudrate
+	dem->dcdPll = (signed)((unsigned)(dem->dcdPll) + (unsigned)(dem->pllStep)); //keep PLL ticking at the frequency equal to baudrate
 
 	if((sample > 0) != dem->dcdLastSymbol) //tone changed
 	{
-		if(abs(dem->dcdPll) < dem->dcdInc) //tone change occurred near zero
+		if(abs(dem->dcdPll) < dem->pllStep) //tone change occurred near zero
 			dem->dcdCounter += dem->dcdInc; //increase DCD counter
 		else //tone change occurred far from zero
 		{
@@ -504,43 +518,41 @@ static int32_t demodulate(int16_t sample, struct DemodState *dem)
 	else //below DCD threshold
 		dem->dcd = 0; //no DCD
 
-	//TODO: check if demodulator works well after all changes
 	sample = filter(&dem->lpf, sample);
 
-	if(ModemConfig.modem == MODEM_9600)
-	{
-		//AGC
-		if(sample >= dem->peak)
-		{
-			dem->peak += (((int32_t)(AGC9600_ATTACK * (float)32768) * (int32_t)(sample - dem->peak)) >> 15);
-		}
-		else
-		{
-			dem->peak += (((int32_t)(AGC9600_DECAY * (float)32768) * (int32_t)(sample - dem->peak)) >> 15);
-		}
+	// if(ModemConfig.modem == MODEM_9600)
+	// {
+	// 	//AGC, seems to be not needed
+	// 	if(sample >= dem->peak)
+	// 	{
+	// 		dem->peak += (((int32_t)(AGC9600_ATTACK * (float)32768) * (int32_t)(sample - dem->peak)) >> 15);
+	// 	}
+	// 	else
+	// 	{
+	// 		dem->peak += (((int32_t)(AGC9600_DECAY * (float)32768) * (int32_t)(sample - dem->peak)) >> 15);
+	// 	}
 
-		if(sample <= dem->valley)
-		{
-			dem->valley += (((int32_t)(AGC9600_ATTACK * (float)32768) * (int32_t)(sample - dem->valley)) >> 15);
-		}
-		else
-		{
-			dem->valley += (((int32_t)(AGC9600_DECAY * (float)32768) * (int32_t)(sample - dem->valley)) >> 15);
-		}
+	// 	if(sample <= dem->valley)
+	// 	{
+	// 		dem->valley += (((int32_t)(AGC9600_ATTACK * (float)32768) * (int32_t)(sample - dem->valley)) >> 15);
+	// 	}
+	// 	else
+	// 	{
+	// 		dem->valley += (((int32_t)(AGC9600_DECAY * (float)32768) * (int32_t)(sample - dem->valley)) >> 15);
+	// 	}
 
-		//remove DC component (subtract average value of peaks)
-		//and normalize to 32768 peak-to-peak (-16384:16384)
-		//32768 is equal to 1 << 15
-		if(dem->peak > dem->valley)
-		{
-			sample = ((((int32_t)(sample) - ((int32_t)(dem->peak + dem->valley) >> 1)) << 15) / (int32_t)(dem->peak - dem->valley));
-		}
-	}
+	// 	//remove DC component (subtract average value of peaks)
+	// 	//and normalize to 32768 peak-to-peak (-16384:16384)
+	// 	//32768 is equal to 1 << 15
+	// 	if(dem->peak > dem->valley)
+	// 	{
+	// 		sample = ((((int32_t)(sample) - ((int32_t)(dem->peak + dem->valley) >> 1)) << 15) / (int32_t)(dem->peak - dem->valley));
+	// 	}
+	// }
 
 	return sample > 0;
 }
 
-
 /**
  * @brief Decode received symbol: bit recovery, NRZI decoding and pass the decoded bit to higher level protocol
  * @param[in] symbol Received symbol
@@ -555,7 +567,7 @@ static void decode(uint8_t symbol, uint8_t demod)
 	//Current symbol is sampled at PLL counter overflow, so symbol transition should occur at PLL counter zero
 	int32_t previous = dem->pll; //store last clock state
 
-	dem->pll = (signed)((unsigned)(dem->pll) + (unsigned)dem->pllStep); //keep PLL running
+	dem->pll = (signed)((unsigned)(dem->pll) + (unsigned)(dem->pllStep)); //keep PLL running
 
 	dem->rawSymbols <<= 1; //store received unsynchronized symbol
 	dem->rawSymbols |= (symbol & 1);
@@ -572,7 +584,7 @@ static void decode(uint8_t symbol, uint8_t demod)
 			sym = 0;
 
 		if(ModemConfig.modem == MODEM_9600)
-			sym = scramble(sym); //descramble
+			sym = descramble(sym); //descramble
 
 		dem->syncSymbols |= sym;
 
@@ -589,12 +601,11 @@ static void decode(uint8_t symbol, uint8_t demod)
 
 	if(((dem->rawSymbols & 0x03) == 0b10) || ((dem->rawSymbols & 0x03) == 0b01)) //if there was a symbol transition, adjust PLL
 	{
-
-		if(Ax25GetRxStage(demod) != RX_STAGE_FRAME) //not in a frame
+		if(!dem->dcd) //PLL not locked
 		{
 			dem->pll = (int)(dem->pll * dem->pllNotLockedAdjust); //adjust PLL faster
 		}
-		else //in a frame
+		else //PLL locked
 		{
 			dem->pll = (int)(dem->pll * dem->pllLockedAdjust); //adjust PLL slower
 		}
@@ -606,62 +617,62 @@ static void decode(uint8_t symbol, uint8_t demod)
 
 void ModemTxTestStart(enum ModemTxTestMode type)
 {
-	if(txTestState != TEST_DISABLED) //TX test is already running
-		ModemTxTestStop(); //stop this test
+	 if(txTestState != TEST_DISABLED) //TX test is already running
+	 	ModemTxTestStop(); //stop this test
 
-	setPtt(1); //PTT on
-	txTestState = type;
+	 setPtt(1); //PTT on
+	 txTestState = type;
 
-	TIM2->CR1 &= ~TIM_CR1_CEN; //disable RX timer
-	TIM1->CR1 |= TIM_CR1_CEN; //enable DAC timer
+	 TIM2->CR1 &= ~TIM_CR1_CEN; //disable RX timer
+	 TIM1->CR1 |= TIM_CR1_CEN; //enable DAC timer
 
-	NVIC_DisableIRQ(DMA1_Channel2_IRQn); //disable RX DMA interrupt
-	NVIC_EnableIRQ(TIM1_UP_IRQn); //enable DAC interrupt
+	 NVIC_DisableIRQ(DMA1_Channel2_IRQn); //disable RX DMA interrupt
+	 NVIC_EnableIRQ(TIM1_UP_IRQn); //enable DAC interrupt
 
-	if(type == TEST_MARK)
-	{
-		TIM1->ARR = markStep;
-	}
-	else if(type == TEST_SPACE)
-	{
-		TIM1->ARR = spaceStep;
-	}
-	else //alternating tones
-	{
-		//enable baudrate generator
-		TIM3->CR1 = TIM_CR1_CEN; //enable timer
-		NVIC_EnableIRQ(TIM3_IRQn); //enable interrupt in NVIC
-	}
+	 if(type == TEST_MARK)
+	 {
+	 	TIM1->ARR = markStep;
+	 }
+	 else if(type == TEST_SPACE)
+	 {
+	 	TIM1->ARR = spaceStep;
+	 }
+	 else //alternating tones
+	 {
+	 	//enable baudrate generator
+	 	TIM3->CR1 = TIM_CR1_CEN; //enable timer
+	 	NVIC_EnableIRQ(TIM3_IRQn); //enable interrupt in NVIC
+	 }
 }
 
 
 void ModemTxTestStop(void)
 {
-	txTestState = TEST_DISABLED;
+	 txTestState = TEST_DISABLED;
 
-	TIM3->CR1 &= ~TIM_CR1_CEN; //disable baudrate timer
-	TIM1->CR1 &= ~TIM_CR1_CEN; //disable DAC timer
-	TIM2->CR1 |= TIM_CR1_CEN; //enable RX timer
+	 TIM3->CR1 &= ~TIM_CR1_CEN; //disable baudrate timer
+	 TIM1->CR1 &= ~TIM_CR1_CEN; //disable DAC timer
+	 TIM2->CR1 |= TIM_CR1_CEN; //enable RX timer
 
-	NVIC_DisableIRQ(TIM3_IRQn);
-	NVIC_DisableIRQ(TIM1_UP_IRQn);
-	NVIC_EnableIRQ(DMA1_Channel2_IRQn);
+	 NVIC_DisableIRQ(TIM3_IRQn);
+	 NVIC_DisableIRQ(TIM1_UP_IRQn);
+	 NVIC_EnableIRQ(DMA1_Channel2_IRQn);
 
-	setPtt(0); //PTT off
+	 setPtt(0); //PTT off
 }
 
 
 void ModemTransmitStart(void)
 {
-	setPtt(1); //PTT on
+	 setPtt(1); //PTT on
 
-	TIM3->CR1 = TIM_CR1_CEN;
-	TIM1->CR1 = TIM_CR1_CEN;
-	TIM2->CR1 &= ~TIM_CR1_CEN;
+	 TIM3->CR1 = TIM_CR1_CEN;
+	 TIM1->CR1 = TIM_CR1_CEN;
+	 TIM2->CR1 &= ~TIM_CR1_CEN;
 
-	NVIC_DisableIRQ(DMA1_Channel2_IRQn);
-	NVIC_EnableIRQ(TIM1_UP_IRQn);
-	NVIC_EnableIRQ(TIM3_IRQn);
+	 NVIC_DisableIRQ(DMA1_Channel2_IRQn);
+	 NVIC_EnableIRQ(TIM1_UP_IRQn);
+	 NVIC_EnableIRQ(TIM3_IRQn);
 }
 
 
@@ -689,10 +700,10 @@ void ModemTransmitStop(void)
  */
 static void setPtt(uint8_t state)
 {
-	if(state)
-		GPIOB->BSRR = GPIO_BSRR_BS7;
-	else
-		GPIOB->BSRR = GPIO_BSRR_BR7;
+	 if(state)
+	 	GPIOB->BSRR = GPIO_BSRR_BS7;
+	 else
+	 	GPIOB->BSRR = GPIO_BSRR_BR7;
 }
 
 
@@ -710,86 +721,108 @@ void ModemInit(void)
 	 * TIM2 is the RX sampling timer with no software interrupt, but it directly calls DMA
 	 */
 
-	RCC->APB2ENR |= RCC_APB2ENR_IOPBEN;
-	RCC->APB2ENR |= RCC_APB2ENR_IOPCEN;
-	RCC->APB2ENR |= RCC_APB2ENR_IOPAEN;
-	RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;
-	RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;
-	RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
-	RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;
-	RCC->AHBENR |= RCC_AHBENR_DMA1EN;
+	 RCC->APB2ENR |= RCC_APB2ENR_IOPBEN;
+	 RCC->APB2ENR |= RCC_APB2ENR_IOPCEN;
+	 RCC->APB2ENR |= RCC_APB2ENR_IOPAEN;
+	 RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;
+	 RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;
+	 RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
+	 RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;
+	 RCC->AHBENR |= RCC_AHBENR_DMA1EN;
 
 
-	GPIOC->CRH |= GPIO_CRH_MODE13_1; //DCD LED on PC13
-	GPIOC->CRH &= ~GPIO_CRH_MODE13_0;
-	GPIOC->CRH &= ~GPIO_CRH_CNF13;
+	 GPIOC->CRH |= GPIO_CRH_MODE13_1; //DCD LED on PC13
+	 GPIOC->CRH &= ~GPIO_CRH_MODE13_0;
+	 GPIOC->CRH &= ~GPIO_CRH_CNF13;
 
-	GPIOB->CRH &= ~0xFFFF0000; //R2R output on PB12-PB15
-	GPIOB->CRH |= 0x22220000;
+	 GPIOB->CRH &= ~0xFFFF0000; //R2R output on PB12-PB15
+	 GPIOB->CRH |= 0x22220000;
 
-	GPIOA->CRL &= ~GPIO_CRL_CNF0; //ADC input on PA0
-	GPIOA->CRL &= ~GPIO_CRL_MODE0;
+	 GPIOA->CRL &= ~GPIO_CRL_CNF0; //ADC input on PA0
+	 GPIOA->CRL &= ~GPIO_CRL_MODE0;
 
 
-	GPIOB->CRL |= GPIO_CRL_MODE7_1; //PTT output on PB7
-	GPIOB->CRL &= ~GPIO_CRL_MODE7_0;
-	GPIOB->CRL &= ~GPIO_CRL_CNF7;
+	 GPIOB->CRL |= GPIO_CRL_MODE7_1; //PTT output on PB7
+	 GPIOB->CRL &= ~GPIO_CRL_MODE7_0;
+	 GPIOB->CRL &= ~GPIO_CRL_CNF7;
 
-	GPIOB->CRL |= GPIO_CRL_MODE5_1; //2nd DCD LED on PB5
-	GPIOB->CRL &= ~GPIO_CRL_MODE5_0;
-	GPIOB->CRL &= ~GPIO_CRL_CNF5;
+	 GPIOB->CRL |= GPIO_CRL_MODE5_1; //2nd DCD LED on PB5
+	 GPIOB->CRL &= ~GPIO_CRL_MODE5_0;
+	 GPIOB->CRL &= ~GPIO_CRL_CNF5;
 
 
-	RCC->CFGR |= RCC_CFGR_ADCPRE_1; //ADC prescaler /6
-	RCC->CFGR &= ~RCC_CFGR_ADCPRE_0;
+	 RCC->CFGR |= RCC_CFGR_ADCPRE_1; //ADC prescaler /6
+	 RCC->CFGR &= ~RCC_CFGR_ADCPRE_0;
 
-	ADC1->CR2 |= ADC_CR2_CONT; //continuous conversion
-	ADC1->CR2 |= ADC_CR2_EXTSEL;
-	ADC1->SQR1 &= ~ADC_SQR1_L; //1 conversion
-    ADC1->SMPR2 |= ADC_SMPR2_SMP0_2; //41.5 cycle sampling
-	ADC1->SQR3 &= ~ADC_SQR3_SQ1; //channel 0 is first in the sequence
-    ADC1->CR2 |= ADC_CR2_ADON; //ADC on
+	 ADC1->CR2 |= ADC_CR2_CONT; //continuous conversion
+	 ADC1->CR2 |= ADC_CR2_EXTSEL;
+	 ADC1->SQR1 &= ~ADC_SQR1_L; //1 conversion
+     ADC1->SMPR2 |= ADC_SMPR2_SMP0_2; //41.5 cycle sampling
+	 ADC1->SQR3 &= ~ADC_SQR3_SQ1; //channel 0 is first in the sequence
+     ADC1->CR2 |= ADC_CR2_ADON; //ADC on
 
-	ADC1->CR2 |= ADC_CR2_RSTCAL; //calibrate ADC
-	while(ADC1->CR2 & ADC_CR2_RSTCAL)
-		;
-	ADC1->CR2 |= ADC_CR2_CAL;
-	while(ADC1->CR2 & ADC_CR2_CAL)
-		;
+	 ADC1->CR2 |= ADC_CR2_RSTCAL; //calibrate ADC
+	 while(ADC1->CR2 & ADC_CR2_RSTCAL)
+	 	;
+	 ADC1->CR2 |= ADC_CR2_CAL;
+	 while(ADC1->CR2 & ADC_CR2_CAL)
+	 	;
 
-	ADC1->CR2 |= ADC_CR2_EXTTRIG;
-	ADC1->CR2 |= ADC_CR2_SWSTART; //start ADC conversion
+	 ADC1->CR2 |= ADC_CR2_EXTTRIG;
+	 ADC1->CR2 |= ADC_CR2_SWSTART; //start ADC conversion
 
-	//prepare DMA
-	DMA1_Channel2->CCR |= DMA_CCR_MSIZE_0; //16 bit memory region
-	DMA1_Channel2->CCR &= ~DMA_CCR_MSIZE_1;
-    DMA1_Channel2->CCR |= DMA_CCR_PSIZE_0;
-	DMA1_Channel2->CCR &= ~DMA_CCR_PSIZE_1;
+	 //prepare DMA
+	 DMA1_Channel2->CCR |= DMA_CCR_MSIZE_0; //16 bit memory region
+	 DMA1_Channel2->CCR &= ~DMA_CCR_MSIZE_1;
+     DMA1_Channel2->CCR |= DMA_CCR_PSIZE_0;
+	 DMA1_Channel2->CCR &= ~DMA_CCR_PSIZE_1;
 
-	DMA1_Channel2->CCR |= DMA_CCR_MINC | DMA_CCR_CIRC| DMA_CCR_TCIE; //circular mode, memory increment and interrupt
-    DMA1_Channel2->CNDTR = 4; //4 samples
-	DMA1_Channel2->CPAR = (uint32_t)&(ADC1->DR); //ADC data register address
-	DMA1_Channel2->CMAR = (uint32_t)samples; //sample buffer address
-	DMA1_Channel2->CCR |= DMA_CCR_EN; //enable DMA
+	 DMA1_Channel2->CCR |= DMA_CCR_MINC | DMA_CCR_CIRC| DMA_CCR_TCIE; //circular mode, memory increment and interrupt
+     DMA1_Channel2->CNDTR = 4; //4 samples
+	 DMA1_Channel2->CPAR = (uint32_t)&(ADC1->DR); //ADC data register address
+	 DMA1_Channel2->CMAR = (uint32_t)samples; //sample buffer address
+	 DMA1_Channel2->CCR |= DMA_CCR_EN; //enable DMA
 
-	NVIC_EnableIRQ(DMA1_Channel2_IRQn);
+	 NVIC_EnableIRQ(DMA1_Channel2_IRQn);
 
 
-	//RX sampling timer
-	TIM2->PSC = 8; //72/9=8 MHz
-	TIM2->DIER |= TIM_DIER_UDE; //enable calling DMA on timer tick
+	 //RX sampling timer
+	 TIM2->PSC = 8; //72/9=8 MHz
+	 TIM2->DIER |= TIM_DIER_UDE; //enable calling DMA on timer tick
 
-	//TX DAC timer
-	TIM1->PSC = 17; //72/18=4 MHz
-	TIM1->DIER |= TIM_DIER_UIE;
+	 //TX DAC timer
+	 TIM1->PSC = 17; //72/18=4 MHz
+	 TIM1->DIER |= TIM_DIER_UIE;
 
-	//baudrate timer
-	TIM3->PSC = 71; //72/72=1 MHz
-	TIM3->DIER |= TIM_DIER_UIE;
+	 //baudrate timer
+	 TIM3->PSC = 71; //72/72=1 MHz
+	 TIM3->DIER |= TIM_DIER_UIE;
 
-	if((ModemConfig.modem == MODEM_1200) || (ModemConfig.modem == MODEM_1200_V23))
+	if((ModemConfig.modem == MODEM_1200) || (ModemConfig.modem == MODEM_1200_V23)
+#ifdef ENABLE_PSK
+		|| (ModemConfig.modem == MODEM_BPSK_1200) || (ModemConfig.modem == MODEM_QPSK_1200)
+#endif
+	)
 	{
-		demodCount = 2;
+
+
+		//use one modem in FX.25 mode
+		//FX.25 (RS) functions are not reentrant
+		//also they are BIG
+		if(
+#ifdef ENABLE_FX25
+			Ax25Config.fx25
+#else
+			0
+#endif
+#ifdef ENABLE_PSK
+		|| (ModemConfig.modem == MODEM_BPSK_1200) || (ModemConfig.modem == MODEM_QPSK_1200)
+#endif
+		)
+			demodCount = 1;
+		else
+
+			demodCount = 2;
 		N = N1200;
 		baudRate = 1200.f;
 
@@ -811,27 +844,39 @@ void ModemInit(void)
 		demodState[1].dcdDec = DCD1200_DEC;
 		demodState[1].dcdAdjust = DCD1200_PLLTUNE;
 
-		demodState[0].prefilter = PREFILTER_NONE;
-		demodState[0].lpf.coeffs = lpf1200;
-		demodState[0].lpf.taps = sizeof(lpf1200) / sizeof(*lpf1200);
-		demodState[0].lpf.gainShift = 0; //not important, output is always compared with 0
-		demodState[1].lpf.coeffs = lpf1200;
+		demodState[1].prefilter = PREFILTER_NONE;
+		demodState[1].lpf.coeffs = (int16_t*)lpf1200;
 		demodState[1].lpf.taps = sizeof(lpf1200) / sizeof(*lpf1200);
-		demodState[1].lpf.gainShift = 0; //not important, output is always compared with 0
+		demodState[1].lpf.gainShift = 15;
 
+		demodState[0].lpf.coeffs = (int16_t*)lpf1200;
+		demodState[0].lpf.taps = sizeof(lpf1200) / sizeof(*lpf1200);
+		demodState[0].lpf.gainShift = 15;
+		demodState[0].prefilter = PREFILTER_NONE;
+
+#ifdef ENABLE_PSK
+		if((ModemConfig.modem != MODEM_BPSK_1200) && (ModemConfig.modem != MODEM_QPSK_1200))
+		{
+#endif
 		if(ModemConfig.flatAudioIn) //when used with flat audio input, use deemphasis and flat modems
 		{
-			demodState[1].prefilter = PREFILTER_DEEMPHASIS;
-			demodState[1].bpf.coeffs = bpf1200Inv;
-			demodState[1].bpf.taps = sizeof(bpf1200Inv) / sizeof(*bpf1200Inv);
-			demodState[1].bpf.gainShift = 15;
+#ifdef ENABLE_FX25
+			if(Ax25Config.fx25)
+				demodState[0].prefilter = PREFILTER_NONE;
+			else
+#endif
+				demodState[0].prefilter = PREFILTER_DEEMPHASIS;
+			demodState[0].bpf.coeffs = (int16_t*)bpf1200Inv;
+			demodState[0].bpf.taps = sizeof(bpf1200Inv) / sizeof(*bpf1200Inv);
+			demodState[0].bpf.gainShift = 15;
+
 		}
 		else //when used with normal (filtered) audio input, use flat and preemphasis modems
 		{
-			demodState[1].prefilter = PREFILTER_PREEMPHASIS;
-			demodState[1].bpf.coeffs = bpf1200;
-			demodState[1].bpf.taps = sizeof(bpf1200) / sizeof(*bpf1200);
-			demodState[1].bpf.gainShift = 15;
+			demodState[0].prefilter = PREFILTER_PREEMPHASIS;
+			demodState[0].bpf.coeffs = (int16_t*)bpf1200;
+			demodState[0].bpf.taps = sizeof(bpf1200) / sizeof(*bpf1200);
+			demodState[0].bpf.gainShift = 15;
 		}
 
 		if(ModemConfig.modem == MODEM_1200) //Bell 202
@@ -844,6 +889,13 @@ void ModemInit(void)
 			markFreq = 1300.f;
 			spaceFreq = 2100.f;
 		}
+#ifdef ENABLE_PSK
+		}
+		else
+		{
+			markFreq = 1700.f; //use as center frequency in PSK
+		}
+#endif
 
 		TIM2->ARR = 207; //8MHz / 208 =~38400 Hz (4*9600 Hz for 4x oversampling)
 	}
@@ -865,14 +917,14 @@ void ModemInit(void)
 		demodState[0].dcdAdjust = DCD300_PLLTUNE;
 
 		demodState[0].prefilter = PREFILTER_FLAT;
-		demodState[0].bpf.coeffs = bpf300;
+		demodState[0].bpf.coeffs = (int16_t*)bpf300;
 		demodState[0].bpf.taps = sizeof(bpf300) / sizeof(*bpf300);
 		demodState[0].bpf.gainShift = 16;
-		demodState[0].lpf.coeffs = lpf300;
+		demodState[0].lpf.coeffs = (int16_t*)lpf300;
 		demodState[0].lpf.taps = sizeof(lpf300) / sizeof(*lpf300);
-		demodState[0].lpf.gainShift = 0; //not important, output is always compared with 0
+		demodState[0].lpf.gainShift = 15;
 
-		TIM2->ARR = 416; //8MHz / 416 =~19200 Hz (4*4800 Hz for 4x oversampling)
+		TIM2->ARR = 207; //8MHz / 208 =~38400 Hz (4*9600 Hz for 4x oversampling)
 	}
 	else if(ModemConfig.modem == MODEM_9600)
 	{
@@ -891,7 +943,7 @@ void ModemInit(void)
 
 		demodState[0].prefilter = PREFILTER_NONE;
 		//this filter will be used for RX and TX
-		demodState[0].lpf.coeffs = lpf9600;
+		demodState[0].lpf.coeffs = (int16_t*)lpf9600;
 		demodState[0].lpf.taps = sizeof(lpf9600) / sizeof(*lpf9600);
 		demodState[0].lpf.gainShift = 16;
 
@@ -925,21 +977,21 @@ void ModemInit(void)
 
 	if(ModemConfig.usePWM)
 	{
-		RCC->APB1ENR |= RCC_APB1ENR_TIM4EN; //configure timer
+		 RCC->APB1ENR |= RCC_APB1ENR_TIM4EN; //configure timer
 
-		GPIOB->CRL |= GPIO_CRL_CNF6_1; //configure pin for PWM
-		GPIOB->CRL |= GPIO_CRL_MODE6;
-		GPIOB->CRL &= ~GPIO_CRL_CNF6_0;
+		 GPIOB->CRL |= GPIO_CRL_CNF6_1; //configure pin for PWM
+		 GPIOB->CRL |= GPIO_CRL_MODE6;
+		 GPIOB->CRL &= ~GPIO_CRL_CNF6_0;
 
-		//set up PWM generation
-		TIM4->PSC = 7; //72MHz/8=9MHz
-		TIM4->ARR = 90; //9MHz/90=100kHz
+		 //set up PWM generation
+		 TIM4->PSC = 7; //72MHz/8=9MHz
+		 TIM4->ARR = 90; //9MHz/90=100kHz
 
-		TIM4->CCMR1 |= TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2;
-		TIM4->CCER |= TIM_CCER_CC1E;
-		TIM4->CCR1 = 44; //initial duty cycle
+		 TIM4->CCMR1 |= TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2;
+		 TIM4->CCER |= TIM_CCER_CC1E;
+		 TIM4->CCR1 = 44; //initial duty cycle
 
-		TIM4->CR1 |= TIM_CR1_CEN;
+		 TIM4->CR1 |= TIM_CR1_CEN;
 	}
 
 }
diff --git a/Src/fx25.c b/Src/fx25.c
index e693ced..9c84b2e 100644
--- a/Src/fx25.c
+++ b/Src/fx25.c
@@ -5,7 +5,10 @@
 #include <stddef.h>
 #include "rs.h"
 
+#define FX25_RS_FCR 1
+
 #define FX25_PREGENERATE_POLYS
+#define FX25_MAX_DISTANCE 10 //maximum Hamming distance when comparing tags
 
 const struct Fx25Mode Fx25ModeList[11] =
 {
@@ -22,8 +25,42 @@ const struct Fx25Mode Fx25ModeList[11] =
 	{.tag =  0x4A4ABEC4A724B796, .K = 64, .T = 64}
 };
 
-const struct Fx25Mode* Fx25GetMode(uint16_t size)
+static inline uint8_t hammingDistance(uint64_t x, uint64_t y)
 {
+	uint8_t distance = 0;
+	for(uint8_t i = 0; i < 64; i++)
+	{
+		distance += (x ^ y) & 1;
+		x >>= 1;
+		y >>= 1;
+	}
+	return distance;
+}
+
+const struct Fx25Mode* Fx25GetModeForTag(uint64_t tag)
+{
+	struct Fx25Mode *closest = NULL;
+	uint8_t closestDistance = 255;
+	for(uint8_t i = 0; i < sizeof(Fx25ModeList) / sizeof(*Fx25ModeList); i++)
+	{
+		uint8_t distance = hammingDistance(tag, Fx25ModeList[i].tag);
+		if(distance == 0)
+			return &Fx25ModeList[i];
+		else if(distance < closestDistance)
+		{
+			closest = (struct Fx25Mode*)&Fx25ModeList[i];
+			closestDistance = distance;
+		}
+	}
+	if(closestDistance <= FX25_MAX_DISTANCE)
+		return closest;
+	else
+		return NULL;
+}
+
+const struct Fx25Mode* Fx25GetModeForSize(uint16_t size)
+{
+	//use "UZ7HO Soundmodem standard" for choosing FX.25 mode
 	if(size <= 32)
 		return &Fx25ModeList[3];
 	else if(size <= 64)
@@ -41,41 +78,71 @@ const struct Fx25Mode* Fx25GetMode(uint16_t size)
 }
 
 #ifdef FX25_PREGENERATE_POLYS
-static uint8_t poly16[17], poly32[33], poly64[65];
+static struct LwFecRS rs16, rs32, rs64;
 #else
-static uint8_t poly[65];
+static struct LwFecRS rs;
 #endif
 
-void Fx25AddParity(uint8_t *buffer, const struct Fx25Mode *mode)
+void Fx25Encode(uint8_t *buffer, const struct Fx25Mode *mode)
 {
 #ifdef FX25_PREGENERATE_POLYS
-	uint8_t *poly = NULL;
+	struct LwFecRS *rs = NULL;
 	switch(mode->T)
 	{
 		case 16:
-			poly = poly16;
+			rs = &rs16;
 			break;
 		case 32:
-			poly = poly32;
+			rs = &rs32;
 			break;
 		case 64:
-			poly = poly64;
+			rs = &rs64;
 			break;
 		default:
-			poly = poly16;
+			rs = &rs16;
+			break;
 	}
+	RsEncode(rs, buffer, mode->K);
 #else
-	RsGeneratePolynomial(mode->T, poly);
+	RsInit(&rs, mode->T, FX25_RS_FCR);
+	RsEncode(&rs, buffer, mode->K);
 #endif
-	RsEncode(buffer, mode->K + mode->T, poly, mode->T);
+
+}
+
+bool Fx25Decode(uint8_t *buffer, const struct Fx25Mode *mode, uint8_t *fixed)
+{
+#ifdef FX25_PREGENERATE_POLYS
+	struct LwFecRS *rs = NULL;
+	switch(mode->T)
+	{
+		case 16:
+			rs = &rs16;
+			break;
+		case 32:
+			rs = &rs32;
+			break;
+		case 64:
+			rs = &rs64;
+			break;
+		default:
+			rs = &rs16;
+			break;
+	}
+	return RsDecode(rs, buffer, mode->K, fixed);
+#else
+	RsInit(&rs, mode->T, FX25_RS_FCR);
+	return RsDecode(&rs, buffer, mode->K, fixed);
+#endif
+
 }
 
 void Fx25Init(void)
 {
 #ifdef FX25_PREGENERATE_POLYS
-	RsGeneratePolynomial(16, poly16);
-	RsGeneratePolynomial(32, poly32);
-	RsGeneratePolynomial(64, poly64);
+	RsInit(&rs16, 16, FX25_RS_FCR);
+	RsInit(&rs32, 32, FX25_RS_FCR);
+	RsInit(&rs64, 64, FX25_RS_FCR);
 #else
 #endif
 }
diff --git a/Src/main.c b/Src/main.c
index 2f37590..ec9e314 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -99,14 +99,12 @@ static void handleFrame(void)
 	uint8_t *buf;
 	uint16_t size = 0;
 	uint16_t signalLevel = 0;
+	uint8_t fixed = 0;
 
-	while(Ax25ReadNextRxFrame(&buf, &size, &signalLevel))
+	while(Ax25ReadNextRxFrame(&buf, &size, &signalLevel, &fixed))
 	{
-
 		TermSendToAll(MODE_KISS, buf, size);
 
-
-
 		if(((UartUsb.mode == MODE_MONITOR) || (Uart1.mode == MODE_MONITOR) || (Uart2.mode == MODE_MONITOR)))
 		{
 			//in general, the RMS of the frame is calculated (excluding preamble!)
@@ -127,30 +125,35 @@ static void handleFrame(void)
 				TermSendToAll(MODE_MONITOR, (uint8_t*)"\r\nInput level too low! Please increase so most stations are around 50-70%.\r\n", 0);
 			}
 
-			TermSendToAll(MODE_MONITOR, (uint8_t*)"(AX.25) Frame received [", 0); //show which modem received the frame: [FP] (flat and preemphasized), [FD] (flat and deemphasized - in flat audio input mode)
-																	   //[F_] (only flat), [_P] (only preemphasized) or [_D] (only deemphasized - in flat audio input mode)
-			for(uint8_t i = 0; i < ModemGetDemodulatorCount(); i++)
+			TermSendToAll(MODE_MONITOR, (uint8_t*)"(AX.25) Frame received [", 0);
+			if(fixed == 0)
 			{
-				if(modemBitmap & (1 << i))
+				for(uint8_t i = 0; i < ModemGetDemodulatorCount(); i++)
 				{
-					enum ModemPrefilter m = ModemGetFilterType(i);
-					switch(m)
+					if(modemBitmap & (1 << i))
 					{
-						case PREFILTER_PREEMPHASIS:
-							TermSendToAll(MODE_MONITOR, (uint8_t*)"P", 1);
-							break;
-						case PREFILTER_DEEMPHASIS:
-							TermSendToAll(MODE_MONITOR, (uint8_t*)"D", 1);
-							break;
-						case PREFILTER_FLAT:
-						default:
-							TermSendToAll(MODE_MONITOR, (uint8_t*)"F", 1);
-							break;
+						enum ModemPrefilter m = ModemGetFilterType(i);
+						switch(m)
+						{
+							case PREFILTER_PREEMPHASIS:
+								TermSendToAll(MODE_MONITOR, (uint8_t*)"P", 1);
+								break;
+							case PREFILTER_DEEMPHASIS:
+								TermSendToAll(MODE_MONITOR, (uint8_t*)"D", 1);
+								break;
+							case PREFILTER_FLAT:
+								TermSendToAll(MODE_MONITOR, (uint8_t*)"F", 1);
+								break;
+							case PREFILTER_NONE:
+								TermSendToAll(MODE_MONITOR, (uint8_t*)"|", 1);
+						}
 					}
+					else
+						TermSendToAll(MODE_MONITOR, (uint8_t*)"_", 1);
 				}
-				else
-					TermSendToAll(MODE_MONITOR, (uint8_t*)"_", 1);
 			}
+			else
+				TermSendNumberToAll(MODE_MONITOR, fixed);
 
 			TermSendToAll(MODE_MONITOR, (uint8_t*)"], signal level ", 0);
 			TermSendNumberToAll(MODE_MONITOR, signalLevel);
@@ -229,6 +232,7 @@ int main(void)
 	Ax25Config.quietTime = 300;
 	Ax25Config.txDelayLength = 300;
 	Ax25Config.txTailLength = 30;
+	Ax25Config.fx25 = 0;
 	DigiConfig.dupeTime = 30;
 
 	ConfigRead();
diff --git a/Src/terminal.c b/Src/terminal.c
index 5a99b73..6bf6527 100644
--- a/Src/terminal.c
+++ b/Src/terminal.c
@@ -150,7 +150,9 @@ static const char configHelp[] = 	"\r\nCommands available in config mode:\r\n"
 		"digi dupe <5-255> - sets anti-dupe buffer time (s)\r\n"
 		"digi list <0-19> [set <call>/remove] - sets/clears specified callsign slot in filter list\r\n"
 		"monkiss [on/off] - send own and digipeated frames to KISS ports\r\n"
-		"nonaprs [on/off] - enable reception of non-APRS frames\r\n";
+		"nonaprs [on/off] - enable reception of non-APRS frames\r\n"
+		"fx25 [on/off] - enable FX.25 protocol (AX.25 + FEC)\r\n"
+		"fx25tx [on/off] - enable TX in FX.25 mode\r\n";
 
 
 
@@ -330,6 +332,16 @@ static void printConfig(Uart *src)
 		UartSendString(src, "On\r\n", 0);
 	else
 		UartSendString(src, "Off\r\n", 0);
+	UartSendString(src, "FX.25 protocol: ", 0);
+	if(Ax25Config.fx25 == 1)
+		UartSendString(src, "On\r\n", 0);
+	else
+		UartSendString(src, "Off\r\n", 0);
+	UartSendString(src, "FX.25 TX: ", 0);
+	if(Ax25Config.fx25Tx == 1)
+		UartSendString(src, "On\r\n", 0);
+	else
+		UartSendString(src, "Off\r\n", 0);
 }
 
 static void sendTime(Uart *src)
@@ -1002,6 +1014,24 @@ void TermParse(Uart *src)
 		else
 			err = true;
 	}
+	else if(!strncmp(cmd, "fx25 ", 5))
+	{
+		if(!strncmp(&cmd[5], "on", 2))
+			Ax25Config.fx25 = 1;
+		else if(!strncmp(&cmd[5], "off", 2))
+			Ax25Config.fx25 = 0;
+		else
+			err = true;
+	}
+	else if(!strncmp(cmd, "fx25tx ", 7))
+	{
+		if(!strncmp(&cmd[7], "on", 2))
+			Ax25Config.fx25Tx = 1;
+		else if(!strncmp(&cmd[7], "off", 2))
+			Ax25Config.fx25Tx = 0;
+		else
+			err = true;
+	}
 	else
 	{
 		UartSendString(src, "Unknown command. For command list type \"help\"\r\n", 0);
diff --git a/lwfec b/lwfec
new file mode 160000
index 0000000..c4b8bbf
--- /dev/null
+++ b/lwfec
@@ -0,0 +1 @@
+Subproject commit c4b8bbf1ff6fa1b3f6fa1a584eab6fb42732a91b