diff --git a/Modem/Modem_user.mk b/Modem/Modem_user.mk
index f6fad09..4762ddb 100644
--- a/Modem/Modem_user.mk
+++ b/Modem/Modem_user.mk
@@ -11,7 +11,9 @@ Modem_PROGRAMMER_PORT = none
 Modem_USER_CSRC = \
 	$(Modem_SRC_PATH)/main.c \
 	$(Modem_HW_PATH)/hardware.c \
-        $(Modem_HW_PATH)/afsk.c \
+    $(Modem_HW_PATH)/afsk.c \
+    bertos/net/ax25.c \
+    bertos/algo/crc_ccitt.c \
 	#
 
 # Files included by the user.
diff --git a/Modem/afsk.c b/Modem/afsk.c
index 7fa74ce..b01dbf5 100644
--- a/Modem/afsk.c
+++ b/Modem/afsk.c
@@ -5,17 +5,13 @@
 #include <drv/timer.h>
 #include <cfg/module.h>
 
-#define HDLC_FLAG  0x7E
-#define HDLC_RESET 0x7F
-#define AX25_ESC   0x1B
-
 #include <cfg/log.h>
 
 #include <cpu/power.h>
 #include <cpu/pgm.h>
 #include <struct/fifobuf.h>
 
-#include <string.h> /* memset */
+#include <string.h>
 
 // Sine table for DAC DDS
 #define SIN_LEN 512 // Length of a full wave. Table is 1/4 wave.
