diff --git a/.gitignore b/.gitignore
index 1869019..8f5d990 100755
--- a/.gitignore
+++ b/.gitignore
@@ -17,3 +17,4 @@ testkit
 flashdefault
 flashtestkit
 flashcurrent
+vendor
diff --git a/Makefile b/Makefile
index b67b2d2..8db9e3c 100755
--- a/Makefile
+++ b/Makefile
@@ -6,9 +6,9 @@ TARGET = images/OpenModem
 OPT = s
 FORMAT = ihex
 
-SRC = main.c hardware/Serial.c hardware/AFSK.c hardware/VREF.c hardware/LED.c hardware/UserIO.c hardware/SD.c hardware/sdcard/diskio.c hardware/sdcard/ff.c hardware/sdcard/ffsystem.c hardware/sdcard/ffunicode.c hardware/Bluetooth.c hardware/GPS.c hardware/Crypto.c hardware/crypto/AES.c util/CRC-CCIT.c protocol/AX25.c protocol/KISS.c
-
+SRC = main.c hardware/Serial.c hardware/AFSK.c hardware/VREF.c hardware/LED.c hardware/UserIO.c hardware/SD.c hardware/sdcard/diskio.c hardware/sdcard/ff.c hardware/sdcard/ffsystem.c hardware/sdcard/ffunicode.c hardware/Bluetooth.c hardware/GPS.c hardware/Crypto.c hardware/crypto/AES.c hardware/crypto/HMAC_MD5.c hardware/crypto/MD5.c hardware/crypto/MD5_sbox.c util/CRC-CCIT.c protocol/AX25.c protocol/KISS.c
 
+# TODO: Try hardware/crypto/MD5_asm.S
 # List Assembler source files here.
 ASRC = 
 
diff --git a/device.h b/device.h
index e71e1fb..f210287 100755
--- a/device.h
+++ b/device.h
@@ -37,7 +37,9 @@
 #define CONFIG_CSMA_P 255
 
 #define AX25_MIN_FRAME_LEN 1
-#define AX25_MAX_FRAME_LEN 600
+#define AX25_MAX_FRAME_LEN 611
+// TODO: increase back to 576
+#define AX25_MAX_PAYLOAD   576
 
 // Packet settings
 #define CONFIG_PASSALL false
diff --git a/hardware/Crypto.c b/hardware/Crypto.c
index 05b32b6..a53be4b 100644
--- a/hardware/Crypto.c
+++ b/hardware/Crypto.c
@@ -25,23 +25,26 @@ void crypto_init(void) {
 	}
 
 	if (encryption_enabled) {
-		// TODO: Set flags for crypto enabled
-
-		// TODO: Remove
-		// for (uint8_t i = 0; i < 130; i++) {
-		// 	crypto_test();
-		// }
+		LED_indicate_enabled_crypto();
 	} else {
 		LED_indicate_error_crypto();
 	}
 }
 
