Add A5/3-4 cipher support

diff --git a/include/Makefile.am b/include/Makefile.am
index 74396de..f7d77c0 100644
--- a/include/Makefile.am
+++ b/include/Makefile.am
@@ -110,7 +110,8 @@  endif
 
 noinst_HEADERS = \
 	osmocom/core/timer_compat.h \
-	osmocom/gsm/kasumi.h
+	osmocom/gsm/kasumi.h \
+	osmocom/gsm/a34.h
 
 osmocom/core/bit%gen.h: osmocom/core/bitXXgen.h.tpl
 	$(AM_V_GEN)$(MKDIR_P) $(dir $@)
diff --git a/include/osmocom/gsm/a34.h b/include/osmocom/gsm/a34.h
new file mode 100644
index 0000000..39e32db
--- /dev/null
+++ b/include/osmocom/gsm/a34.h
@@ -0,0 +1,45 @@ 
+/*
+ * a34.h
+ *
+ * Copyright (C) 2014  Max Suraev <Max.Suraev@fairwaves.co>
+ *
+ * All Rights Reserved
+ *
+ * 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 2 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, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*! \defgroup a5 GSM A5/3-4 ciphering algorithms
+ *  @{
+ */
+
+/*! \file gsm/a34.h
+ *  \brief Osmocom implementation of GSM A5/3 and A5/4 ciphering algorithms
+ */
+
+	/* Notes:
+	 *  - key must be 8/16 bytes long depending on algorithm variant
+	 *  - the dl and ul pointer must be either NULL or 114 bits long
+	 *  - fn is the _real_ GSM frame number (unless fn_correct is false),
+	 *    converted internally to fn_count
+	 */
+void _a5_3(const uint8_t *key, uint32_t fn, ubit_t *dl, ubit_t *ul, bool fn_correct);
+void _a5_4(const uint8_t *key, uint32_t fn, ubit_t *dl, ubit_t *ul, bool fn_correct);
+
+/*! @} */
diff --git a/src/gsm/a5.c b/src/gsm/a5.c
index de821e8..fc5e64f 100644
--- a/src/gsm/a5.c
+++ b/src/gsm/a5.c
@@ -36,8 +36,11 @@ 
 
 #include <errno.h>
 #include <string.h>
+#include <stdbool.h>
 
 #include <osmocom/gsm/a5.h>
+#include <osmocom/gsm/a34.h>
+#include <osmocom/gsm/kasumi.h>
 
 /*! \brief Main method to generate a A5/x cipher stream
  *  \param[in] n Which A5/x method to use
@@ -47,7 +50,7 @@ 
  *  \param[out] ul Pointer to array of ubits to return Uplink cipher stream
  *  \returns 0 for success, -ENOTSUP for invalid cipher selection.
  *
- * Currently A5/[0-2] are supported.
+ * Currently A5/[0-4] are supported.
  * Either (or both) of dl/ul can be NULL if not needed.
  */
 int
@@ -70,8 +73,16 @@  osmo_a5(int n, const uint8_t *key, uint32_t fn, ubit_t *dl, ubit_t *ul)
 		osmo_a5_2(key, fn, dl, ul);
 		break;
 
+	case 3:
+		_a5_3(key, fn, dl, ul, true);
+		break;
+
+	case 4:
+		_a5_4(key, fn, dl, ul, true);
+		break;
+
 	default:
-		/* a5/[3..7] not supported here/yet */
+		/* a5/[5..7] not supported here/yet */
 		return -ENOTSUP;
 	}
 
@@ -368,4 +379,59 @@  osmo_a5_2(const uint8_t *key, uint32_t fn, ubit_t *dl, ubit_t *ul)
 	}
 }
 
