3 files changed, 48 insertions, 0 deletions

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index f57854f0786d..763b4635c304 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -32,6 +32,8 @@ void amp_ctrl_list_flush(struct amp_mgr *mgr);
 struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
                              u8 remote_id);
 
+int phylink_gen_key(struct hci_conn *hcon, u8 *data, u8 *len, u8 *type);
+
 void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr);
 void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle);
 void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr);
diff --git a/net/bluetooth/Kconfig b/net/bluetooth/Kconfig
index 3537d385035e..1c11d0dcd863 100644
--- a/net/bluetooth/Kconfig
+++ b/net/bluetooth/Kconfig
@@ -11,6 +11,7 @@ menuconfig BT
         select CRYPTO_BLKCIPHER
         select CRYPTO_AES
         select CRYPTO_ECB
+        select CRYPTO_SHA256
         help
           Bluetooth is low-cost, low-power, short-range wireless technology.
           It was designed as a replacement for cables and other short-range
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index ea4d5ffc1151..67bc2c2f8783 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -161,6 +161,51 @@ static int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
         return ret;
 }
 
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type)
+{
+        struct hci_dev *hdev = conn->hdev;
+        struct link_key *key;
+        u8 keybuf[HCI_AMP_LINK_KEY_SIZE];
+        u8 gamp_key[HCI_AMP_LINK_KEY_SIZE];
+        int err;
+
+        if (!hci_conn_check_link_mode(conn))
+                return -EACCES;
+
+        BT_DBG("conn %p key_type %d", conn, conn->key_type);
+
+        /* Legacy key */
+        if (conn->key_type < 3) {
+                BT_ERR("Legacy key type %d", conn->key_type);
+                return -EACCES;
+        }
+
+        *type = conn->key_type;
+        *len = HCI_AMP_LINK_KEY_SIZE;
+
+        key = hci_find_link_key(hdev, &conn->dst);
+
+        /* BR/EDR Link Key concatenated together with itself */
+        memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE);
+        memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE);
+
+        /* Derive Generic AMP Link Key (gamp) */
+        err = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4, gamp_key);
+        if (err) {
+                BT_ERR("Could not derive Generic AMP Key: err %d", err);
+                return err;
+        }
+
+        if (conn->key_type == HCI_LK_DEBUG_COMBINATION) {
+                BT_DBG("Use Generic AMP Key (gamp)");
+                memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE);
+                return err;
+        }
+
+        /* Derive Dedicated AMP Link Key: "802b" is 802.11 PAL keyID */
+        return hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4, data);
+}
+
 void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
 {
         struct hci_cp_read_local_amp_assoc cp;