-void crypto_prepare(uint8_t key[CRYPTO_KEY_SIZE], uint8_t initialization_vector[CRYPTO_KEY_SIZE]) {
+void crypto_generate_hmac(uint8_t *data, size_t length) {
+	hmac_md5(crypto_work_block, active_key, CRYPTO_KEY_SIZE_BITS, data, length*8);
+}
+
+bool crypto_enabled(void) {
+	return encryption_enabled;
+}
+
+void crypto_prepare(void) {
   // Initialise the context with the key
-  aes_128_init(&context, key);
+  aes_128_init(&context, active_key);
 
   // Copy the IV into the current vector array
-  memcpy(current_vector, initialization_vector, CRYPTO_KEY_SIZE);
+  memcpy(current_vector, active_iv, CRYPTO_KEY_SIZE);
 }
 
 void crypto_encrypt_block(uint8_t block[CRYPTO_KEY_SIZE]) {
@@ -82,7 +85,6 @@ bool load_entropy_index(void) {
 	if (sd_mounted()) {
 		crypto_fr = f_open(&crypto_fp, PATH_ENTROPY_INDEX, FA_READ);
 		if (crypto_fr == FR_NO_FILE) {
-			//printf("Entropy index file does not exist\r\n");
 			f_close(&crypto_fp);
 			crypto_fr = f_open(&crypto_fp, PATH_ENTROPY_INDEX, FA_CREATE_NEW | FA_WRITE);
 
@@ -99,27 +101,20 @@ bool load_entropy_index(void) {
 				} else {
 					//printf("Could not write index to index file\r\n");
 				}
-			} else {
-				//printf("Could not create index file\r\n");
 			}
 
 			crypto_fr = f_open(&crypto_fp, PATH_ENTROPY_INDEX, FA_READ);
 		}
 
 		if (crypto_fr == FR_OK) {
-			//printf("Opened entropy index file\r\n");
 			UINT read = 0;
 			crypto_fr = f_read(&crypto_fp, crypto_fb, sizeof(entropy_index), &read);
 			f_close(&crypto_fp);
 			if (crypto_fr == FR_OK && read == sizeof(entropy_index)) {
 				memcpy(&entropy_index, crypto_fb, sizeof(entropy_index));
-				//printf("Entropy index is now: %lX\r\n", entropy_index);
 				return true;
 			}
-		} else {
-			//printf("Error opening entropy index file\r\n");
 		}
-
 	}
 
 	f_close(&crypto_fp);
@@ -148,15 +143,8 @@ bool load_entropy(void) {
 			crypto_fr = f_open(&crypto_fp, PATH_ENTROPY_SOURCE, FA_READ);
 			if (crypto_fr == FR_OK) {
 				uint32_t fsize = f_size(&crypto_fp);
-				//uint32_t fpoint = crypto_fp.fptr;
-
-				//printf("Opened entropy file\r\n\tSize is %lu\r\n\tPointer is at %lX \r\n\tSeeking to index: %lX\r\n", fsize, fpoint, entropy_index);
-
+				
 				crypto_fr = f_lseek(&crypto_fp, entropy_index);
-
-				//fpoint = crypto_fp.fptr;
-				//printf("After seek, pointer is now at %lX\r\n", fpoint);
-
 				if (crypto_fr == FR_OK && crypto_fp.fptr < fsize-sizeof(entropy)) {
 					UINT read = 0;
 					crypto_fr = f_read(&crypto_fp, crypto_fb, sizeof(entropy), &read);
@@ -164,18 +152,14 @@ bool load_entropy(void) {
 
 					if (crypto_fr == FR_OK) {
 						memcpy(&entropy, crypto_fb, sizeof(entropy));
-						//printf("Read entropy from SD: %lX\r\n", entropy);
 						srandom(entropy);
 						entropy_loaded = true;
 						ivs_generated = 0;
 						return true;
-					} else {
-						//printf("Could not read entropy data from SD\r\n");
 					}
 
 				} else {
 					f_close(&crypto_fp);
-					//printf("Could not seek in index file, entropy exhausted\r\n");
 					LED_indicate_error_crypto();
 				}
 			}
@@ -190,46 +174,30 @@ bool load_key(void) {
 	if (sd_mounted()) {
 		crypto_fr = f_open(&crypto_fp, PATH_AES_128_KEY, FA_READ);
 		if (crypto_fr == FR_OK) {
-			//printf("File open\r\n");
 			UINT read = 0;
 			crypto_fr = f_read(&crypto_fp, crypto_fb, CRYPTO_KEY_SIZE, &read);
 			f_close(&crypto_fp);
 
 			if (crypto_fr == FR_OK && read == CRYPTO_KEY_SIZE) {
-				//printf("Loaded AES-128 Key: ");
 				for (uint8_t i = 0; i < 16; i++) {
 					active_key[i] = crypto_fb[i];
-					//printf("%X ", crypto_fb[i]);
 				}
-				//printf("\r\n");	
+
 				return true;
-			} else {
-				//printf("Error %d reading file, read %d bytes.\r\n", crypto_fr, read);
 			}
-		} else {
-			//printf("Could not open file\r\n");
 		}
-	} else {
-		//printf("SD not mounted\r\n");
 	}
 
 	return false;
 }
 
-bool generate_iv(void) {
+bool crypto_generate_iv(void) {
 	if (entropy_loaded) {
 		for (uint8_t i = 0; i < 16; i++) {
 			active_iv[i] = (uint8_t)random();
 		}
 		ivs_generated++;
 
-		// TODO: remove
-		/*printf("Generated IV: ");
-		for (uint8_t i = 0; i < 16; i++) {
-			printf("%X ", active_iv[i]);
-		}
-		printf("\r\n");*/
-
 		if (ivs_generated >= MAX_IVS_PER_ENTROPY_BLOCK) {
 			load_entropy();
 		}
@@ -240,24 +208,12 @@ bool generate_iv(void) {
 	}
 }
 
-// TODO: Remove this
-void crypto_test(void) {
-	generate_iv();
-
-	uint8_t work_block[16];
-	memset(work_block, 0x70, 16);
-	work_block[15] = 0x00;
-
-	//printf("Work block plaintext:  ===%s===\r\n", work_block);
-	
-	crypto_prepare(active_key, active_iv);
-	crypto_encrypt_block(work_block);
-	//printf("Work block ciphertext: ===%s===\r\n", work_block);
-
-	crypto_prepare(active_key, active_iv);
-	crypto_decrypt_block(work_block);
-	printf("Work block plaintext:  ===%s===\r\n", work_block);
+uint8_t *crypto_get_iv(void) {
+	return active_iv;
+}
 
+void crypto_set_iv_from_workblock(void) {
+	memcpy(active_iv, crypto_work_block, CRYPTO_KEY_SIZE);
 }
 
 // TODO: test entropy exhaustion
\ No newline at end of file
diff --git a/hardware/Crypto.h b/hardware/Crypto.h
index 703a03a..6e56394 100644
--- a/hardware/Crypto.h
+++ b/hardware/Crypto.h
@@ -5,7 +5,8 @@
 #include <stdlib.h>
 #include <stdbool.h>
 #include <stdio.h>
-#include "hardware/crypto/aes.h"
+#include "hardware/crypto/AES.h"
+#include "hardware/crypto/HMAC_MD5.h"
 #include "hardware/LED.h"
 #include "hardware/SD.h"
 
@@ -15,9 +16,23 @@
 
 #define CRYPTO_KEY_SIZE_BITS 128
 #define CRYPTO_KEY_SIZE (CRYPTO_KEY_SIZE_BITS/8)
+#define CRYPTO_HMAC_SIZE_BITS 128
+#define CRYPTO_HMAC_SIZE (CRYPTO_HMAC_SIZE_BITS/8)
 #define MAX_IVS_PER_ENTROPY_BLOCK 128
 
+uint8_t crypto_work_block[CRYPTO_KEY_SIZE];
+
 void crypto_init(void);
+bool crypto_enabled(void);
+bool crypto_generate_iv(void);
+uint8_t* crypto_get_iv(void);
+void crypto_set_iv_from_workblock(void);
+void crypto_generate_hmac(uint8_t *data, size_t length);
+void crypto_prepare(void);
+
+void crypto_encrypt_block(uint8_t block[CRYPTO_KEY_SIZE]);
+void crypto_decrypt_block(uint8_t block[CRYPTO_KEY_SIZE]);
+
 void crypto_test(void);
 
 bool load_key(void);
diff --git a/hardware/LED.c b/hardware/LED.c
index f573a5a..50a0341 100644
--- a/hardware/LED.c
+++ b/hardware/LED.c
@@ -1,6 +1,8 @@
 #include "LED.h"
 #include "util/time.h"
 
+bool LED_softblock_enabled = false;
+
 uint8_t ledIntensity = CONFIG_LED_INTENSITY;
 ticks_t led_status_ticks_top = 0;
 ticks_t led_status_ticks = 0;
@@ -28,6 +30,13 @@ void LED_init(void) {
     OCR0A = ledIntensity;
 }
 
+void LED_softblock_on(void) {
+    LED_softblock_enabled = true;
+}
+
+void LED_softblock_off(void) {
+    LED_softblock_enabled = false;
+}
 
 void LED_setIntensity(uint8_t value) {
 	ledIntensity = value;
@@ -35,11 +44,22 @@ void LED_setIntensity(uint8_t value) {
 }
 
 void LED_COM_ON(void) {
+    if (!LED_softblock_enabled) {
+        LED_PORT |= _BV(4);
+        com_led_timeout = timer_clock() + ms_to_ticks(CONFIG_COM_LED_TIMEOUT_MS);
+    }
+}
+
+void LED_COM_OFF(void) {
+    if (!LED_softblock_enabled) LED_PORT &= ~_BV(4); 
+}
+
+void LED_F_COM_ON(void) {
     LED_PORT |= _BV(4);
     com_led_timeout = timer_clock() + ms_to_ticks(CONFIG_COM_LED_TIMEOUT_MS);
 }
 
-void LED_COM_OFF(void) {
+void LED_F_COM_OFF(void) {
     LED_PORT &= ~_BV(4); 
 }
 
@@ -54,6 +74,37 @@ void update_led_status(void) {
     }
 }
 
+#define LED_DELAY_E_C_1 200
+#define LED_DELAY_E_C_2 50
+void LED_indicate_enabled_crypto(void) {
+    LED_softblock_on();
+
+    LED_F_STATUS_OFF();
+    LED_F_COM_OFF();
+    LED_F_TX_OFF();
+    LED_F_RX_OFF();
+    delay_ms(LED_DELAY_E_C_1);
+
+    for (uint8_t i = 0; i < 2; i++) {
+        LED_F_STATUS_ON();
+        delay_ms(LED_DELAY_E_C_2);
+        LED_F_STATUS_OFF();
+        LED_F_COM_ON();
+        delay_ms(LED_DELAY_E_C_2);
+        LED_F_COM_OFF();
+        LED_F_RX_ON();
+        delay_ms(LED_DELAY_E_C_2);
+        LED_F_RX_OFF();
+        LED_F_TX_ON();
+        delay_ms(LED_DELAY_E_C_2);
+        LED_F_TX_OFF();
+        delay_ms(LED_DELAY_E_C_2);
+    }
+
+    LED_F_STATUS_ON();
+    LED_softblock_off();
+}
+
 void LED_indicate_error_crypto(void) {
     while (true) {
         LED_COM_ON();
diff --git a/hardware/LED.h b/hardware/LED.h
index 3b5d7b6..147812e 100644
--- a/hardware/LED.h
+++ b/hardware/LED.h
@@ -2,18 +2,29 @@
 #define LED_H
 
 #include <avr/io.h>
+#include <stdbool.h>
 #include "device.h"
 
+extern bool LED_softblock_enabled;
+
 void LED_init(void);
 void LED_setIntensity(uint8_t value);
 
-#define LED_STATUS_ON()   do { LED_PORT |= _BV(2); } while (0)
-#define LED_STATUS_OFF()  do { LED_PORT &= ~_BV(2); } while (0)
-#define LED_STATUS_TOGGLE()  do { LED_PORT ^= _BV(2); } while (0)
-#define LED_TX_ON()   do { LED_PORT |= _BV(1); } while (0)
-#define LED_TX_OFF()  do { LED_PORT &= ~_BV(1); } while (0)
-#define LED_RX_ON()   do { LED_PORT |= _BV(0); } while (0)
-#define LED_RX_OFF()  do { LED_PORT &= ~_BV(0); } while (0)
+#define LED_STATUS_ON()   do { if (!LED_softblock_enabled) LED_PORT |= _BV(2); } while (0)
+#define LED_STATUS_OFF()  do { if (!LED_softblock_enabled) LED_PORT &= ~_BV(2); } while (0)
+#define LED_STATUS_TOGGLE()  do { if (!LED_softblock_enabled) LED_PORT ^= _BV(2); } while (0)
+#define LED_TX_ON()   do { if (!LED_softblock_enabled) LED_PORT |= _BV(1); } while (0)
+#define LED_TX_OFF()  do { if (!LED_softblock_enabled) LED_PORT &= ~_BV(1); } while (0)
+#define LED_RX_ON()   do { if (!LED_softblock_enabled) LED_PORT |= _BV(0); } while (0)
+#define LED_RX_OFF()  do { if (!LED_softblock_enabled) LED_PORT &= ~_BV(0); } while (0)
+
+#define LED_F_STATUS_ON()   do { LED_PORT |= _BV(2); } while (0)
+#define LED_F_STATUS_OFF()  do { LED_PORT &= ~_BV(2); } while (0)
+#define LED_F_STATUS_TOGGLE()  do { LED_PORT ^= _BV(2); } while (0)
+#define LED_F_TX_ON()   do { LED_PORT |= _BV(1); } while (0)
+#define LED_F_TX_OFF()  do { LED_PORT &= ~_BV(1); } while (0)
+#define LED_F_RX_ON()   do { LED_PORT |= _BV(0); } while (0)
+#define LED_F_RX_OFF()  do { LED_PORT &= ~_BV(0); } while (0)
 
 void LED_COM_ON(void);
 void LED_COM_OFF(void);
@@ -21,4 +32,6 @@ void update_led_status(void);
 
 void LED_indicate_error_crypto(void);
 
+void LED_indicate_enabled_crypto(void);
+
 #endif
\ No newline at end of file
diff --git a/hardware/SD.c b/hardware/SD.c
index ae53222..a578dd4 100644
--- a/hardware/SD.c
+++ b/hardware/SD.c
@@ -30,6 +30,9 @@ void sd_automounted_hook(void) {
 
 void sd_autounmounted_hook(void) {
     sd_statuschange_indication(0);
+    if (crypto_enabled()) {
+        LED_indicate_error_crypto();
+    }
 }
 
 void sd_jobs(void) {
diff --git a/hardware/crypto/HMAC_MD5.c b/hardware/crypto/HMAC_MD5.c
new file mode 100644
index 0000000..548a584
--- /dev/null
+++ b/hardware/crypto/HMAC_MD5.c
@@ -0,0 +1,129 @@
+/* hmac-md5.c */
+/*
+    This file is part of the AVR-Crypto-Lib.
+    Copyright (C) 2006-2015 Daniel Otte (bg@nerilex.org)
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+/**
+ * 
+ * implementation of HMAC as described in RFC2104
+ * Author:      Daniel Otte
+ * email:       bg@nerilex.org
+ * License:     GPLv3 or later
+ **/
+
+/* 
+ * hmac = hash ( k^opad , hash( k^ipad  , msg))
+ */
+
+#include <stdint.h>
+#include <string.h>
+#include "MD5.h"
+#include "HMAC_MD5.h"
+
+#define IPAD 0x36
+#define OPAD 0x5C
+
+#ifndef HMAC_SHORTONLY
+
+void hmac_md5_init(hmac_md5_ctx_t *s, void *key, uint16_t keylength_b){
+	uint8_t buffer[MD5_BLOCK_BYTES];
+	uint8_t i;
+	
+	memset(buffer, 0, MD5_BLOCK_BYTES);
+	if (keylength_b > MD5_BLOCK_BITS){
+		md5((void*)buffer, key, keylength_b);
+	} else {
+		memcpy(buffer, key, (keylength_b+7)/8);
+	}
+	
+	for (i=0; i<MD5_BLOCK_BYTES; ++i){
+		buffer[i] ^= IPAD;
+	}
+	md5_init(&(s->a));
+	md5_nextBlock(&(s->a), buffer);
+	
+	for (i=0; i<MD5_BLOCK_BYTES; ++i){
+		buffer[i] ^= IPAD^OPAD;
+	}
+	md5_init(&(s->b));
+	md5_nextBlock(&(s->b), buffer);
+	
+#if defined SECURE_WIPE_BUFFER
+	memset(buffer, 0, MD5_BLOCK_BYTES);
+#endif
+}
+
+void hmac_md5_nextBlock(hmac_md5_ctx_t *s, const void *block){
+	md5_nextBlock(&(s->a), block);
+}
+
+void hmac_md5_lastBlock(hmac_md5_ctx_t *s, const void *block, uint16_t length_b){
+	md5_lastBlock(&(s->a), block, length_b);
+}
+
+void hmac_md5_final(void *dest, hmac_md5_ctx_t *s){
+	md5_ctx2hash((md5_hash_t*)dest, &(s->a));
+	md5_lastBlock(&(s->b), dest, MD5_HASH_BITS);
+	md5_ctx2hash((md5_hash_t*)dest, &(s->b));
+}
+
+#endif
+
+/*
+void hmac_md5_nextBlock()
+void hmac_md5_lastBlock()
+*/
+
+/*
+ * keylength in bits!
+ * message length in bits!
+ */
+void hmac_md5(void *dest, void *key, uint16_t keylength_b, void *msg, uint32_t msglength_b){ /* a one-shot*/
+	md5_ctx_t s;
+	uint8_t i;
+	uint8_t buffer[MD5_BLOCK_BYTES];
+	
+	memset(buffer, 0, MD5_BLOCK_BYTES);
+	
+	/* if key is larger than a block we have to hash it*/
+	if (keylength_b > MD5_BLOCK_BITS){
+		md5((void*)buffer, key, keylength_b);
+	} else {
+		memcpy(buffer, key, (keylength_b+7)/8);
+	}
+	
+	for (i=0; i<MD5_BLOCK_BYTES; ++i){
+		buffer[i] ^= IPAD;
+	}
+	md5_init(&s);
+	md5_nextBlock(&s, buffer);
+	while (msglength_b >= MD5_BLOCK_BITS){
+		md5_nextBlock(&s, msg);
+		msg = (uint8_t*)msg + MD5_BLOCK_BYTES;
+		msglength_b -=  MD5_BLOCK_BITS;
+	}
+	md5_lastBlock(&s, msg, msglength_b);
+	/* since buffer still contains key xor ipad we can do ... */
+	for (i=0; i<MD5_BLOCK_BYTES; ++i){
+		buffer[i] ^= IPAD ^ OPAD;
+	}
+	md5_ctx2hash(dest, &s); /* save inner hash temporary to dest */
+	md5_init(&s);
+	md5_nextBlock(&s, buffer);
+	md5_lastBlock(&s, dest, MD5_HASH_BITS);
+	md5_ctx2hash(dest, &s);
+}
+
diff --git a/hardware/crypto/HMAC_MD5.h b/hardware/crypto/HMAC_MD5.h
new file mode 100644
index 0000000..b4f84ac
--- /dev/null
+++ b/hardware/crypto/HMAC_MD5.h
@@ -0,0 +1,42 @@
+/* hmac-md5.h */
+/*
+    This file is part of the AVR-Crypto-Lib.
+    Copyright (C) 2006-2015 Daniel Otte (bg@nerilex.org)
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+#ifndef HMACMD5_H_
+#define HMACMD5_H_
+
+#include "MD5.h"
+
+#define HMAC_MD5_BITS        MD5_HASH_BITS
+#define HMAC_MD5_BYTES       MD5_HASH_BYTES
+#define HMAC_MD5_BLOCK_BITS  MD5_BLOCK_BITS
+#define HMAC_MD5_BLOCK_BYTES MD5_BLOCK_BYTES
+
+typedef struct{
+	md5_ctx_t a,b;
+} hmac_md5_ctx_t;
+
+
+void hmac_md5_init(hmac_md5_ctx_t *s, void *key, uint16_t keylength_b);
+void hmac_md5_nextBlock(hmac_md5_ctx_t *s, const void *block);
+void hmac_md5_lastBlock(hmac_md5_ctx_t *s, const void *block, uint16_t length_b);
+void hmac_md5_final(void *dest, hmac_md5_ctx_t *s);
+
+void hmac_md5(void *dest, void *key, uint16_t keylength_b, void *msg, uint32_t msglength_b);
+
+
+#endif /*HMACMD5_H_*/
diff --git a/hardware/crypto/MD5.c b/hardware/crypto/MD5.c
new file mode 100644
index 0000000..c616b1d
--- /dev/null
+++ b/hardware/crypto/MD5.c
@@ -0,0 +1,187 @@
+/* md5.c */
+/*
+    This file is part of the AVR-Crypto-Lib.
+    Copyright (C) 2006-2015 Daniel Otte (bg@nerilex.org)
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+/* 
+ * \file	md5.c
+ * \author	Daniel Otte
+ * \date 	2006-07-31
+ * \license GPLv3 or later
+ * \brief   Implementation of the MD5 hash algorithm as described in RFC 1321
+ * 
+ */
+
+ #include "MD5.h"
+ #include "MD5_sbox.h"
+ #include <stdint.h>
+ #include <string.h>
+ 
+ #undef DEBUG
+ 
+void md5_init(md5_ctx_t *s){
+	s->counter = 0;
+	s->a[0] = 0x67452301;
+	s->a[1] = 0xefcdab89;
+	s->a[2] = 0x98badcfe;
+	s->a[3] = 0x10325476;
+}
+
+static 
+uint32_t md5_F(uint32_t x, uint32_t y, uint32_t z){
+	return ((x&y)|((~x)&z));
+}
+
+static
+uint32_t md5_G(uint32_t x, uint32_t y, uint32_t z){
+	return ((x&z)|((~z)&y));
+}
+
+static
+uint32_t md5_H(uint32_t x, uint32_t y, uint32_t z){
+	return (x^y^z);
+}
+
+static
+uint32_t md5_I(uint32_t x, uint32_t y, uint32_t z){
+	return (y ^ (x | (~z)));
+}
+
+typedef uint32_t md5_func_t(uint32_t, uint32_t, uint32_t);
+
+#define ROTL32(x,n) (((x)<<(n)) | ((x)>>(32-(n))))  
+
+static
+void md5_core(uint32_t *a, void *block, uint8_t as, uint8_t s, uint8_t i, uint8_t fi){
+	uint32_t t;
+	md5_func_t *funcs[]={md5_F, md5_G, md5_H, md5_I};
+	as &= 0x3;
+	/* a = b + ((a + F(b,c,d) + X[k] + T[i]) <<< s). */
+#ifdef DEBUG
+	char funcc[]={'*', '-', '+', '~'};
+	cli_putstr("\r\n DBG: md5_core [");
+	cli_putc(funcc[fi]);
+	cli_hexdump(&as, 1); cli_putc(' ');
+	cli_hexdump(&k, 1); cli_putc(' ');
+	cli_hexdump(&s, 1); cli_putc(' ');
+	cli_hexdump(&i, 1); cli_putc(']');
+#endif	
+	t = a[as] + funcs[fi](a[(as+1)&3], a[(as+2)&3], a[(as+3)&3]) 
+	    + *((uint32_t*)block) + pgm_read_dword(md5_T+i) ;
+	a[as]=a[(as+1)&3] + ROTL32(t, s);
+}
+
+void md5_nextBlock(md5_ctx_t *state, const void *block){
+	uint32_t	a[4];
+	uint8_t		m,n,i=0;
+	/* this requires other mixed sboxes */
+#ifdef DEBUG
+	cli_putstr("\r\n DBG: md5_nextBlock: block:\r\n");
+	cli_hexdump(block, 16);	cli_putstr("\r\n");
+	cli_hexdump(block+16, 16);	cli_putstr("\r\n");
+	cli_hexdump(block+32, 16);	cli_putstr("\r\n");
+	cli_hexdump(block+48, 16);	cli_putstr("\r\n");
+#endif	
+	
+	a[0]=state->a[0];
+	a[1]=state->a[1];
+	a[2]=state->a[2];
+	a[3]=state->a[3];
+	
+	/* round 1 */
+	uint8_t s1t[]={7,12,17,22}; // 1,-1   1,4   2,-1   3,-2
+	for(m=0;m<4;++m){
+		for(n=0;n<4;++n){
+			md5_core(a, &(((uint32_t*)block)[m*4+n]), 4-n, s1t[n],i++,0);
+		}
+	}
+	/* round 2 */
+	uint8_t s2t[]={5,9,14,20}; // 1,-3   1,1   2,-2   2,4
+	for(m=0;m<4;++m){
+		for(n=0;n<4;++n){
+			md5_core(a, &(((uint32_t*)block)[(1+m*4+n*5)&0xf]), 4-n, s2t[n],i++,1);
+		}
+	}
+	/* round 3 */
+	uint8_t s3t[]={4,11,16,23}; // 0,4   1,3   2,0   3,-1
+	for(m=0;m<4;++m){
+		for(n=0;n<4;++n){
+			md5_core(a, &(((uint32_t*)block)[(5-m*4+n*3)&0xf]), 4-n, s3t[n],i++,2);
+		}
+	}
+	/* round 4 */
+	uint8_t s4t[]={6,10,15,21}; // 1,-2   1,2   2,-1   3,-3
+	for(m=0;m<4;++m){
+		for(n=0;n<4;++n){
+			md5_core(a, &(((uint32_t*)block)[(0-m*4+n*7)&0xf]), 4-n, s4t[n],i++,3);
+		}
+	}
+	state->a[0] += a[0];
+	state->a[1] += a[1];
+	state->a[2] += a[2];
+	state->a[3] += a[3];
+	state->counter++;
+}
+
+void md5_lastBlock(md5_ctx_t *state, const void *block, uint16_t length_b){
+	uint16_t l;
+	union {
+		uint8_t   v8[64];
+		uint64_t v64[ 8];
+	} buffer;
+	while (length_b >= 512){
+		md5_nextBlock(state, block);
+		length_b -= 512;
+		block = ((uint8_t*)block) + 512/8;
+	}
+	memset(buffer.v8, 0, 64);
+	memcpy(buffer.v8, block, length_b/8);
+	/* insert padding one */
+	l=length_b/8;
+	if(length_b%8){
+		uint8_t t;
+		t = ((uint8_t*)block)[l];
+		t |= (0x80>>(length_b%8));
+		buffer.v8[l]=t;
+	}else{
+		buffer.v8[l]=0x80;
+	}
+	/* insert length value */
+	if(l+sizeof(uint64_t) >= 512/8){
+		md5_nextBlock(state, buffer.v8);
+		state->counter--;
+		memset(buffer.v8, 0, 64-8);
+	}
+	buffer.v64[7] = (state->counter * 512) + length_b;
+	md5_nextBlock(state, buffer.v8);
+}
+
+void md5_ctx2hash(md5_hash_t *dest, const md5_ctx_t *state){
+	memcpy(dest, state->a, MD5_HASH_BYTES);
+}
+
+void md5(md5_hash_t *dest, const void *msg, uint32_t length_b){
+	md5_ctx_t ctx;
+	md5_init(&ctx);
+	while(length_b>=MD5_BLOCK_BITS){
+		md5_nextBlock(&ctx, msg);
+		msg = (uint8_t*)msg + MD5_BLOCK_BYTES;
+		length_b -= MD5_BLOCK_BITS;
+	}
+	md5_lastBlock(&ctx, msg, length_b);
+	md5_ctx2hash(dest, &ctx);
+}
+
diff --git a/hardware/crypto/MD5.h b/hardware/crypto/MD5.h
new file mode 100644
index 0000000..da44ad0
--- /dev/null
+++ b/hardware/crypto/MD5.h
@@ -0,0 +1,55 @@
+/* md5.h */
+/*
+    This file is part of the AVR-Crypto-Lib.
+    Copyright (C) 2006-2015 Daniel Otte (bg@nerilex.org)
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+/* 
+ * File:	md5.h
+ * Author:	Daniel Otte
+ * Date: 	31.07.2006
+ * License: GPL
+ * Description: Implementation of the MD5 hash algorithm as described in RFC 1321
+ * 
+ */
+
+
+#ifndef MD5_H_
+#define MD5_H_
+
+#include <stdint.h>
+
+
+#define MD5_HASH_BITS  128
+#define MD5_HASH_BYTES (MD5_HASH_BITS/8)
+#define MD5_BLOCK_BITS 512
+#define MD5_BLOCK_BYTES (MD5_BLOCK_BITS/8)
+
+
+typedef struct md5_ctx_st {
+	uint32_t a[4];
+	uint32_t counter;
+} md5_ctx_t;
+
+typedef uint8_t md5_hash_t[MD5_HASH_BYTES];
+
+ 
+void md5_init(md5_ctx_t *s);
+void md5_nextBlock(md5_ctx_t *state, const void *block);
+void md5_lastBlock(md5_ctx_t *state, const void *block, uint16_t length);
+void md5_ctx2hash(md5_hash_t *dest, const md5_ctx_t *state);
+void md5(md5_hash_t *dest, const void *msg, uint32_t length_b);
+
+#endif /*MD5_H_*/
diff --git a/hardware/crypto/MD5_sbox.c b/hardware/crypto/MD5_sbox.c
new file mode 100644
index 0000000..8efea01
--- /dev/null
+++ b/hardware/crypto/MD5_sbox.c
@@ -0,0 +1,36 @@
+/* md5_sbox.c */
+/*
+    This file is part of the AVR-Crypto-Lib.
+    Copyright (C) 2006-2015 Daniel Otte (bg@nerilex.org)
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+#include <stdint.h>
+#include <avr/pgmspace.h>
+
+const uint32_t md5_T[] PROGMEM = {
+	0xd76aa478, 0xe8c7b756, 0x242070db, 0xc1bdceee, 0xf57c0faf, 
+	0x4787c62a, 0xa8304613, 0xfd469501, 0x698098d8, 0x8b44f7af, 
+	0xffff5bb1, 0x895cd7be, 0x6b901122, 0xfd987193, 0xa679438e, 
+	0x49b40821, 0xf61e2562, 0xc040b340, 0x265e5a51, 0xe9b6c7aa, 
+	0xd62f105d, 0x02441453, 0xd8a1e681, 0xe7d3fbc8, 0x21e1cde6, 
+	0xc33707d6, 0xf4d50d87, 0x455a14ed, 0xa9e3e905, 0xfcefa3f8, 
+	0x676f02d9, 0x8d2a4c8a, 0xfffa3942, 0x8771f681, 0x6d9d6122, 
+	0xfde5380c, 0xa4beea44, 0x4bdecfa9, 0xf6bb4b60, 0xbebfbc70, 
+	0x289b7ec6, 0xeaa127fa, 0xd4ef3085, 0x04881d05, 0xd9d4d039, 
+	0xe6db99e5, 0x1fa27cf8, 0xc4ac5665, 0xf4292244, 0x432aff97, 
+	0xab9423a7, 0xfc93a039, 0x655b59c3, 0x8f0ccc92, 0xffeff47d, 
+	0x85845dd1, 0x6fa87e4f, 0xfe2ce6e0, 0xa3014314, 0x4e0811a1, 
+	0xf7537e82, 0xbd3af235, 0x2ad7d2bb, 0xeb86d391 };
+
diff --git a/hardware/crypto/MD5_sbox.h b/hardware/crypto/MD5_sbox.h
new file mode 100644
index 0000000..9f305b6
--- /dev/null
+++ b/hardware/crypto/MD5_sbox.h
@@ -0,0 +1,27 @@
+/* md5_sbox.h */
+/*
+    This file is part of the AVR-Crypto-Lib.
+    Copyright (C) 2006-2015 Daniel Otte (bg@nerilex.org)
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+#ifndef MD5_SBOX_H_
+#define MD5_SBOX_H_
+
+#include <stdint.h>
+#include <avr/pgmspace.h>
+
+extern const uint32_t md5_T[];
+
+#endif /*MD5_SBOX_H_*/
diff --git a/protocol/KISS.c b/protocol/KISS.c
index 984203f..3f9bdb1 100755
--- a/protocol/KISS.c
+++ b/protocol/KISS.c
@@ -4,6 +4,7 @@
 #include "device.h"
 #include "hardware/Serial.h"
 #include "hardware/LED.h"
+#include "hardware/Crypto.h"
 #include "util/FIFO16.h"
 #include "util/time.h"
 #include "KISS.h"
@@ -65,21 +66,76 @@ void kiss_messageCallback(AX25Ctx *ctx) {
         decodes++;
         printf("%d\r\n", decodes);
     #else
-        fputc(FEND, &serial->uart0);
-        fputc(0x00, &serial->uart0);
-        for (unsigned i = 0; i < ctx->frame_len-2; i++) {
-            uint8_t b = ctx->buf[i];
-            if (b == FEND) {
-                fputc(FESC, &serial->uart0);
-                fputc(TFEND, &serial->uart0);
-            } else if (b == FESC) {
-                fputc(FESC, &serial->uart0);
-                fputc(TFESC, &serial->uart0);
-            } else {
-                fputc(b, &serial->uart0);
+        bool integrity_ok = false;
+        if (crypto_enabled()) {
+            size_t rxpos = 0;
+
+            // Get padding size
+            uint8_t padding = ctx->buf[rxpos++];
+            size_t data_length = ctx->frame_len - 2 - 1 - CRYPTO_HMAC_SIZE - CRYPTO_KEY_SIZE;
+            size_t hmac_offset = ctx->frame_len - 2 - CRYPTO_HMAC_SIZE;
+
+            // Get HMAC
+            uint8_t hmac[CRYPTO_HMAC_SIZE];
+            memset(hmac, 0x00, CRYPTO_HMAC_SIZE);
+            for (uint8_t i = 0; i < CRYPTO_HMAC_SIZE; i++) {
+                size_t pos = hmac_offset + i;
+                hmac[i] = ctx->buf[pos];
             }
+
+            // Calculate HMAC
+            crypto_generate_hmac(ctx->buf, ctx->frame_len-2-CRYPTO_HMAC_SIZE);
+            bool HMAC_ok = true;
+            for (uint8_t i = 0; i < CRYPTO_HMAC_SIZE; i++) {
+                if (hmac[i] != crypto_work_block[i]) {
+                    HMAC_ok = false;
+                }
+            }
+
+            if (HMAC_ok) {
+                // Get IV
+                for (uint8_t i = 0; i < CRYPTO_KEY_SIZE; i++) {
+                    crypto_work_block[i] = ctx->buf[rxpos++];
+                }
+
+                crypto_set_iv_from_workblock();
+                crypto_prepare();
+                uint8_t blocks = data_length / CRYPTO_KEY_SIZE;
+
+                size_t decrypted_pos = 0;
+                for (uint8_t block = 0; block < blocks; block++) {
+                    for (uint8_t i = 0; i < CRYPTO_KEY_SIZE; i++) {
+                        crypto_work_block[i] = ctx->buf[rxpos++];
+                    }
+                    crypto_decrypt_block(crypto_work_block);
+                    for (uint8_t i = 0; i < CRYPTO_KEY_SIZE; i++) {
+                        ctx->buf[decrypted_pos++] = crypto_work_block[i];
+                    }
+                }
+                ctx->frame_len = data_length - padding;
+                integrity_ok = true;
+            }
+        } else {
+            integrity_ok = true;
+        }
+
+        if (integrity_ok) {
+            fputc(FEND, &serial->uart0);
+            fputc(0x00, &serial->uart0);
+            for (unsigned i = 0; i < ctx->frame_len-2; i++) {
+                uint8_t b = ctx->buf[i];
+                if (b == FEND) {
+                    fputc(FESC, &serial->uart0);
+                    fputc(TFEND, &serial->uart0);
+                } else if (b == FESC) {
+                    fputc(FESC, &serial->uart0);
+                    fputc(TFESC, &serial->uart0);
+                } else {
+                    fputc(b, &serial->uart0);
+                }
+            }
+            fputc(FEND, &serial->uart0);
         }
-        fputc(FEND, &serial->uart0);
     #endif
 }
 
@@ -110,24 +166,6 @@ void kiss_csma(void) {
     }
 }
 
-// TODO: Remove this
-// void kiss_flushQueueDebug(void) {
-//     printf("Queue height %d\r\n", queue_height);
-//     for (size_t n = 0; n < queue_height; n++) {
-//         size_t start = fifo16_pop(&packet_starts);
-//         size_t length = fifo16_pop(&packet_lengths);
-
-//         printf("--- Packet %d, %d bytes ---\r\n", n+1, length);
-//         for (size_t i = 0; i < length; i++) {
-//             size_t pos = (start+i)%CONFIG_QUEUE_SIZE;
-//             printf("%02x", packet_queue[pos]);
-//         }
-//         printf("\r\n\r\n");
-//     }
-//     queue_height = 0;
-//     queued_bytes = 0;
-// }
-
 volatile bool queue_flushing = false;
 void kiss_flushQueue(void) {
     if (!queue_flushing) {
@@ -138,14 +176,74 @@ void kiss_flushQueue(void) {
             size_t start = fifo16_pop_locked(&packet_starts);
             size_t length = fifo16_pop_locked(&packet_lengths);
 
-            //kiss_poll();
-            for (size_t i = 0; i < length; i++) {
-                size_t pos = (start+i)%CONFIG_QUEUE_SIZE;
-                tx_buffer[i] = packet_queue[pos];
-            }
+            if (crypto_enabled()) {
+                uint8_t padding = CRYPTO_KEY_SIZE - (length % CRYPTO_KEY_SIZE);
+                if (padding == CRYPTO_KEY_SIZE) padding = 0;
 
-            ax25_sendRaw(ax25ctx, tx_buffer, length);
-            processed++;
+                uint8_t blocks = (length + padding) / CRYPTO_KEY_SIZE;
+                
+                if (crypto_generate_iv()) {
+                    crypto_prepare();
+
+                    size_t tx_pos = 0;
+                    tx_buffer[tx_pos++] = padding;
+
+                    uint8_t *iv = crypto_get_iv();
+                    for (uint8_t i = 0; i < CRYPTO_KEY_SIZE; i++) {
+                        tx_buffer[tx_pos++] = iv[i];
+                    }
+
+                    // Encrypt each block
+                    for (uint8_t i = 0; i < blocks; i++) {
+                        if (i < blocks-1 || padding == 0) {
+                            for (uint8_t j = 0; j < CRYPTO_KEY_SIZE; j++) {
+                                size_t pos = (start+j)%CONFIG_QUEUE_SIZE;
+                                crypto_work_block[j] = packet_queue[pos];
+                            }
+                            start += CRYPTO_KEY_SIZE;
+                        } else {
+                            for (uint8_t j = 0; j < CRYPTO_KEY_SIZE - padding; j++) {
+                                size_t pos = (start+j)%CONFIG_QUEUE_SIZE;
+                                crypto_work_block[j] = packet_queue[pos];
+                            }
+                            for (uint8_t j = 0; j < padding; j++) {
+                                crypto_work_block[j] = 0xFF;
+                            }
+                        }
+                        
+                        crypto_encrypt_block(crypto_work_block);
+
+                        for (uint8_t j = 0; j < CRYPTO_KEY_SIZE; j++) {
+                            tx_buffer[tx_pos++] = crypto_work_block[j];
+                        }
+                    }
+
+                    // Genereate MAC
+                    crypto_generate_hmac(tx_buffer, tx_pos);
+                    for (uint8_t i = 0; i < CRYPTO_HMAC_SIZE; i++) {
+                        tx_buffer[tx_pos++] = crypto_work_block[i];
+                    }
+
+                    // Check size and send
+                    if (tx_pos <= AX25_MAX_FRAME_LEN) {
+                        ax25_sendRaw(ax25ctx, tx_buffer, tx_pos);
+                        processed++;
+                    } else {
+                        processed++;
+                    }
+
+                } else {
+                    LED_indicate_error_crypto();
+                }
+            } else {
+                for (size_t i = 0; i < length; i++) {
+                    size_t pos = (start+i)%CONFIG_QUEUE_SIZE;
+                    tx_buffer[i] = packet_queue[pos];
+                }
+
+                ax25_sendRaw(ax25ctx, tx_buffer, length);
+                processed++;
+            }
         }
 
         if (processed < queue_height) {
@@ -181,7 +279,7 @@ void kiss_serialCallback(uint8_t sbyte) {
         IN_FRAME = true;
         command = CMD_UNKNOWN;
         frame_len = 0;
-    } else if (IN_FRAME && frame_len < AX25_MAX_FRAME_LEN) {
+    } else if (IN_FRAME && frame_len < AX25_MAX_PAYLOAD) {
         // Have a look at the command byte first
         if (frame_len == 0 && command == CMD_UNKNOWN) {
             // OpenModem supports only one HDLC port, so we
@@ -214,9 +312,6 @@ void kiss_serialCallback(uint8_t sbyte) {
             p = sbyte;
         } else if (command == CMD_FLUSHQUEUE) {
             kiss_flushQueue();
-        // TODO: Remove this
-        //} else if (command == CMD_FLUSHQUEUE_DEBUG) {
-        //    kiss_flushQueueDebug();
         } else if (command == CMD_LED_INTENSITY) {
             if (sbyte == FESC) {
                 ESCAPE = true;