Added crypto supportHEADmaster

Added TETRA cryptographic primitives (TEA1-3, TAA1 minus TA61).
If a keyfile is loaded (using -k flag), matching signalling frames
will be decrypted. No support for traffic or identity encryption
yet. Based on https://github.com/MidnightBlueLabs/TETRA_crypto

Change-Id: I0c0227cf5b747bd5032602390175b898173f6ae6

12 files changed, 1185 insertions, 11 deletions

diff --git a/src/Makefile b/src/Makefile
index b424ea4..8a4277e 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -16,7 +16,7 @@ libosmo-tetra-phy.a: phy/tetra_burst_sync.o phy/tetra_burst.o
 libosmo-tetra-mac.a: lower_mac/tetra_conv_enc.o lower_mac/tch_reordering.o tetra_tdma.o lower_mac/tetra_scramb.o lower_mac/tetra_rm3014.o lower_mac/tetra_interleave.o lower_mac/crc_simple.o tetra_common.o lower_mac/viterbi.o lower_mac/viterbi_cch.o lower_mac/viterbi_tch.o lower_mac/tetra_lower_mac.o tetra_upper_mac.o tetra_mac_pdu.o tetra_llc_pdu.o tetra_llc.o tetra_mle_pdu.o tetra_mle.o tetra_mm_pdu.o tetra_cmce_pdu.o tetra_sndcp_pdu.o tetra_gsmtap.o tuntap.o
 	$(AR) r $@ $^
 
-libosmo-tetra-crypto.a: crypto/tetra_crypto.o
+libosmo-tetra-crypto.a: crypto/tea1.o crypto/tea2.o crypto/tea3.o crypto/hurdle.o crypto/taa1.o crypto/tetra_crypto.o
 	$(AR) r $@ $^
 
 float_to_bits: float_to_bits.o
