From 30ac69eaea08e5e29d330eae9b3efa272a3dbadd Mon Sep 17 00:00:00 2001
From: Johan Hedberg <johan.hedberg@intel.com>
Date: Tue, 27 May 2014 12:33:52 +0300
Subject: [PATCH] shared/crypto: Add bt_crypto_f4 for LE SC

---
 src/shared/crypto.c | 51 +++++++++++++++++++++++++++++++++++++++++++++
 src/shared/crypto.h |  2 ++
 2 files changed, 53 insertions(+)

diff --git a/src/shared/crypto.c b/src/shared/crypto.c
index c438ab352..5f40710e0 100644
--- a/src/shared/crypto.c
+++ b/src/shared/crypto.c
@@ -66,6 +66,9 @@ struct af_alg_iv {
 #define SOL_ALG		279
 #endif
 
+/* Maximum message length that can be passed to aes_cmac */
+#define CMAC_MSG_MAX	80
+
 struct bt_crypto {
 	int ref_count;
 	int ecb_aes;
@@ -560,3 +563,51 @@ bool bt_crypto_s1(struct bt_crypto *crypto, const uint8_t k[16],
 
 	return bt_crypto_e(crypto, k, res, res);
 }
+
+static bool aes_cmac(struct bt_crypto *crypto, uint8_t key[16], uint8_t *msg,
+					size_t msg_len, uint8_t res[16])
+{
+	uint8_t key_msb[16], out[16], msg_msb[CMAC_MSG_MAX];
+	ssize_t len;
+	int fd;
+
+	if (msg_len > CMAC_MSG_MAX)
+		return false;
+
+	swap_buf(key, key_msb, 16);
+	fd = alg_new(crypto->cmac_aes, key_msb, 16);
+	if (fd < 0)
+		return false;
+
+	swap_buf(msg, msg_msb, msg_len);
+	len = send(fd, msg_msb, msg_len, 0);
+	if (len < 0) {
+		close(fd);
+		return false;
+	}
+
+	len = read(fd, out, 16);
+	if (len < 0) {
+		close(fd);
+		return false;
+	}
+
+	swap_buf(out, res, 16);
+
+	return true;
+}
+
+bool bt_crypto_f4(struct bt_crypto *crypto, uint8_t u[32], uint8_t v[32],
+				uint8_t x[16], uint8_t z, uint8_t res[16])
+{
+	uint8_t m[65];
+
+	if (!crypto)
+		return false;
+
+	m[0] = z;
+	memcpy(&m[1], v, 32);
+	memcpy(&m[33], u, 32);
+
+	return aes_cmac(crypto, x, m, sizeof(m), res);
+}
diff --git a/src/shared/crypto.h b/src/shared/crypto.h
index b10f3ff51..5dc8a7e64 100644
--- a/src/shared/crypto.h
+++ b/src/shared/crypto.h
@@ -46,6 +46,8 @@ bool bt_crypto_c1(struct bt_crypto *crypto, const uint8_t k[16],
 bool bt_crypto_s1(struct bt_crypto *crypto, const uint8_t k[16],
 			const uint8_t r1[16], const uint8_t r2[16],
 			uint8_t res[16]);
+bool bt_crypto_f4(struct bt_crypto *crypto, uint8_t u[32], uint8_t v[32],
+				uint8_t x[16], uint8_t z, uint8_t res[16]);
 bool bt_crypto_sign_att(struct bt_crypto *crypto, const uint8_t key[16],
 				const uint8_t *m, uint16_t m_len,
 				uint32_t sign_cnt, uint8_t signature[12]);
-- 
2.47.3