+/* ------------------------------------------------------------------------ */
+/* A5/3&4                                                                   */
+/* ------------------------------------------------------------------------ */
+
+/*! \brief Generate a GSM A5/4 cipher stream
+ *  \param[in] key 16 byte array for the key (as received from the SIM)
+ *  \param[in] fn Frame number
+ *  \param[out] dl Pointer to array of ubits to return Downlink cipher stream
+ *  \param[out] ul Pointer to array of ubits to return Uplink cipher stream
+ *  \param[in] fn_correct true if fn is a real GSM frame number and thus requires internal conversion
+ *
+ * Either (or both) of dl/ul should be NULL if not needed.
+ *
+ * Implementation based on specifications from 3GPP TS 55.216, 3GPP TR 55.919 and ETSI TS 135 202
+ * with slight simplifications (CE hardcoded to 0).
+ */
+void
+_a5_4(const uint8_t *ck, uint32_t fn, ubit_t *dl, ubit_t *ul, bool fn_correct)
+{
+	uint8_t i, gamma[32], uplink[15];
+	uint32_t fn_count = (fn_correct) ? osmo_a5_fn_count(fn) : fn;
+
+	if (ul) {
+		_kasumi_kgcore(0xF, 0, fn_count, 0, ck, gamma, 228);
+		for(i = 0; i < 15; i++) uplink[i] = (gamma[i + 14] << 2) + (gamma[i + 15] >> 6);
+		osmo_pbit2ubit(ul, uplink, 114);
+	}
+	if (dl) {
+		_kasumi_kgcore(0xF, 0, fn_count, 0, ck, gamma, 114);
+		osmo_pbit2ubit(dl, gamma, 114);
+	}
+}
+
+/*! \brief Generate a GSM A5/3 cipher stream
+ *  \param[in] key 8 byte array for the key (as received from the SIM)
+ *  \param[in] fn Frame number
+ *  \param[out] dl Pointer to array of ubits to return Downlink cipher stream
+ *  \param[out] ul Pointer to array of ubits to return Uplink cipher stream
+ *  \param[in] fn_correct true if fn is a real GSM frame number and thus requires internal conversion
+ *
+ * Either (or both) of dl/ul should be NULL if not needed.
+ *
+ * Implementation based on specifications from 3GPP TS 55.216, 3GPP TR 55.919 and ETSI TS 135 202
+ * with slight simplifications (CE hardcoded to 0).
+ */
+void
+_a5_3(const uint8_t *key, uint32_t fn, ubit_t *dl, ubit_t *ul, bool fn_correct)
+{
+	uint8_t ck[16];
+	memcpy(ck, key, 8);
+	memcpy(ck + 8, key, 8);
+	/* internal function require 128 bit key so we expand by concatenating supplied 64 bit key */
+	_a5_4(ck, fn, dl, ul, fn_correct);
+}
+
 /*! @} */
diff --git a/tests/Makefile.am b/tests/Makefile.am
index ea4bf9c..baf11a2 100644
--- a/tests/Makefile.am
+++ b/tests/Makefile.am
@@ -18,6 +18,7 @@  utils_utils_test_LDADD = $(top_builddir)/src/libosmocore.la
 
 a5_a5_test_SOURCES = a5/a5_test.c
 a5_a5_test_LDADD = $(top_builddir)/src/libosmocore.la $(top_builddir)/src/gsm/libosmogsm.la
+a5_a5_test_LDFLAGS = -static
 
 kasumi_kasumi_test_SOURCES = kasumi/kasumi_test.c
 kasumi_kasumi_test_LDADD = $(top_builddir)/src/libosmocore.la $(top_builddir)/src/gsm/libosmogsm.la
diff --git a/tests/a5/a5_test.c b/tests/a5/a5_test.c
index 14436f1..a40489e 100644
--- a/tests/a5/a5_test.c
+++ b/tests/a5/a5_test.c
@@ -2,10 +2,12 @@ 
 #include <stdlib.h>
 #include <stdint.h>
 #include <string.h>
+#include <stdbool.h>
 
 #include <osmocom/core/bits.h>
 #include <osmocom/core/utils.h>
 #include <osmocom/gsm/a5.h>
+#include <osmocom/gsm/a34.h>
 
 static const uint8_t key[] = { 0x01, 0x23, 0x45, 0x67, 0x89, 0xab, 0xcd, 0xef };
 static const uint32_t fn = 123456;
@@ -36,24 +38,48 @@  static const uint8_t ul[] = {
 	0x80, 0xba, 0xab, 0xc0, 0x59, 0x26, 0x40,
 };
 
