src/shared/hci-crypto.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2013-2014  Intel Corporation
   6  *
   7  *
   8  *  This library is free software; you can redistribute it and/or
   9  *  modify it under the terms of the GNU Lesser General Public
  10  *  License as published by the Free Software Foundation; either
  11  *  version 2.1 of the License, or (at your option) any later version.
  12  *
  13  *  This library is distributed in the hope that it will be useful,
  14  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16  *  Lesser General Public License for more details.
  17  *
  18  *  You should have received a copy of the GNU Lesser General Public
  19  *  License along with this library; if not, write to the Free Software
  20  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  21  *
  22  */
  23
  24 #ifdef HAVE_CONFIG_H
  25 #include <config.h>
  26 #endif
  27
  28 #include <string.h>
  29
  30 #include "monitor/bt.h"
  31 #include "src/shared/util.h"
  32 #include "src/shared/hci.h"
  33 #include "src/shared/hci-crypto.h"
  34
  35 struct crypto_data {
  36         uint8_t size;
  37         bt_hci_crypto_func_t callback;
  38         void *user_data;
  39 };
  40
  41 static void le_encrypt_callback(const void *response, uint8_t size,
  42                                                         void *user_data)
  43 {
  44         struct crypto_data *data = user_data;
  45         const struct bt_hci_rsp_le_encrypt *rsp = response;
  46
  47         if (rsp->status) {
  48                 data->callback(NULL, 0, data->user_data);
  49                 return;
  50         }
  51
  52         data->callback(rsp->data, data->size, data->user_data);
  53 }
  54
  55 static bool le_encrypt(struct bt_hci *hci, uint8_t size,
  56                         const uint8_t key[16], const uint8_t plaintext[16],
  57                         bt_hci_crypto_func_t callback, void *user_data)
  58 {
  59         struct crypto_data *data;
  60         struct bt_hci_cmd_le_encrypt cmd;
  61
  62         if (!callback || !size || size > 16)
  63                 return false;
  64
  65         memcpy(cmd.key, key, 16);
  66         memcpy(cmd.plaintext, plaintext, 16);
  67
  68         data = new0(struct crypto_data, 1);
  69         data->size = size;
  70         data->callback = callback;
  71         data->user_data = user_data;
  72
  73         if (!bt_hci_send(hci, BT_HCI_CMD_LE_ENCRYPT, &cmd, sizeof(cmd),
  74                                         le_encrypt_callback, data, free)) {
  75                 free(data);
  76                 return false;
  77         }
  78
  79         return true;
  80 }
  81
  82 static void prand_callback(const void *response, uint8_t size,
  83                                                         void *user_data)
  84 {
  85         struct crypto_data *data = user_data;
  86         const struct bt_hci_rsp_le_rand *rsp = response;
  87         uint8_t prand[3];
  88
  89         if (rsp->status) {
  90                 data->callback(NULL, 0, data->user_data);
  91                 return;
  92         }
  93
  94         prand[0] = (rsp->number & 0xff0000) >> 16;
  95         prand[1] = (rsp->number & 0x00ff00) >> 8;
  96         prand[2] = (rsp->number & 0x00003f) | 0x40;
  97
  98         data->callback(prand, 3, data->user_data);
  99 }
 100
 101 bool bt_hci_crypto_prand(struct bt_hci *hci,
 102                         bt_hci_crypto_func_t callback, void *user_data)
 103 {
 104         struct crypto_data *data;
 105
 106         if (!callback)
 107                 return false;
 108
 109         data = new0(struct crypto_data, 1);
 110         data->callback = callback;
 111         data->user_data = user_data;
 112
 113         if (!bt_hci_send(hci, BT_HCI_CMD_LE_RAND, NULL, 0,
 114                                         prand_callback, data, free)) {
 115                 free(data);
 116                 return false;
 117         }
 118
 119         return true;
 120 }
 121
 122 bool bt_hci_crypto_e(struct bt_hci *hci,
 123                         const uint8_t key[16], const uint8_t plaintext[16],
 124                         bt_hci_crypto_func_t callback, void *user_data)
 125 {
 126         return le_encrypt(hci, 16, key, plaintext, callback, user_data);
 127 }
 128
 129 bool bt_hci_crypto_d1(struct bt_hci *hci,
 130                         const uint8_t k[16], uint16_t d, uint16_t r,
 131                         bt_hci_crypto_func_t callback, void *user_data)
 132 {
 133         uint8_t dp[16];
 134
 135         /* d' = padding || r || d */
 136         dp[0] = d & 0xff;
 137         dp[1] = d >> 8;
 138         dp[2] = r & 0xff;
 139         dp[3] = r >> 8;
 140         memset(dp + 4, 0, 12);
 141
 142         /* d1(k, d, r) = e(k, d') */
 143         return le_encrypt(hci, 16, k, dp, callback, user_data);
 144 }
 145
 146 bool bt_hci_crypto_dm(struct bt_hci *hci,
 147                         const uint8_t k[16], const uint8_t r[8],
 148                         bt_hci_crypto_func_t callback, void *user_data)
 149 {
 150         uint8_t rp[16];
 151
 152         /* r' = padding || r */
 153         memcpy(rp, r, 8);
 154         memset(rp + 8, 0, 8);
 155
 156         /* dm(k, r) = e(k, r') mod 2^16 */
 157         return le_encrypt(hci, 8, k, rp, callback, user_data);
 158 }
 159
 160 bool bt_hci_crypto_ah(struct bt_hci *hci,
 161                         const uint8_t k[16], const uint8_t r[3],
 162                         bt_hci_crypto_func_t callback, void *user_data)
 163 {
 164         uint8_t rp[16];
 165
 166         /* r' = padding || r */
 167         memcpy(rp, r, 3);
 168         memset(rp + 3, 0, 13);
 169
 170         /* ah(k, r) = e(k, r') mod 2^24 */
 171         return le_encrypt(hci, 3, k, rp, callback, user_data);
 172 }