diff --git a/src/crypto/hurdle.c b/src/crypto/hurdle.c
new file mode 100644
index 0000000..58a1bb0
--- /dev/null
+++ b/src/crypto/hurdle.c
@@ -0,0 +1,194 @@
+/* TETRA HURDLE block cipher implementation */
+/*
+ * Copyright (C) 2023 Midnight Blue B.V.
+ *
+ * Author: Wouter Bokslag <w.bokslag [ ] midnightblue [ ] nl>
+ *
+ * SPDX-License-Identifier: AGPL-3.0+
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero 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 Affero General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ * See the COPYING file in the main directory for details.
+ */
+
+#include <stdio.h>
+#include <inttypes.h>
+#include <stdlib.h>
+
+#include "hurdle.h"
+
+const uint8_t g_abHurdleSbox[256] = {
+	0xF4, 0x65, 0x01, 0x00, 0xBA, 0x7A, 0xA7, 0x47, 0x98, 0xDD, 0x9D, 0xAD, 0x96, 0x5D, 0xAA, 0x3D,
+	0x58, 0xC0, 0x72, 0xD8, 0x66, 0x4C, 0x3E, 0xE0, 0x80, 0x55, 0xDE, 0x90, 0x2A, 0x4B, 0x83, 0xA0,
+	0x51, 0x39, 0xED, 0x6C, 0x8A, 0x2C, 0x56, 0x60, 0x4A, 0x1F, 0xD0, 0x70, 0x6E, 0x33, 0x8B, 0x26,
+	0x2E, 0x6F, 0x89, 0x48, 0x5E, 0x40, 0xC3, 0xA4, 0xA9, 0xCF, 0x22, 0x50, 0xE1, 0x15, 0x0C, 0xAB,
+	0xD5, 0xF8, 0x5F, 0x36, 0x04, 0xA6, 0x4E, 0x92, 0x1E, 0x2B, 0x88, 0x30, 0x93, 0x45, 0x67, 0x16,
+	0x8C, 0x68, 0x23, 0x38, 0x61, 0x25, 0x1A, 0x81, 0x63, 0xCB, 0xC1, 0x13, 0x41, 0x37, 0x0E, 0x97,
+	0x5B, 0xCA, 0x57, 0x24, 0x4D, 0x17, 0xC4, 0xB9, 0xB3, 0xEF, 0x8D, 0x52, 0x32, 0x2F, 0xEC, 0x20,
+	0xD9, 0x11, 0xD1, 0x28, 0x79, 0xDA, 0xFB, 0xE9, 0xBB, 0x06, 0x77, 0xDB, 0xFC, 0xFE, 0xCD, 0x84,
+	0x1D, 0xA1, 0x54, 0x1B, 0xB0, 0xE4, 0xCC, 0x7C, 0x2D, 0x27, 0x31, 0x49, 0xF5, 0x02, 0x69, 0x53,
+	0x4F, 0x44, 0xDF, 0x18, 0x5C, 0x0F, 0xBC, 0x9B, 0x94, 0xBD, 0xDC, 0x0B, 0xA2, 0xC7, 0x09, 0xAC,
+	0xC6, 0x9F, 0x82, 0x1C, 0x05, 0x46, 0xC2, 0x34, 0x3C, 0x0D, 0x3B, 0xCE, 0xB7, 0xBE, 0x08, 0x9C,
+	0x6B, 0xEE, 0xE5, 0x87, 0xAF, 0xBF, 0xF2, 0xEB, 0x7B, 0x07, 0x64, 0xC5, 0xB6, 0xAE, 0x9A, 0x95,
+	0x35, 0xA5, 0x59, 0x12, 0x9E, 0xA3, 0xB8, 0x8E, 0x5A, 0xF7, 0x62, 0xD2, 0x3A, 0xA8, 0x7D, 0x85,
+	0xF6, 0xC8, 0x71, 0x29, 0xD6, 0xD7, 0x43, 0xF9, 0x78, 0x76, 0x73, 0x10, 0x91, 0x19, 0x0A, 0x99,
+	0xF0, 0xE6, 0x3F, 0x14, 0xF1, 0xE2, 0xB1, 0x86, 0xB4, 0xF3, 0x74, 0xFA, 0x6A, 0xB2, 0x21, 0x6D,
+	0xEA, 0xB5, 0xE7, 0xE3, 0xC9, 0xD3, 0x8F, 0x03, 0x75, 0xE8, 0xD4, 0x42, 0xFD, 0x7E, 0xFF, 0x7F};
+
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+static const uint32_t g_adwReorder[16] = {
+	0x00000000, 0x80000000, 0x00800000, 0x80800000,
+	0x00008000, 0x80008000, 0x00808000, 0x80808000,
+	0x00000080, 0x80000080, 0x00800080, 0x80800080,
+	0x00008080, 0x80008080, 0x00808080, 0x80808080
+};
+#else
+static const uint32_t g_adwReorder[16] = {
+	0x00000000, 0x00000080, 0x00008000, 0x00008080,
+	0x00800000, 0x00800080, 0x00808000, 0x00808080,
+	0x80000000, 0x80000080, 0x80008000, 0x80008080,
+	0x80800000, 0x80800080, 0x80808000, 0x80808080
+};
+#endif
+
+void hurdle_set_key(uint8_t *k, struct hurdle_ctx *lpContextOut)
+{
+	/* Simplified key schedule by precomputing rotates and xor constants */
+	uint8_t abKeyBytes[256] = {
+		k[0], k[1], k[2], k[3], k[4], k[5], k[6], k[7], k[8], k[9], k[10], k[11], k[12], k[13], k[14], k[15],
+		k[5], k[6], k[7], k[8], k[9], k[10], k[11], k[12], k[13], k[14], k[15], k[0], k[1], k[2], k[3], k[4],
+		k[10], k[11], k[12], k[13], k[14], k[15], k[0], k[1], k[2], k[3], k[4], k[5], k[6], k[7], k[8], k[9],
+		k[15], k[0], k[1], k[2], k[3], k[4], k[5], k[6], k[7], k[8], k[9], k[10], k[11], k[12], k[13], k[14],
+		k[4], k[5], k[6], k[7], k[8], k[9], k[10], k[11], k[12], k[13], k[14], k[15], k[0], k[1], k[2], k[3],
+		k[7], k[8], k[9], k[10], k[11], k[12], k[13], k[14], k[15], k[0], k[1], k[2], k[3], k[4], k[5], k[6],
+		k[14], k[15], k[0], k[1], k[2], k[3], k[4], k[5], k[6], k[7], k[8], k[9], k[10], k[11], k[12], k[13],
+		k[3], k[4], k[5], k[6], k[7], k[8], k[9], k[10], k[11], k[12], k[13], k[14], k[15], k[0], k[1], k[2],
+		k[8], k[9], k[10], k[11], k[12], k[13], k[14], k[15], k[0], k[1], k[2], k[3], k[4], k[5], k[6], k[7],
+		k[13], k[14], k[15], k[0], k[1], k[2], k[3], k[4], k[5], k[6], k[7], k[8], k[9], k[10], k[11], k[12],
+		k[2], k[3], k[4], k[5], k[6], k[7], k[8], k[9], k[10], k[11], k[12], k[13], k[14], k[15], k[0], k[1],
+		k[9], k[10], k[11], k[12], k[13], k[14], k[15], k[0], k[1], k[2], k[3], k[4], k[5], k[6], k[7], k[8],
+		k[12], k[13], k[14], k[15], k[0], k[1], k[2], k[3], k[4], k[5], k[6], k[7], k[8], k[9], k[10], k[11],
+		k[1], k[2], k[3], k[4], k[5], k[6], k[7], k[8], k[9], k[10], k[11], k[12], k[13], k[14], k[15], k[0],
+		k[6], k[7], k[8], k[9], k[10], k[11], k[12], k[13], k[14], k[15], k[0], k[1], k[2], k[3], k[4], k[5],
+		k[11], k[12], k[13], k[14], k[15], k[0], k[1], k[2], k[3], k[4], k[5], k[6], k[7], k[8], k[9], k[10]};
+	static const uint8_t abKeyXorConsts[256] = {
+		0x00,  0x00,  0x00,  0x00,  0x00,  0x00,  0x00,  0x00,  0x00,  0x00,  0x00,  0x00,  0x00,  0x00,  0x00,  0x00,
+		0x3C,  0xA7,  0xEC,  0x25,  0x79,  0x57,  0xDF,  0xC0,  0x38,  0x0A,  0x33,  0x1E,  0xF3,  0x8C,  0xF4,  0xF7,
+		0x6B,  0x78,  0x2C,  0x1D,  0x73,  0x64,  0xC1,  0x33,  0xB4,  0xFE,  0xC4,  0x22,  0x54,  0x60,  0xD1,  0x8E,
+		0x58,  0x66,  0xDF,  0x91,  0x87,  0x93,  0xFD,  0x94,  0x58,  0xDB,  0xBD,  0x75,  0x8B,  0xA0,  0xE9,  0x84,
+		0xAF,  0x5A,  0x78,  0x7D,  0xA2,  0xEA,  0xAA,  0x4B,  0x98,  0xE3,  0xB7,  0x46,  0x95,  0x53,  0x65,  0x70,
+		0x41,  0x05,  0x06,  0x8F,  0x32,  0xCF,  0x3C,  0x77,  0x7E,  0x9F,  0x60,  0x7B,  0x83,  0x23,  0xAE,  0x8F,
+		0x4B,  0xD9,  0x73,  0x45,  0x02,  0xD4,  0xFC,  0x6E,  0xB7,  0x4B,  0x36,  0x18,  0x7C,  0xBE,  0x3B,  0xCB,
+		0xE8,  0x5B,  0x82,  0x92,  0x32,  0x61,  0xC7,  0xBC,  0x86,  0x31,  0xF8,  0x55,  0x2A,  0xFF,  0xB1,  0xF5,
+		0x5D,  0x60,  0x50,  0xA3,  0x48,  0xAF,  0x8A,  0xEA,  0xC7,  0xBB,  0xC6,  0xF6,  0xA8,  0x0E,  0x66,  0xC5,
+		0x93,  0x2D,  0x06,  0xE2,  0xC2,  0x91,  0x29,  0x68,  0x36,  0x6C,  0xF6,  0x43,  0x93,  0xDC,  0x57,  0xBF,
+		0xAD,  0x8E,  0x84,  0x13,  0x15,  0xA1,  0x9C,  0x53,  0xE4,  0x5D,  0x8C,  0x8D,  0xDE,  0x8A,  0x16,  0x35,
+		0x6F,  0x43,  0xB1,  0xA9,  0xF4,  0x89,  0x55,  0xD6,  0x0D,  0xA7,  0xBD,  0x9A,  0xE0,  0x99,  0x55,  0x6B,
+		0x95,  0x53,  0x65,  0x70,  0xAF,  0x5A,  0x78,  0x7D,  0xA2,  0xEA,  0xAA,  0x4B,  0x98,  0xE3,  0xB7,  0x46,
+		0x66,  0xDF,  0x91,  0x87,  0x93,  0xFD,  0x94,  0x58,  0xDB,  0xBD,  0x75,  0x8B,  0xA0,  0xE9,  0x84,  0x58,
+		0xC1,  0x33,  0xB4,  0xFE,  0xC4,  0x22,  0x54,  0x60,  0xD1,  0x8E,  0x6B,  0x78,  0x2C,  0x1D,  0x73,  0x64,
+		0x1E,  0xF3,  0x8C,  0xF4,  0xF7,  0x3C,  0xA7,  0xEC,  0x25,  0x79,  0x57,  0xDF,  0xC0,  0x38,  0x0A,  0x33};
+
+	/* Xor original key byte with round- and offset-specific xor byte */
+	for (int i = 0; i < 256; i++)
+		lpContextOut->abRoundKeys[i] = abKeyBytes[i] ^ abKeyXorConsts[i];
+}
+
+void HURDLE_f(uint8_t abOutput[4], const uint8_t abRhs[4], const uint8_t *lpRoundKey)
+{
+	#define PUSH_OUTPUT_NIBBLE(x) do { \
+		dwOutputBits >>= 1; \
+		dwOutputBits |= g_adwReorder[(x) & 0xf]; \
+	} while (0)
+
+	uint32_t dwOutputBits = 0;
+	uint8_t bSboxState = 0;
+
+	bSboxState = g_abHurdleSbox[(abRhs[3] + lpRoundKey[15]) & 0xff];
+	bSboxState = g_abHurdleSbox[((abRhs[2] + lpRoundKey[14]) ^ bSboxState) & 0xff];
+	bSboxState = g_abHurdleSbox[((abRhs[1] + lpRoundKey[13]) ^ bSboxState) & 0xff];
+	bSboxState = g_abHurdleSbox[((abRhs[0] + lpRoundKey[12]) ^ bSboxState) & 0xff];
+	bSboxState = g_abHurdleSbox[((abRhs[3] + lpRoundKey[11]) ^ bSboxState) & 0xff]; PUSH_OUTPUT_NIBBLE(bSboxState);
+	bSboxState = g_abHurdleSbox[((abRhs[1] + lpRoundKey[10]) ^ bSboxState) & 0xff]; PUSH_OUTPUT_NIBBLE(bSboxState);
+	bSboxState = g_abHurdleSbox[((abRhs[2] + lpRoundKey[9]) ^ bSboxState) & 0xff]; PUSH_OUTPUT_NIBBLE(bSboxState);
+	bSboxState = g_abHurdleSbox[((abRhs[0] + lpRoundKey[8]) ^ bSboxState) & 0xff]; PUSH_OUTPUT_NIBBLE(bSboxState);
+	bSboxState = g_abHurdleSbox[((abRhs[1] + lpRoundKey[7]) ^ bSboxState) & 0xff]; PUSH_OUTPUT_NIBBLE(bSboxState);
+	bSboxState = g_abHurdleSbox[((abRhs[3] + lpRoundKey[6]) ^ bSboxState) & 0xff]; PUSH_OUTPUT_NIBBLE(bSboxState);
+	bSboxState = g_abHurdleSbox[((abRhs[0] + lpRoundKey[5]) ^ bSboxState) & 0xff]; PUSH_OUTPUT_NIBBLE(bSboxState);
+	bSboxState = g_abHurdleSbox[((abRhs[2] + lpRoundKey[4]) ^ bSboxState) & 0xff]; PUSH_OUTPUT_NIBBLE(bSboxState);
+
+	*(uint32_t *)abOutput = dwOutputBits;
+}
+
+void HURDLE_encrypt(uint8_t abOutput[8], const uint8_t abInput[8], struct hurdle_ctx *lpKey, uint8_t eEncryptMode)
+{
+	uint32_t dwLhs, dwRhs, dwTemp;
+	int i;
+
+	/* start at first/last round key depending on encrypt/decrypt mode */
+	uint8_t *lpRoundKey = (eEncryptMode == HURDLE_DECRYPT) ? &lpKey->abRoundKeys[240] : lpKey->abRoundKeys;
+
+	/* copy state */
+	dwLhs = *(uint32_t *)&abInput[0];
+	dwRhs = *(uint32_t *)&abInput[4];
+
+	for (i = 0; i < 16; i++) {
+		/* Round function */
+		HURDLE_f((uint8_t *)&dwTemp, (uint8_t *)&dwRhs, lpRoundKey);
+
+		/* perform a left-right switcharoo */
+		dwTemp ^= dwLhs;
+		dwLhs = dwRhs;
+		dwRhs = dwTemp;
+
+		/* move to next/previous round key depending on encrypt/decrypt mode */
+		lpRoundKey += (eEncryptMode == HURDLE_DECRYPT) ? -16 : 16;
+	}
+
+	*(uint32_t *)&abOutput[0] = dwRhs;
+	*(uint32_t *)&abOutput[4] = dwLhs;
+}
+
+void HURDLE_enc_cbc(uint8_t abCiphertext[16], const uint8_t abPlaintext[16], uint8_t abKey[16])
+{
+	uint8_t abIntermediate[8];
+	struct hurdle_ctx stCipher;
+
+	hurdle_set_key(abKey, &stCipher);
+	HURDLE_encrypt(abCiphertext, abPlaintext, &stCipher, HURDLE_ENCRYPT);
+	*(uint32_t *)&abIntermediate[0] = *(uint32_t *)&abCiphertext[0] ^ *(uint32_t *)&abPlaintext[8];
+	*(uint32_t *)&abIntermediate[4] = *(uint32_t *)&abCiphertext[4] ^ *(uint32_t *)&abPlaintext[12];
+	HURDLE_encrypt(&abCiphertext[8], abIntermediate, &stCipher, HURDLE_ENCRYPT);
+}
+
+void HURDLE_dec_cts(uint8_t abPlaintext[15], const uint8_t abCiphertext[15], uint8_t abKey[16])
+{
+	uint8_t abIntermediate[16];
+	struct hurdle_ctx stCipher;
+	hurdle_set_key(abKey, &stCipher);
+
+	HURDLE_encrypt(&abIntermediate[8], &abCiphertext[7], &stCipher, HURDLE_DECRYPT);
+	*(uint32_t *)&abIntermediate[0] = *(uint32_t *)&abCiphertext[0];
+	*(uint32_t *)&abIntermediate[4] = *(uint32_t *)&abCiphertext[4];
+	abIntermediate[7] = abIntermediate[15];
+	HURDLE_encrypt(&abIntermediate[0], &abIntermediate[0], &stCipher, HURDLE_DECRYPT);
+	*(uint32_t *)&abIntermediate[8]  ^= *(uint32_t *)&abCiphertext[0];
+	*(uint16_t *)&abIntermediate[12] ^= *(uint16_t *)&abCiphertext[4];
+	*(uint8_t  *)&abIntermediate[14] ^= *(uint8_t  *)&abCiphertext[6];
+
+	*(uint32_t *)&abPlaintext[0]  = *(uint32_t *)&abIntermediate[0];
+	*(uint32_t *)&abPlaintext[4]  = *(uint32_t *)&abIntermediate[4];
+	*(uint32_t *)&abPlaintext[8]  = *(uint32_t *)&abIntermediate[8];
+	*(uint16_t *)&abPlaintext[12] = *(uint16_t *)&abIntermediate[12];
+	*(uint8_t  *)&abPlaintext[14] = *(uint8_t  *)&abIntermediate[14];
+}
diff --git a/src/crypto/hurdle.h b/src/crypto/hurdle.h
new file mode 100644
index 0000000..bbd641c
--- /dev/null
+++ b/src/crypto/hurdle.h
@@ -0,0 +1,20 @@
+#ifndef HAVE_HURDLE_H
+#define HAVE_HURDLE_H
+
+#include <inttypes.h>
+
+struct hurdle_ctx {
+	uint8_t abRoundKeys[256];
+};
+
+#define HURDLE_ENCRYPT 0
+#define HURDLE_DECRYPT 1
+
+void hurdle_set_key(uint8_t *k, struct hurdle_ctx *lpContextOut);
+void HURDLE_encrypt(uint8_t abOutput[8], const uint8_t abInput[8], struct hurdle_ctx *lpKey, uint8_t eEncryptMode);
+void HURDLE_enc_cbc(uint8_t abCiphertext[16], const uint8_t abPlaintext[16], uint8_t abKey[16]);
+void HURDLE_dec_cts(uint8_t abPlaintext[15], const uint8_t abCiphertext[15], uint8_t abKey[16]);
+
+extern const uint8_t g_abHurdleSbox[256];
+
+#endif /* HAVE_HURDLE_H */
diff --git a/src/crypto/taa1.c b/src/crypto/taa1.c
new file mode 100644
index 0000000..09e3a4b
--- /dev/null
+++ b/src/crypto/taa1.c
@@ -0,0 +1,474 @@
+/* TETRA TAA1 suite implementation */
+/*
+ * Copyright (C) 2023 Midnight Blue B.V.
+ *
+ * Author: Wouter Bokslag <w.bokslag [ ] midnightblue [ ] nl>
+ *
+ * SPDX-License-Identifier: AGPL-3.0+
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero 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 Affero General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ * See the COPYING file in the main directory for details.
+ */
+
+#include <stdio.h>
+#include <inttypes.h>
+#include <assert.h>
+#include <string.h>
+
+#include "taa1.h"
+#include "hurdle.h"
+
+void transform_80_to_120(const uint8_t *lpBuffer, uint8_t *lpBufferOut)
+{
+	lpBufferOut[0] = lpBuffer[0] + lpBuffer[9];
+	lpBufferOut[1] = lpBuffer[0];
+	lpBufferOut[2] = lpBuffer[9];
+	lpBufferOut[3] = lpBuffer[1] + lpBuffer[8];
+	lpBufferOut[4] = lpBuffer[1];
+	lpBufferOut[5] = lpBuffer[8];
+	lpBufferOut[6] = lpBuffer[2] + lpBuffer[7];
+	lpBufferOut[7] = lpBuffer[2];
+	lpBufferOut[8] = lpBuffer[7];
+	lpBufferOut[9] = lpBuffer[3] + lpBuffer[6];
+	lpBufferOut[10] = lpBuffer[3];
+	lpBufferOut[11] = lpBuffer[6];
+	lpBufferOut[12] = lpBuffer[4] + lpBuffer[5];
+	lpBufferOut[13] = lpBuffer[4];
+	lpBufferOut[14] = lpBuffer[5];
+}
+
+void transform_80_to_128(const uint8_t *lpBuffer, uint8_t *lpBufferOut)
+{
+	transform_80_to_120(lpBuffer, lpBufferOut + 1);
+	lpBufferOut[0] = lpBufferOut[1] ^ lpBufferOut[4] ^ lpBufferOut[7] ^ lpBufferOut[10] ^ lpBufferOut[13];
+}
+
+void transform_80_to_120_alt(const uint8_t *lpBuffer, uint8_t *lpBufferOut)
+{
+	lpBufferOut[0] = lpBuffer[0];
+	lpBufferOut[1] = lpBuffer[1];
+	lpBufferOut[2] = lpBufferOut[0] ^ lpBufferOut[1];
+	lpBufferOut[3] = lpBuffer[2];
+	lpBufferOut[4] = lpBuffer[3];
+	lpBufferOut[5] = lpBufferOut[3] ^ lpBufferOut[4];
+	lpBufferOut[6] = lpBuffer[4];
+	lpBufferOut[7] = lpBuffer[5];
+	lpBufferOut[8] = lpBufferOut[6] ^ lpBufferOut[7];
+	lpBufferOut[9] = lpBuffer[6];
+	lpBufferOut[10] = lpBuffer[7];
+	lpBufferOut[11] = lpBufferOut[9] ^ lpBufferOut[10];
+	lpBufferOut[12] = lpBuffer[8];
+	lpBufferOut[13] = lpBuffer[9];
+	lpBufferOut[14] = lpBufferOut[12] ^ lpBufferOut[13];
+}
+
+void transform_80_to_128_alt(const uint8_t *lpBuffer, uint8_t *lpBufferOut)
+{
+	transform_80_to_120_alt(lpBuffer, lpBufferOut);
+	lpBufferOut[15] = lpBufferOut[2] + lpBufferOut[5] + lpBufferOut[8] + lpBufferOut[11] + lpBufferOut[14];
+}
+
+void transform_88_to_120(const uint8_t *lpBuffer, uint8_t *lpBufferOut)
+{
+	lpBufferOut[0] = lpBuffer[0];
+	lpBufferOut[1] = lpBuffer[1];
+	lpBufferOut[2] = lpBuffer[0] ^ lpBuffer[1];
+	lpBufferOut[3] = lpBuffer[2];
+	lpBufferOut[4] = lpBuffer[3];
+	lpBufferOut[5] = lpBuffer[4];
+	lpBufferOut[6] = lpBuffer[2] ^ lpBuffer[3] ^ lpBuffer[4];
+	lpBufferOut[7] = lpBuffer[5];
+	lpBufferOut[8] = lpBuffer[6];
+	lpBufferOut[9] = lpBuffer[7];
+	lpBufferOut[10] = lpBuffer[5] ^ lpBuffer[6] ^ lpBuffer[7];
+	lpBufferOut[11] = lpBuffer[8];
+	lpBufferOut[12] = lpBuffer[9];
+	lpBufferOut[13] = lpBuffer[10];
+	lpBufferOut[14] = lpBuffer[8] ^ lpBuffer[9] ^ lpBuffer[10];
+}
+
+void transform_120_to_88(const uint8_t *lpBuffer, uint8_t *lpBufferOut)
+{
+	lpBufferOut[0] = lpBuffer[0];
+	lpBufferOut[1] = lpBuffer[1];
+	lpBufferOut[2] = lpBuffer[3];
+	lpBufferOut[3] = lpBuffer[4];
+	lpBufferOut[4] = lpBuffer[5];
+	lpBufferOut[5] = lpBuffer[7];
+	lpBufferOut[6] = lpBuffer[8];
+	lpBufferOut[7] = lpBuffer[9];
+	lpBufferOut[8] = lpBuffer[11];
+	lpBufferOut[9] = lpBuffer[12];
+	lpBufferOut[10] = lpBuffer[13];
+}
+
+void transform_120_to_80_alt(const uint8_t *lpBuffer, uint8_t *lpBufferOut)
+{
+	lpBufferOut[0] = lpBuffer[0];
+	lpBufferOut[1] = lpBuffer[1];
+	lpBufferOut[2] = lpBuffer[3];
+	lpBufferOut[3] = lpBuffer[4];
+	lpBufferOut[4] = lpBuffer[6];
+	lpBufferOut[5] = lpBuffer[7];
+	lpBufferOut[6] = lpBuffer[9];
+	lpBufferOut[7] = lpBuffer[10];
+	lpBufferOut[8] = lpBuffer[12];
+	lpBufferOut[9] = lpBuffer[13];
+}
+
+void ta11_ta41(uint8_t *lpKeyK, uint8_t *lpChallengeRs, uint8_t *lpKsOut)
+{
+	uint8_t abChallengeExpanded[16];
+	transform_80_to_128_alt(lpChallengeRs, abChallengeExpanded);
+	HURDLE_enc_cbc(lpKsOut, abChallengeExpanded, lpKeyK);
+}
+
+void ta12_ta22(uint8_t *lpKeyKs, uint8_t *lpRand, uint8_t *lpResOut, uint8_t *lpDckOut)
+{
+	uint8_t abRandExpanded[16];
+	uint8_t abCiphertext[16];
+	transform_80_to_128_alt(lpRand, abRandExpanded);
+	HURDLE_enc_cbc(abCiphertext, abRandExpanded, lpKeyKs);
+
+	lpResOut[0] = abCiphertext[0] ^ abCiphertext[3];
+	lpResOut[1] = abCiphertext[6];
+	lpResOut[2] = abCiphertext[9];
+	lpResOut[3] = abCiphertext[12] ^ abCiphertext[15];
+
+	lpDckOut[0] = abCiphertext[1];
+	lpDckOut[1] = abCiphertext[2];
+	lpDckOut[2] = abCiphertext[4];
+	lpDckOut[3] = abCiphertext[5];
+	lpDckOut[4] = abCiphertext[7];
+	lpDckOut[5] = abCiphertext[8];
+	lpDckOut[6] = abCiphertext[10];
+	lpDckOut[7] = abCiphertext[11];
+	lpDckOut[8] = abCiphertext[13];
+	lpDckOut[9] = abCiphertext[14];
+}
+
+void ta21(uint8_t *lpKeyK, uint8_t *lpChallengeRs, uint8_t *lpKspOut)
+{
+	uint8_t abChallengeExpanded[16];
+	uint8_t abChallengeReversed[10];
+	int i;
+
+	for (i = 0; i < 10; i++)
+		abChallengeReversed[i] = lpChallengeRs[9-i];
+
+	transform_80_to_128_alt(abChallengeReversed, abChallengeExpanded);
+	HURDLE_enc_cbc(lpKspOut, abChallengeExpanded, lpKeyK);
+}
+
+void ta31(uint8_t *lpUnsealedCck, uint8_t *lpCckId, uint8_t *lpDck, uint8_t *lpSealedCckOut)
+{
+	uint8_t abUnsealedPadded[16];
+	uint8_t abHurdleKey[16];
+	uint8_t abAdjustedDck[10];
+	uint8_t abSealed[16];
+	int i;
+
+	transform_80_to_120_alt(lpUnsealedCck, abUnsealedPadded);
+	abUnsealedPadded[15] = '\0';
+
+	for (i = 0; i < 10; i++)
+		abAdjustedDck[i] = lpDck[i] ^ lpCckId[i & 1];
+
+	transform_80_to_128(abAdjustedDck, abHurdleKey);
+	HURDLE_enc_cbc(abSealed, abUnsealedPadded, abHurdleKey);
+	/* ciphertext stealing */
+	memcpy(lpSealedCckOut, abSealed, 7);
+	memcpy(lpSealedCckOut + 7, abSealed + 8, 8);
+}
+
+void ta32(uint8_t *lpSealedCck, uint8_t *lpCckId, uint8_t *lpDck, uint8_t *lpUnsealedCckOut, uint8_t *lpMfOut)
+{
+	uint8_t abHurdleKey[16];
+	uint8_t abAdjustedDck[10];
+	uint8_t abUnsealedPadded[16];
+	int i;
+
+	for (i = 0; i < 10; i++)
+		abAdjustedDck[i] = lpDck[i] ^ lpCckId[i & 1];
+
+	transform_80_to_128(abAdjustedDck, abHurdleKey);
+
+	HURDLE_dec_cts(abUnsealedPadded, lpSealedCck, abHurdleKey);
+	transform_120_to_80_alt(abUnsealedPadded, lpUnsealedCckOut);
+
+	*lpMfOut =
+		((abUnsealedPadded[0] ^ abUnsealedPadded[1]) != abUnsealedPadded[2]) ||
+		((abUnsealedPadded[3] ^ abUnsealedPadded[4]) != abUnsealedPadded[5]) ||
+		((abUnsealedPadded[6] ^ abUnsealedPadded[7]) != abUnsealedPadded[8]) ||
+		((abUnsealedPadded[9] ^ abUnsealedPadded[10]) != abUnsealedPadded[11]) ||
+		((abUnsealedPadded[12] ^ abUnsealedPadded[13]) != abUnsealedPadded[14]);
+}
+
+void ta51(uint8_t *lpUnsealed, uint8_t *lpVn, uint8_t *lpKey, uint8_t *lpKeyN, uint8_t *lpSealedOut)
+{
+	uint8_t abUnsealed[11];
+	uint8_t abUnsealedPadded[16];
+	uint8_t abSealed[16];
+	uint8_t abAdjustedKey[16];
+	int i;
+
+	assert((*lpKeyN & 0xe0) == 0);
+
+	memcpy(abUnsealed, lpUnsealed, 10);
+	abUnsealed[10] = *lpKeyN;
+	transform_88_to_120(abUnsealed, abUnsealedPadded);
+	abUnsealedPadded[15] = '\0';
+
+	for (i = 0; i < 16; i++)
+		abAdjustedKey[i] = lpKey[i] ^ lpVn[i & 1];
+
+	HURDLE_enc_cbc(abSealed, abUnsealedPadded, abAdjustedKey);
+	/* ciphertext stealing */
+	memcpy(lpSealedOut, abSealed, 7);
+	memcpy(lpSealedOut + 7, abSealed + 8, 8);
+}
+
+void ta52(uint8_t *lpSealed, uint8_t *lpKey, uint8_t *lpVn, uint8_t *lpUnsealedOut, uint8_t *lpMfOut, uint8_t *lpKeyNOut)
+{
+	uint8_t abAdjustedKey[16];
+	uint8_t abUnsealedPadded[15];
+	uint8_t abUnsealed[11];
+	int i;
+
+	for (i = 0; i < 16; i++)
+		abAdjustedKey[i] = lpKey[i] ^ lpVn[i & 1];
+
+	HURDLE_dec_cts(abUnsealedPadded, lpSealed, abAdjustedKey);
+	transform_120_to_88(abUnsealedPadded, abUnsealed);
+	memcpy(lpUnsealedOut, abUnsealed, 10);
+	*lpKeyNOut = abUnsealed[10];
+	*lpMfOut =
+		((abUnsealedPadded[0] ^ abUnsealedPadded[1]) != abUnsealedPadded[2]) ||
+		((abUnsealedPadded[3] ^ abUnsealedPadded[4] ^ abUnsealedPadded[5]) != abUnsealedPadded[6]) ||
+		((abUnsealedPadded[7] ^ abUnsealedPadded[8] ^ abUnsealedPadded[9]) != abUnsealedPadded[10]) ||
+		((abUnsealedPadded[11] ^ abUnsealedPadded[12] ^ abUnsealedPadded[13]) != abUnsealedPadded[14]) ||
+		abUnsealed[10] & 0xe0;
+}
+
+void ta71(uint8_t *lpGck, uint8_t *lpCck, uint8_t *lpMgckOut)
+{
+	uint8_t abCiphertext[16];
+	uint8_t abHurdleKey[16];
+	uint8_t abPlaintextExpanded[16];
+	uint8_t abPlaintext[10];
+	int i;
+
+	for (i = 0; i < 10; i++)
+		abPlaintext[i] = lpGck[i] ^ lpCck[i];
+
+	transform_80_to_128_alt(abPlaintext, abPlaintextExpanded);
+
+	abHurdleKey[0] = lpGck[0];
+	abHurdleKey[1] = lpGck[1];
+	abHurdleKey[2] = lpGck[2];
+	abHurdleKey[3] = lpGck[3];
+	abHurdleKey[4] = lpGck[4];
+	abHurdleKey[5] = lpGck[5];
+	abHurdleKey[6] = lpGck[6] ^ lpCck[0];
+	abHurdleKey[7] = lpGck[7] ^ lpCck[1];
+	abHurdleKey[8] = lpGck[8] ^ lpCck[2];
+	abHurdleKey[9] = lpGck[9] ^ lpCck[3];
+	abHurdleKey[10] = lpCck[4];
+	abHurdleKey[11] = lpCck[5];
+	abHurdleKey[12] = lpCck[6];
+	abHurdleKey[13] = lpCck[7];
+	abHurdleKey[14] = lpCck[8];
+	abHurdleKey[15] = lpCck[9];
+
+	HURDLE_enc_cbc(abCiphertext, abPlaintextExpanded, abHurdleKey);
+
+	memcpy(lpMgckOut, &abCiphertext[3], 10);
+}
+
+void ta81(uint8_t *lpUnsealedGck, uint8_t *lpGckVn, uint8_t *lpGckN, uint8_t *lpKey, uint8_t *lpSealedGckOut)
+{
+	uint8_t abUnsealedPadded[16];
+	uint8_t abSealed[16];
+	uint8_t abAdjustedKey[16];
+	int i;
+
+	abUnsealedPadded[0] = lpUnsealedGck[0];
+	abUnsealedPadded[1] = lpUnsealedGck[1];
+	abUnsealedPadded[2] = lpUnsealedGck[2];
+	abUnsealedPadded[3] = lpUnsealedGck[3];
+	abUnsealedPadded[4] = abUnsealedPadded[0] ^ abUnsealedPadded[1] ^ abUnsealedPadded[2] ^ abUnsealedPadded[3];
+	abUnsealedPadded[5] = lpUnsealedGck[4];
+	abUnsealedPadded[6] = lpUnsealedGck[5];
+	abUnsealedPadded[7] = lpUnsealedGck[6];
+	abUnsealedPadded[8] = lpUnsealedGck[7];
+	abUnsealedPadded[9] = abUnsealedPadded[5] ^ abUnsealedPadded[6] ^ abUnsealedPadded[7] ^ abUnsealedPadded[8];
+	abUnsealedPadded[10] = lpUnsealedGck[8];
+	abUnsealedPadded[11] = lpUnsealedGck[9];
+	abUnsealedPadded[12] = lpGckN[0];
+	abUnsealedPadded[13] = lpGckN[1];
+	abUnsealedPadded[14] = abUnsealedPadded[10] ^ abUnsealedPadded[11] ^ abUnsealedPadded[12] ^ abUnsealedPadded[13];
+	abUnsealedPadded[15] = '\0';
+
+	for (i = 0; i < 16; i++)
+		abAdjustedKey[i] = lpKey[i] ^ lpGckVn[i & 1];
+
+	HURDLE_enc_cbc(abSealed, abUnsealedPadded, abAdjustedKey);
+	/* ciphertext stealing */
+	memcpy(lpSealedGckOut, abSealed, 7);
+	memcpy(lpSealedGckOut + 7, abSealed + 8, 8);
+}
+
+void ta82(uint8_t *lpSealedGck, uint8_t *lpGckVn, uint8_t *lpKey, uint8_t *lpUnsealedGckOut, uint8_t *lpMfOut, uint8_t *lpGckNOut)
+{
+	uint8_t abAdjustedKey[16];
+	uint8_t abUnsealedPadded[15];
+	int i;
+
+	for (i = 0; i < 16; i++)
+		abAdjustedKey[i] = lpKey[i] ^ lpGckVn[i & 1];
+
+	HURDLE_dec_cts(abUnsealedPadded, lpSealedGck, abAdjustedKey);
+
+	lpUnsealedGckOut[0] = abUnsealedPadded[0];
+	lpUnsealedGckOut[1] = abUnsealedPadded[1];
+	lpUnsealedGckOut[2] = abUnsealedPadded[2];
+	lpUnsealedGckOut[3] = abUnsealedPadded[3];
+	lpUnsealedGckOut[4] = abUnsealedPadded[5];
+	lpUnsealedGckOut[5] = abUnsealedPadded[6];
+	lpUnsealedGckOut[6] = abUnsealedPadded[7];
+	lpUnsealedGckOut[7] = abUnsealedPadded[8];
+	lpUnsealedGckOut[8] = abUnsealedPadded[10];
+	lpUnsealedGckOut[9] = abUnsealedPadded[11];
+
+	lpGckNOut[0] = abUnsealedPadded[12];
+	lpGckNOut[1] = abUnsealedPadded[13];
+
+	*lpMfOut =
+		(abUnsealedPadded[14] != (abUnsealedPadded[10] ^ abUnsealedPadded[11] ^ abUnsealedPadded[12] ^ abUnsealedPadded[13])) ||
+		(abUnsealedPadded[9] != (abUnsealedPadded[5] ^ abUnsealedPadded[6] ^ abUnsealedPadded[7] ^ abUnsealedPadded[8])) ||
+		(abUnsealedPadded[4] != (abUnsealedPadded[0] ^ abUnsealedPadded[1] ^ abUnsealedPadded[2] ^ abUnsealedPadded[3]));
+}
+
+void ta91(uint8_t *lpUnsealedGsko, uint8_t *lpGskoVn, uint8_t *lpKey, uint8_t *lpSealedGskoOut)
+{
+	return ta81(lpUnsealedGsko, lpGskoVn, lpUnsealedGsko + 10, lpKey, lpSealedGskoOut);
+}
+
+void ta92(uint8_t *lpSealedGsko, uint8_t *lpGskoVn, uint8_t *lpKey, uint8_t *lpUnsealedGskoOut, uint8_t *lpMfOut)
+{
+	return ta82(lpSealedGsko, lpGskoVn, lpKey, lpUnsealedGskoOut, lpMfOut, lpUnsealedGskoOut + 10);
+}
+
+
+void tb4(uint8_t *lpDck1, uint8_t *lpDck2, uint8_t *lpDckOut)
+{
+	int i;
+	for (i = 0; i < 10; i++)
+		lpDckOut[i] = lpDck1[i] ^ lpDck2[i];
+}
+
+void tb5(uint8_t *lpCn, uint8_t *lpLa, uint8_t *lpCc, uint8_t *lpCk, uint8_t *lpEckOut)
+{
+	uint32_t adwComputedEck[3];
+	uint32_t adwInputCk[3];
+
+	adwInputCk[0] = be16(*(uint16_t *)&lpCk[0]);
+	adwInputCk[1] = be32(*(uint32_t *)&lpCk[2]);
+	adwInputCk[2] = be32(*(uint32_t *)&lpCk[6]);
+
+	uint16_t wCn = be16(*(uint16_t *)lpCn);
+	uint16_t wLa = be16(*(uint16_t *)lpLa);
+	uint8_t bCc = *lpCc;
+
+	/*
+	wCn = 12 bits = cn carrier number
+	wLa = 14 bits = la location area code
+	bCc =  6 bits = cc color code
+
+	Computes:
+	[ la:14 cn:12 cc:6 cn:12 cc:6 cn:12 cc:6 cn:12 ]
+				xor
+	[			ck:80                  ]
+	*/
+
+	assert((wCn & ~0xFFF) == 0);
+	assert((wLa & ~0x3FFF) == 0);
+	assert((bCc & ~0x3F) == 0);
+
+	uint32_t dwMask0, dwMask1, dwMask2;
+	dwMask0 = (wLa << 2) | (wCn >> 10);
+	dwMask1 = (wCn << 22) | (bCc << 16) | (wCn << 4) | (bCc >> 2);
+	dwMask2 = (bCc << 30) | (wCn << 18) | (bCc << 12) | wCn;
+
+	adwComputedEck[0] = adwInputCk[0] ^ dwMask0;
+	adwComputedEck[1] = adwInputCk[1] ^ dwMask1;
+	adwComputedEck[2] = adwInputCk[2] ^ dwMask2;
+
+	*(uint16_t *)(&lpEckOut[0]) = be16(adwComputedEck[0]);
+	*(uint32_t *)(&lpEckOut[2]) = be32(adwComputedEck[1]);
+	*(uint32_t *)(&lpEckOut[6]) = be32(adwComputedEck[2]);
+}
+
+void tb6(uint8_t *lpSck, uint8_t *lpCn, uint8_t *lpSsi, uint8_t *lpEckOut)
+{
+	uint32_t adwComputedEck[3];
+	uint32_t adwInputSck[3];
+
+	adwInputSck[0] = be16(*(uint16_t *)&lpSck[0]);
+	adwInputSck[1] = be32(*(uint32_t *)&lpSck[2]);
+	adwInputSck[2] = be32(*(uint32_t *)&lpSck[6]);
+
+	uint16_t wCn = be16(*(uint16_t *)lpCn);
+	uint32_t dwSsi = (be16(*(uint16_t *)lpSsi) << 8) | lpSsi[2];
+
+	uint32_t dwMask0, dwMask1, dwMask2;
+	/*
+	Computes:
+	[cn:12 ssi:24 cn:12 ssi:24 lsb(ssi):8 ]
+			xor
+	[		sck:80                ]
+	*/
+
+	dwMask0 = (wCn << 4) | (dwSsi >> 20);
+	dwMask1 = (dwSsi << 12) | wCn;
+	dwMask2 = (dwSsi << 8) | (dwSsi & 0xff);
+
+	adwComputedEck[0] = adwInputSck[0] ^ dwMask0;
+	adwComputedEck[1] = adwInputSck[1] ^ dwMask1;
+	adwComputedEck[2] = adwInputSck[2] ^ dwMask2;
+
+	*(uint16_t *)(&lpEckOut[0]) = be16(adwComputedEck[0]);
+	*(uint32_t *)(&lpEckOut[2]) = be32(adwComputedEck[1]);
+	*(uint32_t *)(&lpEckOut[6]) = be32(adwComputedEck[2]);
+}
+
+void tb7(uint8_t *lpGsko, uint8_t *lpEgskoOut)
+{
+	lpEgskoOut[0] = lpGsko[0];
+	lpEgskoOut[1] = lpGsko[1];
+	lpEgskoOut[2] = lpGsko[2];
+	lpEgskoOut[3] = lpGsko[0] ^ lpGsko[1] ^ lpGsko[2];
+	lpEgskoOut[4] = lpGsko[3];
+	lpEgskoOut[5] = lpGsko[4];
+	lpEgskoOut[6] = lpGsko[5];
+	lpEgskoOut[7] = lpGsko[3] ^ lpGsko[4] ^ lpGsko[5];
+	lpEgskoOut[8] = lpGsko[6];
+	lpEgskoOut[9] = lpGsko[7];
+	lpEgskoOut[10] = lpGsko[8];
+	lpEgskoOut[11] = lpGsko[6] ^ lpGsko[7] ^ lpGsko[8];
+	lpEgskoOut[12] = lpGsko[9];
+	lpEgskoOut[13] = lpGsko[10];
+	lpEgskoOut[14] = lpGsko[11];
+	lpEgskoOut[15] = lpGsko[9] ^ lpGsko[10] ^ lpGsko[11];
+}
diff --git a/src/crypto/taa1.h b/src/crypto/taa1.h
new file mode 100644
index 0000000..4baff6e
--- /dev/null
+++ b/src/crypto/taa1.h
@@ -0,0 +1,53 @@
+#ifndef HAVE_TAA1_H
+#define HAVE_TAA1_H
+
+#include <inttypes.h>
+
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+#define be32(x) __builtin_bswap32(x)
+#define be16(x) __builtin_bswap16(x)
+#else
+#define be32(x) (x)
+#define be16(x) (x)
+#endif
+
+
+/*
+ * transformation functions used by TAxx primitives
+ */
+void transform_80_to_120(const uint8_t *lpBuffer, uint8_t *lpBufferOut);
+void transform_80_to_128(const uint8_t *lpBuffer, uint8_t *lpBufferOut);
+void transform_80_to_120_alt(const uint8_t *lpBuffer, uint8_t *lpBufferOut);
+void transform_80_to_128_alt(const uint8_t *lpBuffer, uint8_t *lpBufferOut);
+void transform_88_to_120(const uint8_t *lpBuffer, uint8_t *lpBufferOut);
+void transform_120_to_88(const uint8_t *lpBuffer, uint8_t *lpBufferOut);
+void transform_120_to_80_alt(const uint8_t *lpBuffer, uint8_t *lpBufferOut);
+void transform_identity(const uint8_t *lpInput, uint8_t *lpOutput);
+void transform_identity_inverse(const uint8_t *lpInput, uint8_t *lpOutput);
+
+/*
+ * TAxx primitives for authentication, key derivation and sealing functionality
+ * (see ETSI EN 300 392-7)
+ */
+void ta11_ta41(uint8_t *lpKeyK, uint8_t *lpChallengeRs, uint8_t *lpKsOut);
+void ta12_ta22(uint8_t *lpKeyKs, uint8_t *lpRand, uint8_t *lpResOut, uint8_t *lpDckOut);
+void ta21(uint8_t *lpKeyK, uint8_t *lpChallengeRs, uint8_t *lpKspOut);
+void ta31(uint8_t *lpUnsealedCck, uint8_t *lpCckId, uint8_t *lpDck, uint8_t *lpSealedCckOut);
+void ta32(uint8_t *lpSealedCck, uint8_t *lpCckId, uint8_t *lpDck, uint8_t *lpUnsealedCckOut, uint8_t *lpMfOut);
+void ta51(uint8_t *lpUnsealed, uint8_t *lpVn, uint8_t *lpKey, uint8_t *lpKeyN, uint8_t *lpSealedOut);
+void ta52(uint8_t *lpSealed, uint8_t *lpKey, uint8_t *lpVn, uint8_t *lpUnsealedOut, uint8_t *lpMfOut, uint8_t *lpKeyNOut);
+void ta71(uint8_t *lpGck, uint8_t *lpCck, uint8_t *lpMgckOut);
+void ta81(uint8_t *lpUnsealedGck, uint8_t *lpGckVn, uint8_t *lpGckN, uint8_t *lpKey, uint8_t *lpSealedGckOut);
+void ta82(uint8_t *lpSealedGck, uint8_t *lpGckVn, uint8_t *lpKey, uint8_t *lpUnsealedGckOut, uint8_t *lpMfOut, uint8_t *lpGckNOut);
+void ta91(uint8_t *lpUnsealedGsko, uint8_t *lpGskoVn, uint8_t *lpKey, uint8_t *lpSealedGskoOut);
+void ta92(uint8_t *lpSealedGsko, uint8_t *lpGskoVn, uint8_t *lpKey, uint8_t *lpUnsealedGskoOut, uint8_t *lpMfOut);
+
+/*
+ * TBxx non-cryptographic primitives also used for authentication and key derivation
+ */
+void tb4(uint8_t *lpDck1, uint8_t *lpDck2, uint8_t *lpDckOut);
+void tb5(uint8_t *lpCn, uint8_t *lpLa, uint8_t *lpCc, uint8_t *lpCk, uint8_t *lpEckOut);
+void tb6(uint8_t *lpSck, uint8_t *lpCn, uint8_t *lpSsi, uint8_t *lpEckOut);
+void tb7(uint8_t *lpGsko, uint8_t *lpEgskoOut);
+
+#endif /* HAVE_TAA1_H */
diff --git a/src/crypto/tea1.c b/src/crypto/tea1.c
new file mode 100644
index 0000000..02428e3
--- /dev/null
+++ b/src/crypto/tea1.c
@@ -0,0 +1,139 @@
+/* TETRA TEA1 keystream generator implementation */
+/*
+ * Copyright (C) 2023 Midnight Blue B.V.
+ *
+ * Author: Wouter Bokslag <w.bokslag [ ] midnightblue [ ] nl>
+ *
+ * SPDX-License-Identifier: AGPL-3.0+
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero 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 Affero General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ * See the COPYING file in the main directory for details.
+ */
+
+#include <stdio.h>
+#include <inttypes.h>
+#include <string.h>
+
+#include "tea1.h"
+
+
+const uint16_t g_awTea1LutA[8] = { 0xDA86, 0x85E9, 0x29B5, 0x2BC6, 0x8C6B, 0x974C, 0xC671, 0x93E2 };
+const uint16_t g_awTea1LutB[8] = { 0x85D6, 0x791A, 0xE985, 0xC671, 0x2B9C, 0xEC92, 0xC62B, 0x9C47 };
+const uint8_t g_abTea1Sbox[256] = {
+	0x9B, 0xF8, 0x3B, 0x72, 0x75, 0x62, 0x88, 0x22, 0xFF, 0xA6, 0x10, 0x4D, 0xA9, 0x97, 0xC3, 0x7B,
+	0x9F, 0x78, 0xF3, 0xB6, 0xA0, 0xCC, 0x17, 0xAB, 0x4A, 0x41, 0x8D, 0x89, 0x25, 0x87, 0xD3, 0xE3,
+	0xCE, 0x47, 0x35, 0x2C, 0x6D, 0xFC, 0xE7, 0x6A, 0xB8, 0xB7, 0xFA, 0x8B, 0xCD, 0x74, 0xEE, 0x11,
+	0x23, 0xDE, 0x39, 0x6C, 0x1E, 0x8E, 0xED, 0x30, 0x73, 0xBE, 0xBB, 0x91, 0xCA, 0x69, 0x60, 0x49,
+	0x5F, 0xB9, 0xC0, 0x06, 0x34, 0x2A, 0x63, 0x4B, 0x90, 0x28, 0xAC, 0x50, 0xE4, 0x6F, 0x36, 0xB0,
+	0xA4, 0xD2, 0xD4, 0x96, 0xD5, 0xC9, 0x66, 0x45, 0xC5, 0x55, 0xDD, 0xB2, 0xA1, 0xA8, 0xBF, 0x37,
+	0x32, 0x2B, 0x3E, 0xB5, 0x5C, 0x54, 0x67, 0x92, 0x56, 0x4C, 0x20, 0x6B, 0x42, 0x9D, 0xA7, 0x58,
+	0x0E, 0x52, 0x68, 0x95, 0x09, 0x7F, 0x59, 0x9C, 0x65, 0xB1, 0x64, 0x5E, 0x4F, 0xBA, 0x81, 0x1C,
+	0xC2, 0x0C, 0x02, 0xB4, 0x31, 0x5B, 0xFD, 0x1D, 0x0A, 0xC8, 0x19, 0x8F, 0x83, 0x8A, 0xCF, 0x33,
+	0x9E, 0x3A, 0x80, 0xF2, 0xF9, 0x76, 0x26, 0x44, 0xF1, 0xE2, 0xC4, 0xF5, 0xD6, 0x51, 0x46, 0x07,
+	0x14, 0x61, 0xF4, 0xC1, 0x24, 0x7A, 0x94, 0x27, 0x00, 0xFB, 0x04, 0xDF, 0x1F, 0x93, 0x71, 0x53,
+	0xEA, 0xD8, 0xBD, 0x3D, 0xD0, 0x79, 0xE6, 0x7E, 0x4E, 0x9A, 0xD7, 0x98, 0x1B, 0x05, 0xAE, 0x03,
+	0xC7, 0xBC, 0x86, 0xDB, 0x84, 0xE8, 0xD1, 0xF7, 0x16, 0x21, 0x6E, 0xE5, 0xCB, 0xA3, 0x1A, 0xEC,
+	0xA2, 0x7D, 0x18, 0x85, 0x48, 0xDA, 0xAA, 0xF0, 0x08, 0xC6, 0x40, 0xAD, 0x57, 0x0D, 0x29, 0x82,
+	0x7C, 0xE9, 0x8C, 0xFE, 0xDC, 0x0F, 0x2D, 0x3C, 0x2E, 0xF6, 0x15, 0x2F, 0xAF, 0xE1, 0xEB, 0x3F,
+	0x99, 0x43, 0x13, 0x0B, 0xE0, 0xA5, 0x12, 0x77, 0x5D, 0xB3, 0x38, 0xD9, 0xEF, 0x5A, 0x01, 0x70};
+
+uint64_t tea1_expand_iv(uint32_t dwShortIv)
+{
+	uint32_t dwXorred = dwShortIv ^ 0x96724FA1;
+	dwXorred = (dwXorred << 8) | (dwXorred >> 24);
+	uint64_t qwIv = ((uint64_t)dwShortIv << 32) | dwXorred;
+	return (qwIv >> 8) | (qwIv << 56);
+}
+
+uint8_t tea1_state_word_to_newbyte(uint16_t wSt, const uint16_t *awLut)
+{
+	uint8_t bSt0 = wSt;
+	uint8_t bSt1 = wSt >> 8;
+	uint8_t bDist;
+	uint8_t bOut = 0;
+
+	for (int i = 0; i < 8; i++) {
+		/* taps on bit 7,0 for bSt0 and bit 1,2 for bSt1 */
+		bDist = ((bSt0 >> 7) & 1) | ((bSt0 << 1) & 2) | ((bSt1 << 1) & 12);
+		if (awLut[i] & (1 << bDist))
+			bOut |= 1 << i;
+
+		/* rotate one position */
+		bSt0 = ((bSt0 >> 1) | (bSt0 << 7));
+		bSt1 = ((bSt1 >> 1) | (bSt1 << 7));
+	}
+
+	return bOut;
+}
+
+uint8_t tea1_reorder_state_byte(uint8_t bStByte)
+{
+	/* simple re-ordering of bits */
+	uint8_t bOut = 0;
+	bOut |= ((bStByte << 6) & 0x40);
+	bOut |= ((bStByte << 1) & 0x20);
+	bOut |= ((bStByte << 2) & 0x08);
+	bOut |= ((bStByte >> 3) & 0x14);
+	bOut |= ((bStByte >> 2) & 0x01);
+	bOut |= ((bStByte >> 5) & 0x02);
+	bOut |= ((bStByte << 4) & 0x80);
+	return bOut;
+}
+
+int32_t tea1_init_key_register(const uint8_t *lpKey)
+{
+	int32_t dwResult = 0;
+	for (int i = 0; i < 10; i++)
+		dwResult = (dwResult << 8) | g_abTea1Sbox[((dwResult >> 24) ^ lpKey[i] ^ dwResult) & 0xff];
+
+	return dwResult;
+}
+
+void tea1_inner(uint64_t qwIvReg, uint32_t dwKeyReg, uint32_t dwNumKsBytes, uint8_t *lpKsOut)
+{
+	uint32_t dwNumSkipRounds = 54;
+
+	for (int i = 0; i < dwNumKsBytes; i++) {
+		for (int j = 0; j < dwNumSkipRounds; j++) {
+			/* Step 1: Derive a non-linear feedback byte through sbox and feed back into key register */
+			uint8_t bSboxOut = g_abTea1Sbox[((dwKeyReg >> 24) ^ dwKeyReg) & 0xff];
+			dwKeyReg = (dwKeyReg << 8) | bSboxOut;
+
+			/* Step 2: Compute 3 bytes derived from current state */
+			uint8_t bDerivByte12 = tea1_state_word_to_newbyte((qwIvReg >>  8) & 0xffff, g_awTea1LutA);
+			uint8_t bDerivByte56 = tea1_state_word_to_newbyte((qwIvReg >> 40) & 0xffff, g_awTea1LutB);
+			uint8_t bReordByte4  = tea1_reorder_state_byte((qwIvReg >> 32) & 0xff);
+
+			/* Step 3: Combine current state with state derived values, and xor in key derived sbox output */
+			uint8_t bNewByte = (bDerivByte56 ^ (qwIvReg >> 56) ^ bReordByte4 ^ bSboxOut) & 0xff;
+			uint8_t bMixByte = bDerivByte12;
+
+			/* Step 4: Update lfsr: leftshift 8, feed/mix in previously generated bytes */
+			qwIvReg = ((qwIvReg << 8) ^ ((uint64_t)bMixByte << 32)) | bNewByte;
+		}
+
+		lpKsOut[i] = (qwIvReg >> 56);
+		dwNumSkipRounds = 19;
+	}
+}
+
+void tea1(uint32_t dwFrameNumbers, const uint8_t *lpKey, uint32_t dwNumKsBytes, uint8_t *lpKsOut)
+{
+	/* Initialize IV and key register */
+	uint64_t qwIvReg = tea1_expand_iv(dwFrameNumbers);
+	uint32_t dwKeyReg = tea1_init_key_register(lpKey);
+
+	/* Invoke actual TEA1 core function */
+	tea1_inner(qwIvReg, dwKeyReg, dwNumKsBytes, lpKsOut);
+}
diff --git a/src/crypto/tea1.h b/src/crypto/tea1.h
new file mode 100644
index 0000000..855b580
--- /dev/null
+++ b/src/crypto/tea1.h
@@ -0,0 +1,9 @@
+#ifndef HAVE_TEA1_H
+#define HAVE_TEA1_H
+
+#include <inttypes.h>
+
+
+void tea1(uint32_t dwFrameNumbers, const uint8_t *lpKey, uint32_t dwNumKsBytes, uint8_t *lpKsOut);
+
+#endif /* HAVE_TEA1_H */
diff --git a/src/crypto/tea2.c b/src/crypto/tea2.c
new file mode 100644
index 0000000..9b89d4e
--- /dev/null
+++ b/src/crypto/tea2.c
@@ -0,0 +1,130 @@
+/* TETRA TEA2 keystream generator implementation */
+/*
+ * Copyright (C) 2023 Midnight Blue B.V.
+ *
+ * Author: Wouter Bokslag <w.bokslag [ ] midnightblue [ ] nl>
+ *
+ * SPDX-License-Identifier: AGPL-3.0+
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero 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 Affero General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ * See the COPYING file in the main directory for details.
+ */
+
+#include <stdio.h>
+#include <inttypes.h>
+#include <string.h>
+
+#include "tea2.h"
+
+
+const uint16_t g_abTea2LutA[8] = { 0x2579, 0x86E5, 0xB6C8, 0x31D6, 0x7394, 0x934D, 0x638E, 0xC68B };
+const uint16_t g_abTea2LutB[8] = { 0xD68A, 0x97A1, 0xB2C9, 0x239E, 0x9C71, 0x36E8, 0xC9B2, 0x6CD1 };
+const uint8_t g_abTea2Sbox[256] = {
+	0x62, 0xDA, 0xFD, 0xB6, 0xBB, 0x9C, 0xD8, 0x2A, 0xAB, 0x28, 0x6E, 0x42, 0xE7, 0x1C, 0x78, 0x9E,
+	0xFC, 0xCA, 0x81, 0x8E, 0x32, 0x3B, 0xB4, 0xEF, 0x9F, 0x8B, 0xDB, 0x94, 0x0F, 0x9A, 0xA2, 0x96,
+	0x1B, 0x7A, 0xFF, 0xAA, 0xC5, 0xD6, 0xBC, 0x24, 0xDF, 0x44, 0x03, 0x09, 0x0B, 0x57, 0x90, 0xBA,
+	0x7F, 0x1F, 0xCF, 0x71, 0x98, 0x07, 0xF8, 0xA1, 0x60, 0xF7, 0x52, 0x8D, 0xE5, 0xD7, 0x69, 0x87,
+	0x14, 0xED, 0x92, 0xEB, 0xB3, 0x2F, 0xE9, 0x3D, 0xC6, 0x50, 0x5A, 0xA7, 0x45, 0x18, 0x11, 0xC4,
+	0xCE, 0xAC, 0xF4, 0x1D, 0x82, 0x54, 0x3E, 0x49, 0xD5, 0xEE, 0x84, 0x35, 0x41, 0x3A, 0xEC, 0x34,
+	0x17, 0xE0, 0xC9, 0xFE, 0xE8, 0xCB, 0xE6, 0xAE, 0x68, 0xE2, 0x6B, 0x46, 0xC8, 0x47, 0xB2, 0xE3,
+	0x97, 0x10, 0x0E, 0xB8, 0x76, 0x5B, 0xBE, 0xF5, 0xA6, 0x3C, 0x8F, 0xF6, 0xD1, 0xAF, 0xC0, 0x5E,
+	0x7E, 0xCD, 0x7C, 0x51, 0x6D, 0x74, 0x2C, 0x16, 0xF2, 0xA5, 0x65, 0x64, 0x58, 0x72, 0x1E, 0xF1,
+	0x04, 0xA8, 0x13, 0x53, 0x31, 0xB1, 0x20, 0xD3, 0x75, 0x5F, 0xA4, 0x56, 0x06, 0x8A, 0x8C, 0xD9,
+	0x70, 0x12, 0x29, 0x61, 0x4F, 0x4C, 0x15, 0x05, 0xD2, 0xBD, 0x7D, 0x9B, 0x99, 0x83, 0x2B, 0x25,
+	0xD0, 0x23, 0x48, 0x3F, 0xB0, 0x2E, 0x0D, 0x0C, 0xC7, 0xCC, 0xB7, 0x5C, 0xF0, 0xBF, 0x2D, 0x4E,
+	0x40, 0x39, 0x9D, 0x21, 0x37, 0x77, 0x73, 0x4B, 0x4D, 0x5D, 0xFA, 0xDE, 0x00, 0x80, 0x85, 0x6F,
+	0x22, 0x91, 0xDC, 0x26, 0x38, 0xE4, 0x4A, 0x79, 0x6A, 0x67, 0x93, 0xF3, 0xFB, 0x19, 0xA0, 0x7B,
+	0xF9, 0x95, 0x89, 0x66, 0xB9, 0xD4, 0xC1, 0xDD, 0x63, 0x33, 0xE1, 0xC3, 0xB5, 0xA3, 0xC2, 0x27,
+	0x0A, 0x88, 0xA9, 0x1A, 0x6C, 0x43, 0xEA, 0xAD, 0x30, 0x86, 0x36, 0x59, 0x08, 0x55, 0x01, 0x02
+};
+
+
+static uint64_t tea2_expand_iv(uint32_t dwFrameNumbers)
+{
+	uint32_t dwXorred = dwFrameNumbers ^ 0x5A6E3278;
+	dwXorred = (dwXorred << 8) | (dwXorred >> 24);
+	uint64_t qwIv = ((uint64_t)dwFrameNumbers << 32) | dwXorred;
+	return (qwIv >> 8) | (qwIv << 56);
+}
+
+static uint8_t tea2_state_word_to_newbyte(uint16_t wSt, const uint16_t *awLut)
+{
+
+	uint8_t bSt0 = wSt;
+	uint8_t bSt1 = wSt >> 8;
+	uint8_t bDist;
+	uint8_t bOut = 0;
+
+	for (int i = 0; i < 8; i++) {
+		/* taps on bit 0,2 for bSt0 and bit 0,7 for bSt1 */
+		bDist = ((bSt0 >> 1) & 0x1) | ((bSt0 >> 1) & 0x2) | ((bSt1 >> 5) & 0x4) | ((bSt1 << 3) & 0x8);
+		if (awLut[i] & (1 << bDist))
+			bOut |= 1 << i;
+
+		/* rotate one position */
+		bSt0 = ((bSt0 >> 1) | (bSt0 << 7));
+		bSt1 = ((bSt1 >> 1) | (bSt1 << 7));
+	}
+
+	return bOut;
+}
+
+static uint8_t tea2_reorder_state_byte(uint8_t bStByte)
+{
+	/* simple re-ordering of bits */
+	uint8_t bOut = 0;
+	bOut |= ((bStByte << 6) & 0x40);
+	bOut |= ((bStByte << 3) & 0x10);
+	bOut |= ((bStByte >> 2) & 0x01);
+	bOut |= ((bStByte << 2) & 0x20);
+	bOut |= ((bStByte << 3) & 0x80);
+	bOut |= ((bStByte >> 4) & 0x02);
+	bOut |= ((bStByte >> 3) & 0x08);
+	bOut |= ((bStByte >> 5) & 0x04);
+	return bOut;
+}
+
+void tea2(uint32_t dwFrameNumbers, uint8_t *lpKey, uint32_t dwNumKsBytes, uint8_t *lpKsOut)
+{
+	uint8_t abKeyReg[10];
+	uint32_t dwNumSkipRounds = 51;
+
+	/* init registers */
+	uint64_t qwIvReg = tea2_expand_iv(dwFrameNumbers);
+	memcpy(abKeyReg, lpKey, 10);
+
+	for (int i = 0; i < dwNumKsBytes; i++) {
+		for (int j = 0; j < dwNumSkipRounds; j++) {
+			/* Step 1: Derive a non-linear feedback byte through sbox and feed back into key register */
+			uint8_t bSboxOut = g_abTea2Sbox[abKeyReg[0] ^ abKeyReg[7]];
+			memmove(abKeyReg, abKeyReg + 1, 9);
+			abKeyReg[9] = bSboxOut;
+
+			/* Step 2: Compute 3 bytes derived from current state */
+			uint8_t bDerivByte01 = tea2_state_word_to_newbyte((qwIvReg >>  0) & 0xffff, g_abTea2LutA);
+			uint8_t bDerivByte34 = tea2_state_word_to_newbyte((qwIvReg >> 24) & 0xffff, g_abTea2LutB);
+			uint8_t bReordByte5  = tea2_reorder_state_byte((qwIvReg >> 40) & 0xff);
+
+			/* Step 3: Combine current state with state derived values, and xor in key derived sbox output */
+			uint8_t bNewByte = ((qwIvReg >> 56) ^ (qwIvReg >> 16) ^ bReordByte5 ^ bDerivByte01 ^ bSboxOut) & 0xff;
+			uint8_t bMixByte = bDerivByte34;
+
+			/* Step 4: Update lfsr: leftshift 8, feed/mix in previously generated bytes */
+			qwIvReg = ((qwIvReg << 8) ^ ((uint64_t)bMixByte << 24)) | bNewByte;
+		}
+
+		lpKsOut[i] = qwIvReg >> 56;
+		dwNumSkipRounds = 19;
+	}
+}
diff --git a/src/crypto/tea2.h b/src/crypto/tea2.h
new file mode 100644
index 0000000..3a52d52
--- /dev/null
+++ b/src/crypto/tea2.h
@@ -0,0 +1,8 @@
+#ifndef HAVE_TEA2_H
+#define HAVE_TEA2_H
+
+#include <inttypes.h>
+
+void tea2(uint32_t dwFrameNumbers, uint8_t *lpKey, uint32_t dwNumKsBytes, uint8_t *lpKsOut);
+
+#endif /* HAVE_TEA2_H */
diff --git a/src/crypto/tea3.c b/src/crypto/tea3.c
new file mode 100644
index 0000000..cf5e9b4
--- /dev/null
+++ b/src/crypto/tea3.c
@@ -0,0 +1,127 @@
+/* TETRA TEA3 keystream generator implementation */
+/*
+ * Copyright (C) 2023 Midnight Blue B.V.
+ *
+ * Author: Wouter Bokslag <w.bokslag [ ] midnightblue [ ] nl>
+ *
+ * SPDX-License-Identifier: AGPL-3.0+
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero 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 Affero General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ * See the COPYING file in the main directory for details.
+ */
+
+#include <stdio.h>
+#include <inttypes.h>
+#include <string.h>
+
+#include "tea3.h"
+
+
+const uint16_t g_awTea3LutA[8] = { 0x92A7, 0xA761, 0x974C, 0x6B8C, 0x29CE, 0x176C, 0x39D4, 0x7463 };
+const uint16_t g_awTea3LutB[8] = { 0x9D58, 0xA46D, 0x176C, 0x79C4, 0xC62B, 0xB2C9, 0x4D93, 0x2E93 };
+const uint8_t g_abTea3Sbox[256] = {
+	0x7D, 0xBF, 0x7B, 0x92, 0xAE, 0x7C, 0xF2, 0x10, 0x5A, 0x0F, 0x61, 0x7A, 0x98, 0x76, 0x07, 0x64,
+	0xEE, 0x89, 0xF7, 0xBA, 0xC2, 0x02, 0x0D, 0xE8, 0x56, 0x2E, 0xCA, 0x58, 0xC0, 0xFA, 0x2A, 0x01,
+	0x57, 0x6E, 0x3F, 0x4B, 0x9C, 0xDA, 0xA6, 0x5B, 0x41, 0x26, 0x50, 0x24, 0x3E, 0xF8, 0x0A, 0x86,
+	0xB6, 0x5C, 0x34, 0xE9, 0x06, 0x88, 0x1F, 0x39, 0x33, 0xDF, 0xD9, 0x78, 0xD8, 0xA8, 0x51, 0xB2,
+	0x09, 0xCD, 0xA1, 0xDD, 0x8E, 0x62, 0x69, 0x4D, 0x23, 0x2B, 0xA9, 0xE1, 0x53, 0x94, 0x90, 0x1E,
+	0xB4, 0x3B, 0xF9, 0x4E, 0x36, 0xFE, 0xB5, 0xD1, 0xA2, 0x8D, 0x66, 0xCE, 0xB7, 0xC4, 0x60, 0xED,
+	0x96, 0x4F, 0x31, 0x79, 0x35, 0xEB, 0x8F, 0xBB, 0x54, 0x14, 0xCB, 0xDE, 0x6B, 0x2D, 0x19, 0x82,
+	0x80, 0xAC, 0x17, 0x05, 0xFF, 0xA4, 0xCF, 0xC6, 0x6F, 0x65, 0xE6, 0x74, 0xC8, 0x93, 0xF4, 0x7E,
+	0xF3, 0x43, 0x9F, 0x71, 0xAB, 0x9A, 0x0B, 0x87, 0x55, 0x70, 0x0C, 0xAD, 0xCC, 0xA5, 0x44, 0xE7,
+	0x46, 0x45, 0x03, 0x30, 0x1A, 0xEA, 0x67, 0x99, 0xDB, 0x4A, 0x42, 0xD7, 0xAA, 0xE4, 0xC2, 0xD5,
+	0xF0, 0x77, 0x20, 0xC3, 0x3C, 0x16, 0xB9, 0xE2, 0xEF, 0x6C, 0x3D, 0x1B, 0x22, 0x84, 0x2F, 0x81,
+	0x1D, 0xB1, 0x3A, 0xE5, 0x73, 0x40, 0xD0, 0x18, 0xC7, 0x6A, 0x9E, 0x91, 0x48, 0x27, 0x95, 0x72,
+	0x68, 0x0E, 0x00, 0xFC, 0xC5, 0x5F, 0xF1, 0xF5, 0x38, 0x11, 0x7F, 0xE3, 0x5E, 0x13, 0xAF, 0x37,
+	0xE0, 0x8A, 0x49, 0x1C, 0x21, 0x47, 0xD4, 0xDC, 0xB0, 0xEC, 0x83, 0x28, 0xB8, 0xF6, 0xA7, 0xC9,
+	0x63, 0x59, 0xBD, 0x32, 0x85, 0x08, 0xBE, 0xD3, 0xFD, 0x4C, 0x2C, 0xFB, 0xA0, 0xC1, 0x9D, 0xB3,
+	0x52, 0x8C, 0x5D, 0x29, 0x6D, 0x04, 0xBC, 0x25, 0x15, 0x8B, 0x12, 0x9B, 0xD6, 0x75, 0xA3, 0x97
+};
+
+
+static uint64_t tea3_compute_iv(uint32_t dwFrameNumbers)
+{
+	uint32_t dwXorred = dwFrameNumbers ^ 0xC43A7D51;
+	dwXorred = (dwXorred << 8) | (dwXorred >> 24);
+	uint64_t qwIv = ((uint64_t)dwFrameNumbers << 32) | dwXorred;
+	return (qwIv >> 8) | (qwIv << 56);
+}
+
+static uint8_t tea3_state_word_to_newbyte(uint16_t wSt, const uint16_t *awLut)
+{
+	uint8_t bSt0 = wSt;
+	uint8_t bSt1 = wSt >> 8;
+
+	uint8_t bDist;
+	uint8_t bOut = 0;
+
+	for (int i = 0; i < 8; i++) {
+		/* taps on bit 5,6 for bSt0 and bit 5,6 for bSt1 */
+		bDist = ((bSt0 >> 5) & 3) | ((bSt1 >> 3) & 12);
+		if (awLut[i] & (1 << bDist))
+			bOut |= 1 << i;
+
+		/* rotate one position */
+		bSt0 = ((bSt0 >> 1) | (bSt0 << 7));
+		bSt1 = ((bSt1 >> 1) | (bSt1 << 7));
+	}
+
+	return bOut;
+}
+
+static uint8_t tea3_reorder_state_byte(uint8_t bStByte)
+{
+	/* simple re-ordering of bits */
+	uint8_t bOut = 0;
+	bOut |= ((bStByte << 6) & 0x40);
+	bOut |= ((bStByte << 1) & 0x20);
+	bOut |= ((bStByte << 2) & 0x98);
+	bOut |= ((bStByte >> 4) & 0x04);
+	bOut |= ((bStByte >> 3) & 0x01);
+	bOut |= ((bStByte >> 6) & 0x02);
+	return bOut;
+}
+
+void tea3(uint32_t dwFrameNumbers, uint8_t *lpKey, uint32_t dwNumKsBytes, uint8_t *lpKsOut)
+{
+	uint8_t abKeyReg[10];
+	uint32_t dwNumSkipRounds = 51;
+
+	/* init registers */
+	uint64_t qwIvReg = tea3_compute_iv(dwFrameNumbers);
+	memcpy(abKeyReg, lpKey, 10);
+
+	for (int i = 0; i < dwNumKsBytes; i++) {
+		for (int j = 0; j < dwNumSkipRounds; j++) {
+			/* Step 1: Derive a non-linear feedback byte through sbox and feed back into key register */
+			uint8_t bSboxOut = g_abTea3Sbox[abKeyReg[7] ^ abKeyReg[2]] ^ abKeyReg[0];
+			memmove(abKeyReg, abKeyReg + 1, 9);
+			abKeyReg[9] = bSboxOut;
+
+			/* Step 2: Compute 3 bytes derived from current state */
+			uint8_t bDerivByte12 = tea3_state_word_to_newbyte((qwIvReg >>  8) & 0xffff, g_awTea3LutA);
+			uint8_t bDerivByte56 = tea3_state_word_to_newbyte((qwIvReg >> 40) & 0xffff, g_awTea3LutB);
+			uint8_t bReordByte4  = tea3_reorder_state_byte((qwIvReg >> 32) & 0xff);
+
+			/* Step 3: Combine current state with state derived values, and xor in key derived sbox output */
+			uint8_t bNewByte = ((qwIvReg >> 56) ^ bReordByte4 ^ bDerivByte12 ^ bSboxOut) & 0xff;
+			uint8_t bMixByte = bDerivByte56;
+
+			/* Step 4: Update lfsr: leftshift 8, feed/mix in previously generated bytes */
+			qwIvReg = ((qwIvReg << 8) ^ ((uint64_t)bMixByte << 40)) | bNewByte;
+		}
+		lpKsOut[i] = (qwIvReg >> 56);
+		dwNumSkipRounds = 19;
+	}
+}
diff --git a/src/crypto/tea3.h b/src/crypto/tea3.h
new file mode 100644
index 0000000..4a6c7a4
--- /dev/null
+++ b/src/crypto/tea3.h
@@ -0,0 +1,8 @@
+#ifndef HAVE_TEA3_H
+#define HAVE_TEA3_H
+
+#include <inttypes.h>
+
+void tea3(uint32_t dwFrameNumbers, uint8_t *lpKey, uint32_t dwNumKsBytes, uint8_t *lpKsOut);
+
+#endif /* HAVE_TEA3_H */
diff --git a/src/crypto/tetra_crypto.c b/src/crypto/tetra_crypto.c
index d7df293..3230cb8 100644
--- a/src/crypto/tetra_crypto.c
+++ b/src/crypto/tetra_crypto.c
@@ -33,6 +33,11 @@
 #include <phy/tetra_burst.h>
 
 #include "tetra_crypto.h"
