6 files changed, 300 insertions, 0 deletions

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 9ce46cd00ba2..08fbf1253b83 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -82,6 +82,8 @@ enum {
         HCI_MGMT,
         HCI_PAIRABLE,
         HCI_SERVICE_CACHE,
+        HCI_LINK_KEYS,
+        HCI_DEBUG_KEYS,
 };
 
 /* HCI ioctl defines */
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index e62da084e01d..009fa63a9048 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -73,6 +73,14 @@ struct bt_uuid {
         u8 svc_hint;
 };
 
+struct link_key {
+        struct list_head list;
+        bdaddr_t bdaddr;
+        u8 type;
+        u8 val[16];
+        u8 pin_len;
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
         struct list_head list;
@@ -153,6 +161,8 @@ struct hci_dev {
 
         struct list_head        uuids;
 
+        struct list_head        link_keys;
+
         struct hci_dev_stats    stat;
 
         struct sk_buff_head     driver_init;
@@ -461,6 +471,12 @@ int hci_blacklist_clear(struct hci_dev *hdev);
 
 int hci_uuids_clear(struct hci_dev *hdev);
 
+int hci_link_keys_clear(struct hci_dev *hdev);
+struct link_key *hci_find_link_key(struct hci_dev *hdev, bdaddr_t *bdaddr);
+int hci_add_link_key(struct hci_dev *hdev, int new_key, bdaddr_t *bdaddr,
+                                                u8 *key, u8 type, u8 pin_len);
+int hci_remove_link_key(struct hci_dev *hdev, bdaddr_t *bdaddr);
+
 void hci_del_off_timer(struct hci_dev *hdev);
 
 void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb);
@@ -697,6 +713,7 @@ int mgmt_index_removed(u16 index);
 int mgmt_powered(u16 index, u8 powered);
 int mgmt_discoverable(u16 index, u8 discoverable);
 int mgmt_connectable(u16 index, u8 connectable);
+int mgmt_new_key(u16 index, struct link_key *key, u8 old_key_type);
 
 /* HCI info for socket */
 #define hci_pi(sk) ((struct hci_pinfo *) sk)
diff --git a/include/net/bluetooth/mgmt.h b/include/net/bluetooth/mgmt.h
index b092c4c014eb..56b500a2f68c 100644
--- a/include/net/bluetooth/mgmt.h
+++ b/include/net/bluetooth/mgmt.h
@@ -98,6 +98,28 @@ struct mgmt_cp_set_service_cache {
         __u8 enable;
 } __packed;
 
+struct mgmt_key_info {
+        bdaddr_t bdaddr;
+        u8 type;
+        u8 val[16];
+        u8 pin_len;
+} __packed;
+
+#define MGMT_OP_LOAD_KEYS               0x000D
+struct mgmt_cp_load_keys {
+        __le16 index;
+        __u8 debug_keys;
+        __le16 key_count;
+        struct mgmt_key_info keys[0];
+} __packed;
+
+#define MGMT_OP_REMOVE_KEY              0x000E
+struct mgmt_cp_remove_key {
+        __le16 index;
+        bdaddr_t bdaddr;
+        __u8 disconnect;
+} __packed;
+
 #define MGMT_EV_CMD_COMPLETE            0x0001
 struct mgmt_ev_cmd_complete {
         __le16 opcode;
@@ -133,3 +155,10 @@ struct mgmt_ev_index_removed {
 #define MGMT_EV_CONNECTABLE             0x0008
 
 #define MGMT_EV_PAIRABLE                0x0009
+
+#define MGMT_EV_NEW_KEY                 0x000A
+struct mgmt_ev_new_key {
+        __le16 index;
+        struct mgmt_key_info key;
+        __u8 old_key_type;
+} __packed;
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 748f5a65caf4..8ca8cf147058 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -970,6 +970,88 @@ int hci_uuids_clear(struct hci_dev *hdev)
         return 0;
 }
 
+int hci_link_keys_clear(struct hci_dev *hdev)
+{
+        struct list_head *p, *n;
+
+        list_for_each_safe(p, n, &hdev->link_keys) {
+                struct link_key *key;
+
+                key = list_entry(p, struct link_key, list);
+
+                list_del(p);
+                kfree(key);
+        }
+
+        return 0;
+}
+
+struct link_key *hci_find_link_key(struct hci_dev *hdev, bdaddr_t *bdaddr)
+{
+        struct list_head *p;
+
+        list_for_each(p, &hdev->link_keys) {
+                struct link_key *k;
+
+                k = list_entry(p, struct link_key, list);
+
+                if (bacmp(bdaddr, &k->bdaddr) == 0)
+                        return k;
+        }
+
+        return NULL;
+}
+
+int hci_add_link_key(struct hci_dev *hdev, int new_key, bdaddr_t *bdaddr,
+                                                u8 *val, u8 type, u8 pin_len)
+{
+        struct link_key *key, *old_key;
+        u8 old_key_type;
+
+        old_key = hci_find_link_key(hdev, bdaddr);
+        if (old_key) {
+                old_key_type = old_key->type;
+                key = old_key;
+        } else {
+                old_key_type = 0xff;
+                key = kzalloc(sizeof(*key), GFP_ATOMIC);
+                if (!key)
+                        return -ENOMEM;
+                list_add(&key->list, &hdev->link_keys);
+        }
+
+        BT_DBG("%s key for %s type %u", hdev->name, batostr(bdaddr), type);
+
+        bacpy(&key->bdaddr, bdaddr);
+        memcpy(key->val, val, 16);
+        key->type = type;
+        key->pin_len = pin_len;
+
+        if (new_key)
+                mgmt_new_key(hdev->id, key, old_key_type);
+
+        if (type == 0x06)
+                key->type = old_key_type;
+
+        return 0;
+}
+
+int hci_remove_link_key(struct hci_dev *hdev, bdaddr_t *bdaddr)
+{
+        struct link_key *key;
+
+        key = hci_find_link_key(hdev, bdaddr);
+        if (!key)
+                return -ENOENT;
+
+        BT_DBG("%s removing %s", hdev->name, batostr(bdaddr));
+
+        list_del(&key->list);
+        kfree(key);
+
+        return 0;
+}
+
 /* Register HCI device */
 int hci_register_dev(struct hci_dev *hdev)
 {
@@ -1029,6 +1111,8 @@ int hci_register_dev(struct hci_dev *hdev)
 
         INIT_LIST_HEAD(&hdev->uuids);
 
+        INIT_LIST_HEAD(&hdev->link_keys);
+
         INIT_WORK(&hdev->power_on, hci_power_on);
         INIT_WORK(&hdev->power_off, hci_power_off);
         setup_timer(&hdev->off_timer, hci_auto_off, (unsigned long) hdev);
@@ -1105,6 +1189,7 @@ int hci_unregister_dev(struct hci_dev *hdev)
         hci_dev_lock_bh(hdev);
         hci_blacklist_clear(hdev);
         hci_uuids_clear(hdev);
+        hci_link_keys_clear(hdev);
         hci_dev_unlock_bh(hdev);
 
         __hci_dev_put(hdev);
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index c69ee44d5bd7..80ffd3a901fc 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1810,13 +1810,60 @@ static inline void hci_pin_code_request_evt(struct hci_dev *hdev, struct sk_buff
 
 static inline void hci_link_key_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
+        struct hci_ev_link_key_req *ev = (void *) skb->data;
+        struct hci_cp_link_key_reply cp;
+        struct hci_conn *conn;
+        struct link_key *key;
+
         BT_DBG("%s", hdev->name);
+
+        if (!test_bit(HCI_LINK_KEYS, &hdev->flags))
+                return;
+
+        hci_dev_lock(hdev);
+
+        key = hci_find_link_key(hdev, &ev->bdaddr);
+        if (!key) {
+                BT_DBG("%s link key not found for %s", hdev->name,
+                                                        batostr(&ev->bdaddr));
+                goto not_found;
+        }
+
+        BT_DBG("%s found key type %u for %s", hdev->name, key->type,
+                                                        batostr(&ev->bdaddr));
+
+        if (!test_bit(HCI_DEBUG_KEYS, &hdev->flags) && key->type == 0x03) {
+                BT_DBG("%s ignoring debug key", hdev->name);
+                goto not_found;
+        }
+
+        conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr);
+
+        if (key->type == 0x04 && conn && conn->auth_type != 0xff &&
+                                                (conn->auth_type & 0x01)) {
+                BT_DBG("%s ignoring unauthenticated key", hdev->name);
+                goto not_found;
+        }
+
+        bacpy(&cp.bdaddr, &ev->bdaddr);
+        memcpy(cp.link_key, key->val, 16);
+
+        hci_send_cmd(hdev, HCI_OP_LINK_KEY_REPLY, sizeof(cp), &cp);
+
+        hci_dev_unlock(hdev);
+
+        return;
+
+not_found:
+        hci_send_cmd(hdev, HCI_OP_LINK_KEY_NEG_REPLY, 6, &ev->bdaddr);
+        hci_dev_unlock(hdev);
 }
 
 static inline void hci_link_key_notify_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_link_key_notify *ev = (void *) skb->data;
         struct hci_conn *conn;
+        u8 pin_len = 0;
 
         BT_DBG("%s", hdev->name);
 
@@ -1829,6 +1876,10 @@ static inline void hci_link_key_notify_evt(struct hci_dev *hdev, struct sk_buff
                 hci_conn_put(conn);
         }
 
+        if (test_bit(HCI_LINK_KEYS, &hdev->flags))
+                hci_add_link_key(hdev, 1, &ev->bdaddr, ev->link_key,
+                                                        ev->key_type, pin_len);
+
         hci_dev_unlock(hdev);
 }
 
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index a08f4ce03182..bdb0e85f182e 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -794,6 +794,99 @@ static int set_service_cache(struct sock *sk, unsigned char *data, u16 len)
         return err;
 }
 