@@ -57,11 +53,20 @@ INLINE uint8_t sinSample(uint16_t i) {
 #define SPACE_FREQ 2200
 #define SPACE_INC  (uint16_t)(DIV_ROUND(SIN_LEN * (uint32_t)SPACE_FREQ, CONFIG_AFSK_DAC_SAMPLERATE))
 
+// HDLC flag bytes
+#define HDLC_FLAG  0x7E
+#define HDLC_RESET 0x7F
+#define AX25_ESC   0x1B
+
 // Check that sample rate is divisible by bitrate
 STATIC_ASSERT(!(CONFIG_AFSK_DAC_SAMPLERATE % BITRATE));
 
 #define DAC_SAMPLESPERBIT (CONFIG_AFSK_DAC_SAMPLERATE / BITRATE)
 
+//////////////////////////////////////////////////////
+// Link Layer Control and Demodulation              //
+//////////////////////////////////////////////////////
+
 static bool hdlcParse(Hdlc *hdlc, bool bit, FIFOBuffer *fifo) {
 	bool ret = true;
 
@@ -124,6 +129,7 @@ static bool hdlcParse(Hdlc *hdlc, bool bit, FIFOBuffer *fifo) {
 			ret = false;
 		}
 
+		// Wipe received byte and reset bit index to 0
 		hdlc->currentByte = 0;
 		hdlc->bitIndex = 0;
 
@@ -199,6 +205,10 @@ void afsk_adc_isr(Afsk *afsk, int8_t currentSample) {
 	}
 }
 
+//////////////////////////////////////////////////////
+// Signal modulation and DAC                        //
+//////////////////////////////////////////////////////
+
 static void afsk_txStart(Afsk *af)
 {
 	if (!af->sending)
@@ -214,17 +224,12 @@ static void afsk_txStart(Afsk *af)
 }
 
 #define BIT_STUFF_LEN 5
-
 #define SWITCH_TONE(inc)  (((inc) == MARK_INC) ? SPACE_INC : MARK_INC)
 
 /**
  * DAC ISR callback.
  * This function has to be called by the DAC ISR when a sample of the configured
  * channel has been converted out.
- *
- * \param af Afsk context to operate on.
- *
- * \return The next DAC output sample.
  */
 uint8_t afsk_dac_isr(Afsk *af)
 {
@@ -343,6 +348,9 @@ uint8_t afsk_dac_isr(Afsk *af)
 	return sinSample(af->phaseAcc);
 }
 
+//////////////////////////////////////////////////////
+// File operation overwrites for read/write         //
+//////////////////////////////////////////////////////
 
 static size_t afsk_read(KFile *fd, void *_buf, size_t size)
 {
@@ -414,6 +422,10 @@ static void afsk_clearerr(KFile *fd)
 	ATOMIC(af->status = 0);
 }
 
+//////////////////////////////////////////////////////
+// Modem Initialization                             //
+//////////////////////////////////////////////////////
+
 void afsk_init(Afsk *afsk, int _adcPin, int _dacPin) {
 	// Allocate memory for struct
 	memset(afsk, 0, sizeof(*afsk));
diff --git a/Modem/afsk.h b/Modem/afsk.h
index d6fa4f9..2f8acd0 100644
--- a/Modem/afsk.h
+++ b/Modem/afsk.h
@@ -84,10 +84,6 @@ void afsk_adc_isr(Afsk *af, int8_t sample);
 uint8_t afsk_dac_isr(Afsk *af);
 void afsk_init(Afsk *af, int adc_ch, int dac_ch);
 
-
-#define AFSK_BUTTERWORTH  0
-#define AFSK_CHEBYSHEV    1
-
 int afsk_testSetup(void);
 int afsk_testRun(void);
 int afsk_testTearDown(void);
diff --git a/Modem/config.h b/Modem/config.h
index 3dc7801..7746c4c 100644
--- a/Modem/config.h
+++ b/Modem/config.h
@@ -2,13 +2,11 @@
 #ifndef FSK_CFG
 #define FSK_CFG
 
-#define CONFIG_AFSK_FILTER AFSK_CHEBYSHEV
 #define CONFIG_AFSK_RX_BUFLEN 64
 #define CONFIG_AFSK_TX_BUFLEN 64
 #define CONFIG_AFSK_DAC_SAMPLERATE 9600
 #define CONFIG_AFSK_RXTIMEOUT 0
 
-
 #define CONFIG_AFSK_PREAMBLE_LEN 300UL
 #define CONFIG_AFSK_TRAILER_LEN 50UL
 
diff --git a/Modem/main.c b/Modem/main.c
index ce1c5be..4dd73d6 100644
--- a/Modem/main.c
+++ b/Modem/main.c
@@ -15,6 +15,24 @@ static Serial ser;
 
 #define ADC_CH 0
 
+
+///////////////////////// AX25 for testing ////
+#include <net/ax25.h>
+static AX25Ctx ax25;
+static AX25Call path[] = AX25_PATH(AX25_CALL("apzbrt", 0), AX25_CALL("nocall", 0), AX25_CALL("wide1", 1), AX25_CALL("wide2", 2));
+#define APRS_MSG    ">Test BeRTOS APRS http://www.bertos.org"
+static void message_callback(struct AX25Msg *msg)
+{
+	kfile_printf(&ser.fd, "\n\nSRC[%.6s-%d], DST[%.6s-%d]\r\n", msg->src.call, msg->src.ssid, msg->dst.call, msg->dst.ssid);
+
+	for (int i = 0; i < msg->rpt_cnt; i++)
+		kfile_printf(&ser.fd, "via: [%.6s-%d]\r\n", msg->rpt_lst[i].call, msg->rpt_lst[i].ssid);
+
+	kfile_printf(&ser.fd, "DATA: %.*s\r\n", msg->len, msg->info);
+}
+//////////////////////////////////////////////
+
+
 static void init(void)
 {
 	IRQ_ENABLE;
@@ -22,6 +40,7 @@ static void init(void)
 	timer_init();
 
 	afsk_init(&afsk, ADC_CH, 0);
+	ax25_init(&ax25, &afsk.fd, message_callback);
 	
 	ser_init(&ser, SER_UART0);
 	ser_setbaudrate(&ser, 115200);
@@ -30,14 +49,23 @@ static void init(void)
 int main(void)
 {
 	init();
-	//ticks_t start = timer_clock();
+	ticks_t start = timer_clock();
 
 	while (1)
 	{
+		// Raw output, no protocol
 		if (!fifo_isempty(&afsk.rxFifo)) {
 			char c = fifo_pop(&afsk.rxFifo);
 			kprintf("%c", c);
 		}
+
+		// Use AX.25 to send test data
+		if (timer_clock() - start > ms_to_ticks(15000L))
+		{
+			kputs("Test TX\n");
+			start = timer_clock();
+			ax25_sendVia(&ax25, path, countof(path), APRS_MSG, sizeof(APRS_MSG));
+		}
 	}
 	return 0;
 }
\ No newline at end of file
diff --git a/buildrev.h b/buildrev.h
index a95a4f8..e6b5532 100644
--- a/buildrev.h
+++ b/buildrev.h
@@ -1,2 +1,2 @@
-#define VERS_BUILD 96
+#define VERS_BUILD 108
 #define VERS_HOST  "vixen"