+#include "tea1.h"
+#include "tea2.h"
+#include "tea3.h"
+#include "taa1.h"
+
 
 struct tetra_crypto_database _tcdb, *tcdb = &_tcdb;
 
@@ -169,23 +174,30 @@ static bool generate_keystream(struct tetra_crypto_state *tcs, struct tetra_key
 	uint8_t cn[2] = {(tcs->cn >> 8) & 0xFF, tcs->cn & 0xFF};
 	uint8_t la[2] = {(tcs->la >> 8) & 0xFF, tcs->la & 0xFF};
 	uint8_t cc[1] = {tcs->cc & 0xFF};
-	/* TODO FIXME add call to TB5 to derive eck */
+	tb5(cn, la, cc, key->key, eck);
 
 	/* Generate keystream with required KSG */
 	switch (key->network_info->ksg_type) {
-	/* TODO FIXME add cases/calls to TEA keystream generator functions */
+	case KSG_TEA1:
+		tea1(iv, eck, num_bytes, ks_bytes);
+		break;
+
+	case KSG_TEA2:
+		tea2(iv, eck, num_bytes, ks_bytes);
+		break;
+
+	case KSG_TEA3:
+		tea3(iv, eck, num_bytes, ks_bytes);
+			break;
+
 	default:
 		fprintf(stderr, "tetra_crypto: KSG type %d not supported\n", key->network_info->ksg_type);
 		return false;
 	}
 
-	for (int i = 0; i < num_bytes; i++)
-		for (int j = 0; j < 8; j++)
-			ks_out[i * 8 + j] = (ks_bytes[i] >> (7-j)) & 1;
-
 	/* Expand keystream bytes into ubit format */
 	for (int i = 0; i < num_bits; i++)
-		ks_out[i] = (ks_bytes[num_bits / 8] >> (7-(num_bits % 8))) & 1;
+		ks_out[i] = (ks_bytes[i / 8] >> (7-(i % 8))) & 1;
 
 	return true;
 }
@@ -277,12 +289,12 @@ int load_keystore(char *tetra_keyfile)
 	 *
 	 *   network mcc 123 mnc 456 ksg_type 1 security_class 2
 	 *   - ksg_type: decimal, see enum tetra_ksg_type
-	 *   - security_class: 2 for SCK, 3 for CCK (and DCK per ISSI)
+	 *   - security_class: 2 for SCK, 3 for CCK+DCK
 	 *
 	 *   key mcc 123 mnc 456 addr 00000000 key_type 1 key_num 002 key 1234deadbeefcafebabe
-	 *   - addr: decimal, leave zero for key type 1 (CCK/SCK)
+	 *   - addr: decimal, only relevant for DCK/MGCK/GCK, also, currently unimplemented
 	 *   - key_type: 1 CCK/SCK, 2 DCK, 3 MGCK, 4 GCK
-	 *   - key_num: SCK_VN or group key number depending on type
+	 *   - key_num: SCK_VN or group key number depending on type, currently unimplemented
 	 *   - key: 80-bit key hex string
 	 */