From 419bd56a4f3e143056935a6c9845ce988fd29d61 Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Thu, 25 Sep 2014 17:00:13 +0200 Subject: [PATCH] shared: Add helper functions for HCI based crypto --- Makefile.am | 1 + src/shared/hci-crypto.c | 178 ++++++++++++++++++++++++++++++++++++++++ src/shared/hci-crypto.h | 45 ++++++++++ 3 files changed, 224 insertions(+) create mode 100644 src/shared/hci-crypto.c create mode 100644 src/shared/hci-crypto.h diff --git a/Makefile.am b/Makefile.am index f0080b21e..ebd5b4477 100644 --- a/Makefile.am +++ b/Makefile.am @@ -105,6 +105,7 @@ shared_sources = src/shared/io.h src/shared/timeout.h \ src/shared/ringbuf.h src/shared/ringbuf.c \ src/shared/tester.h src/shared/tester.c \ src/shared/hci.h src/shared/hci.c \ + src/shared/hci-crypto.h src/shared/hci-crypto.c \ src/shared/hfp.h src/shared/hfp.c \ src/shared/uhid.h src/shared/uhid.c \ src/shared/pcap.h src/shared/pcap.c \ diff --git a/src/shared/hci-crypto.c b/src/shared/hci-crypto.c new file mode 100644 index 000000000..8a40aa5a9 --- /dev/null +++ b/src/shared/hci-crypto.c @@ -0,0 +1,178 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2013-2014 Intel Corporation + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include + +#include "monitor/bt.h" +#include "src/shared/util.h" +#include "src/shared/hci.h" +#include "src/shared/hci-crypto.h" + +struct crypto_data { + uint8_t size; + bt_hci_crypto_func_t callback; + void *user_data; +}; + +static void le_encrypt_callback(const void *response, uint8_t size, + void *user_data) +{ + struct crypto_data *data = user_data; + const struct bt_hci_rsp_le_encrypt *rsp = response; + + if (rsp->status) { + data->callback(NULL, 0, data->user_data); + return; + } + + data->callback(rsp->data, data->size, data->user_data); +} + +static bool le_encrypt(struct bt_hci *hci, uint8_t size, + const uint8_t key[16], const uint8_t plaintext[16], + bt_hci_crypto_func_t callback, void *user_data) +{ + struct crypto_data *data; + struct bt_hci_cmd_le_encrypt cmd; + + if (!callback || !size || size > 16) + return false; + + memcpy(cmd.key, key, 16); + memcpy(cmd.plaintext, plaintext, 16); + + data = new0(struct crypto_data, 1); + if (!data) + return false; + + data->size = size; + data->callback = callback; + data->user_data = user_data; + + if (!bt_hci_send(hci, BT_HCI_CMD_LE_ENCRYPT, &cmd, sizeof(cmd), + le_encrypt_callback, data, free)) { + free(data); + return false; + } + + return true; +} + +static void prand_callback(const void *response, uint8_t size, + void *user_data) +{ + struct crypto_data *data = user_data; + const struct bt_hci_rsp_le_rand *rsp = response; + uint8_t prand[3]; + + if (rsp->status) { + data->callback(NULL, 0, data->user_data); + return; + } + + prand[0] = (rsp->number & 0xff0000) >> 16; + prand[1] = (rsp->number & 0x00ff00) >> 8; + prand[2] = (rsp->number & 0x00003f) | 0x40; + + data->callback(prand, 3, data->user_data); +} + +bool bt_hci_crypto_prand(struct bt_hci *hci, + bt_hci_crypto_func_t callback, void *user_data) +{ + struct crypto_data *data; + + if (!callback) + return false; + + data = new0(struct crypto_data, 1); + if (!data) + return false; + + data->callback = callback; + data->user_data = user_data; + + if (!bt_hci_send(hci, BT_HCI_CMD_LE_RAND, NULL, 0, + prand_callback, data, free)) { + free(data); + return false; + } + + return true; +} + +bool bt_hci_crypto_e(struct bt_hci *hci, + const uint8_t key[16], const uint8_t plaintext[16], + bt_hci_crypto_func_t callback, void *user_data) +{ + return le_encrypt(hci, 16, key, plaintext, callback, user_data); +} + +bool bt_hci_crypto_d1(struct bt_hci *hci, + const uint8_t k[16], uint16_t d, uint16_t r, + bt_hci_crypto_func_t callback, void *user_data) +{ + uint8_t dp[16]; + + /* d' = padding || r || d */ + dp[0] = d & 0xff; + dp[1] = d >> 8; + dp[2] = r & 0xff; + dp[3] = r >> 8; + memset(dp + 4, 0, 12); + + /* d1(k, d, r) = e(k, d') */ + return le_encrypt(hci, 16, k, dp, callback, user_data); +} + +bool bt_hci_crypto_dm(struct bt_hci *hci, + const uint8_t k[16], const uint8_t r[8], + bt_hci_crypto_func_t callback, void *user_data) +{ + uint8_t rp[16]; + + /* r' = padding || r */ + memcpy(rp, r, 8); + memset(rp + 8, 0, 8); + + /* dm(k, r) = e(k, r') mod 2^16 */ + return le_encrypt(hci, 8, k, rp, callback, user_data); +} + +bool bt_hci_crypto_ah(struct bt_hci *hci, + const uint8_t k[16], const uint8_t r[3], + bt_hci_crypto_func_t callback, void *user_data) +{ + uint8_t rp[16]; + + /* r' = padding || r */ + memcpy(rp, r, 3); + memset(rp + 3, 0, 13); + + /* ah(k, r) = e(k, r') mod 2^24 */ + return le_encrypt(hci, 3, k, rp, callback, user_data); +} diff --git a/src/shared/hci-crypto.h b/src/shared/hci-crypto.h new file mode 100644 index 000000000..b090c2410 --- /dev/null +++ b/src/shared/hci-crypto.h @@ -0,0 +1,45 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2013-2014 Intel Corporation + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#include +#include + +struct bt_hci; + +typedef void (*bt_hci_crypto_func_t)(const void *data, uint8_t size, + void *user_data); + +bool bt_hci_crypto_prand(struct bt_hci *hci, + bt_hci_crypto_func_t callback, void *user_data); +bool bt_hci_crypto_e(struct bt_hci *hci, + const uint8_t key[16], const uint8_t plaintext[16], + bt_hci_crypto_func_t callback, void *user_data); +bool bt_hci_crypto_d1(struct bt_hci *hci, + const uint8_t k[16], uint16_t d, uint16_t r, + bt_hci_crypto_func_t callback, void *user_data); +bool bt_hci_crypto_dm(struct bt_hci *hci, + const uint8_t k[16], const uint8_t r[8], + bt_hci_crypto_func_t callback, void *user_data); +bool bt_hci_crypto_ah(struct bt_hci *hci, + const uint8_t k[16], const uint8_t r[3], + bt_hci_crypto_func_t callback, void *user_data); -- 2.47.3