-static const char *
-binstr(ubit_t *d, int n)
+static inline bool print_a5(int n, int k, const char * dir, const ubit_t * out, const char * block)
 {
-	static char str[256];
-	int i;
+	uint8_t len = 114 / 8 + 1, buf[len], res[len];
+	printf("A5/%d - %s: %s => ", n, dir, osmo_ubit_dump(out, 114));
+	osmo_hexparse(block, res, len);
+	osmo_ubit2pbit(buf, out, 114);
+	if (0 != memcmp(buf, res, len)) {
+		printf("FAIL\nGOT: [%d] %s\nEXP: [%d] %s\n", k, osmo_hexdump_nospc(buf, len), k, osmo_hexdump_nospc(res, len));
+		return false;
+	}
+	printf("OK\n");
+	return true;
+}
+
+static inline bool test_a53(const char * kc, uint32_t count, const char * block1, const char * block2)
+{
+	ubit_t dlout[114], ulout[114];
+	uint8_t key[8];
+
+	osmo_hexparse(kc, key, 8);
+	_a5_3(key, count, dlout, NULL, false);
+	_a5_3(key, count, NULL, ulout, false);
 
-	for (i=0; i<n; i++)
-		str[i] = d[i] ? '1' : '0';
+	return print_a5(3, 8, "DL", dlout, block1) & print_a5(3, 8, "UL", ulout, block2);
+}
+
+static inline bool test_a54(const char * kc, uint32_t count, const char * block1, const char * block2)
+{
+	ubit_t dlout[114], ulout[114];
+	uint8_t key[16];
 
-	str[i] = '\0';
+	osmo_hexparse(kc, key, 16);
+	_a5_4(key, count, dlout, NULL, false);
+	_a5_4(key, count, NULL, ulout, false);
 
-	return str;
+	return print_a5(4, 8, "DL", dlout, block1) & print_a5(4, 8, "UL", ulout, block2);
 }
 
+
 int main(int argc, char **argv)
 {
-	ubit_t exp[114];
-	ubit_t out[114];
+	ubit_t exp[114], out[114];
 	int n, i;
 
 	for (n=0; n<3; n++) {
@@ -66,13 +92,13 @@  int main(int argc, char **argv)
 
 		osmo_a5(n, key, fn, out, NULL);
 
-		printf("A5/%d - DL: %s", n, binstr(out, 114));
+		printf("A5/%d - DL: %s", n, osmo_ubit_dump(out, 114));
 
 		if (!memcmp(exp, out, 114))
 			printf(" => OK\n");
 		else {
 			printf(" => BAD\n");
-			printf(" Expected: %s", binstr(out, 114));
+			printf(" Expected: %s", osmo_ubit_dump(out, 114));
 			fprintf(stderr, "[!] A5/%d DL failed", n);
 			exit(1);
 		}
@@ -82,17 +108,31 @@  int main(int argc, char **argv)
 
 		osmo_a5(n, key, fn, NULL, out);
 
-		printf("A5/%d - UL: %s", n, binstr(out, 114));
+		printf("A5/%d - UL: %s", n, osmo_ubit_dump(out, 114));
 
 		if (!memcmp(exp, out, 114))
 			printf(" => OK\n");
 		else {
 			printf(" => BAD\n");
-			printf(" Expected: %s", binstr(out, 114));
+			printf(" Expected: %s", osmo_ubit_dump(out, 114));
 			fprintf(stderr, "[!] A5/%d UL failed", n);
 			exit(1);
 		}
 	}
 
+// test vectors from 3GPP TS 55.217 and TS 55.218
+	test_a53("2BD6459F82C5BC00", 0x24F20F, "889EEAAF9ED1BA1ABBD8436232E440", "5CA3406AA244CF69CF047AADA2DF40");
+	test_a53("952C49104881FF48", 0x061272, "FB4D5FBCEE13A33389285686E9A5C0", "25090378E0540457C57E367662E440");
+	test_a53("EFA8B2229E720C2A", 0x33FD3F, "0E4015755A336469C3DD8680E30340", "6F10669E2B4E18B042431A28E47F80");
+	test_a53("952C49104881FF48", 0x061527, "AB7DB38A573A325DAA76E4CB800A40", "4C4B594FEA9D00FE8978B7B7BC1080");
+	test_a53("3451F23A43BD2C87", 0x0E418C, "75F7C4C51560905DFBA05E46FB54C0", "192C95353CDF979E054186DF15BF00");
+	test_a53("CAA2639BE82435CF", 0x2FF229, "301437E4D4D6565D4904C631606EC0", "F0A3B8795E264D3E1A82F684353DC0");
+	test_a53("7AE67E87400B9FA6", 0x2F24E5, "F794290FEF643D2EA348A7796A2100", "CB6FA6C6B8A705AF9FEFE975818500");
+	test_a53("58AF69935540698B", 0x05446B, "749CA4E6B691E5A598C461D5FE4740", "31C9E444CD04677ADAA8A082ADBC40");
+	test_a53("017F81E5F236FE62", 0x156B26, "2A6976761E60CC4E8F9F52160276C0", "A544D8475F2C78C35614128F1179C0");
+	test_a53("1ACA8B448B767B39", 0x0BC3B5, "A4F70DC5A2C9707F5FA1C60EB10640", "7780B597B328C1400B5C74823E8500");
+	test_a54("3D43C388C9581E337FF1F97EB5C1F85E", 0x35D2CF, "A2FE3034B6B22CC4E33C7090BEC340", "170D7497432FF897B91BE8AECBA880");
+	test_a54("A4496A64DF4F399F3B4506814A3E07A1", 0x212777, "89CDEE360DF9110281BCF57755A040", "33822C0C779598C9CBFC49183AF7C0");
+
 	return 0;
 }