+static int load_keys(struct sock *sk, unsigned char *data, u16 len)
+{
+        struct hci_dev *hdev;
+        struct mgmt_cp_load_keys *cp;
+        u16 dev_id, key_count, expected_len;
+        int i;
+
+        cp = (void *) data;
+        dev_id = get_unaligned_le16(&cp->index);
+        key_count = get_unaligned_le16(&cp->key_count);
+
+        expected_len = sizeof(*cp) + key_count * sizeof(struct mgmt_key_info);
+        if (expected_len != len) {
+                BT_ERR("load_keys: expected %u bytes, got %u bytes",
+                                                        len, expected_len);
+                return -EINVAL;
+        }
+
+        hdev = hci_dev_get(dev_id);
+        if (!hdev)
+                return cmd_status(sk, MGMT_OP_LOAD_KEYS, ENODEV);
+
+        BT_DBG("hci%u debug_keys %u key_count %u", dev_id, cp->debug_keys,
+                                                                key_count);
+
+        hci_dev_lock_bh(hdev);
+
+        hci_link_keys_clear(hdev);
+
+        set_bit(HCI_LINK_KEYS, &hdev->flags);
+
+        if (cp->debug_keys)
+                set_bit(HCI_DEBUG_KEYS, &hdev->flags);
+        else
+                clear_bit(HCI_DEBUG_KEYS, &hdev->flags);
+
+        for (i = 0; i < key_count; i++) {
+                struct mgmt_key_info *key = &cp->keys[i];
+
+                hci_add_link_key(hdev, 0, &key->bdaddr, key->val, key->type,
+                                                                key->pin_len);
+        }
+
+        hci_dev_unlock_bh(hdev);
+        hci_dev_put(hdev);
+
+        return 0;
+}
+
+static int remove_key(struct sock *sk, unsigned char *data, u16 len)
+{
+        struct hci_dev *hdev;
+        struct mgmt_cp_remove_key *cp;
+        struct hci_conn *conn;
+        u16 dev_id;
+        int err;
+
+        cp = (void *) data;
+        dev_id = get_unaligned_le16(&cp->index);
+
+        hdev = hci_dev_get(dev_id);
+        if (!hdev)
+                return cmd_status(sk, MGMT_OP_REMOVE_KEY, ENODEV);
+
+        hci_dev_lock_bh(hdev);
+
+        err = hci_remove_link_key(hdev, &cp->bdaddr);
+        if (err < 0) {
+                err = cmd_status(sk, MGMT_OP_REMOVE_KEY, -err);
+                goto unlock;
+        }
+
+        err = 0;
+
+        if (!test_bit(HCI_UP, &hdev->flags) || !cp->disconnect)
+                goto unlock;
+
+        conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->bdaddr);
+        if (conn) {
+                struct hci_cp_disconnect dc;
+
+                put_unaligned_le16(conn->handle, &dc.handle);
+                dc.reason = 0x13; /* Remote User Terminated Connection */
+                err = hci_send_cmd(hdev, HCI_OP_DISCONNECT, 0, NULL);
+        }
+
+unlock:
+        hci_dev_unlock_bh(hdev);
+        hci_dev_put(hdev);
+
+        return err;
+}
+
 int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
 {
         unsigned char *buf;
@@ -858,6 +951,12 @@ int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
         case MGMT_OP_SET_SERVICE_CACHE:
                 err = set_service_cache(sk, buf + sizeof(*hdr), len);
                 break;
+        case MGMT_OP_LOAD_KEYS:
+                err = load_keys(sk, buf + sizeof(*hdr), len);
+                break;
+        case MGMT_OP_REMOVE_KEY:
+                err = remove_key(sk, buf + sizeof(*hdr), len);
+                break;
         default:
                 BT_DBG("Unknown op %u", opcode);
                 err = cmd_status(sk, opcode, 0x01);
@@ -974,3 +1073,20 @@ int mgmt_connectable(u16 index, u8 connectable)
 
         return ret;
 }
+
+int mgmt_new_key(u16 index, struct link_key *key, u8 old_key_type)
+{
+        struct mgmt_ev_new_key ev;
+
+        memset(&ev, 0, sizeof(ev));
+
+        put_unaligned_le16(index, &ev.index);
+
+        bacpy(&ev.key.bdaddr, &key->bdaddr);
+        ev.key.type = key->type;
+        memcpy(ev.key.val, key->val, 16);
+        ev.key.pin_len = key->pin_len;
+        ev.old_key_type = old_key_type;
+
+        return mgmt_event(MGMT_EV_NEW_KEY, &ev, sizeof(ev), NULL);
+}