diff --git a/tests/a5/a5_test.ok b/tests/a5/a5_test.ok
index 4497e14..cefcdb6 100644
--- a/tests/a5/a5_test.ok
+++ b/tests/a5/a5_test.ok
@@ -4,3 +4,27 @@  A5/1 - DL: 110010111010001001010101011101100001011101011101001110110001110001111
 A5/1 - UL: 110110010000001101011110000011110010101011101100000100111001101000000101110101001010100001111011101100010110010010 => OK
 A5/2 - DL: 010001011001110010001000110000111000001010110111111111111011001110011000110100101111100101101110000011110001010010 => OK
 A5/2 - UL: 111100000011101010101100110111101110001101011011010111100110010110000000101110101010101111000000010110010010011001 => OK
+A5/3 - DL: 100010001001111011101010101011111001111011010001101110100001101010111011110110000100001101100010001100101110010001 => OK
+A5/3 - UL: 010111001010001101000000011010101010001001000100110011110110100111001111000001000111101010101101101000101101111101 => OK
+A5/3 - DL: 111110110100110101011111101111001110111000010011101000110011001110001001001010000101011010000110111010011010010111 => OK
+A5/3 - UL: 001001010000100100000011011110001110000001010100000001000101011111000101011111100011011001110110011000101110010001 => OK
+A5/3 - DL: 000011100100000000010101011101010101101000110011011001000110100111000011110111011000011010000000111000110000001101 => OK
+A5/3 - UL: 011011110001000001100110100111100010101101001110000110001011000001000010010000110001101000101000111001000111111110 => OK
+A5/3 - DL: 101010110111110110110011100010100101011100111010001100100101110110101010011101101110010011001011100000000000101001 => OK
+A5/3 - UL: 010011000100101101011001010011111110101010011101000000001111111010001001011110001011011110110111101111000001000010 => OK
+A5/3 - DL: 011101011111011111000100110001010001010101100000100100000101110111111011101000000101111001000110111110110101010011 => OK
+A5/3 - UL: 000110010010110010010101001101010011110011011111100101111001111000000101010000011000011011011111000101011011111100 => OK
+A5/3 - DL: 001100000001010000110111111001001101010011010110010101100101110101001001000001001100011000110001011000000110111011 => OK
+A5/3 - UL: 111100001010001110111000011110010101111000100110010011010011111000011010100000101111011010000100001101010011110111 => OK
+A5/3 - DL: 111101111001010000101001000011111110111101100100001111010010111010100011010010001010011101111001011010100010000100 => OK
+A5/3 - UL: 110010110110111110100110110001101011100010100111000001011010111110011111111011111110100101110101100000011000010100 => OK
+A5/3 - DL: 011101001001110010100100111001101011011010010001111001011010010110011000110001000110000111010101111111100100011101 => OK
+A5/3 - UL: 001100011100100111100100010001001100110100000100011001110111101011011010101010001010000010000010101011011011110001 => OK
+A5/3 - DL: 001010100110100101110110011101100001111001100000110011000100111010001111100111110101001000010110000000100111011011 => OK
+A5/3 - UL: 101001010100010011011000010001110101111100101100011110001100001101010110000101000001001010001111000100010111100111 => OK
+A5/3 - DL: 101001001111011100001101110001011010001011001001011100000111111101011111101000011100011000001110101100010000011001 => OK
+A5/3 - UL: 011101111000000010110101100101111011001100101000110000010100000000001011010111000111010010000010001111101000010100 => OK
+A5/4 - DL: 101000101111111000110000001101001011011010110010001011001100010011100011001111000111000010010000101111101100001101 => OK
+A5/4 - UL: 000101110000110101110100100101110100001100101111111110001001011110111001000110111110100010101110110010111010100010 => OK
+A5/4 - DL: 100010011100110111101110001101100000110111111001000100010000001010000001101111001111010101110111010101011010000001 => OK
+A5/4 - UL: 001100111000001000101100000011000111011110010101100110001100100111001011111111000100100100011000001110101111011111 => OK