diff --git a/include/net/bluetooth/mgmt.h b/include/net/bluetooth/mgmt.h index 4899286ed4e4..45bea25d737f 100644 --- a/include/net/bluetooth/mgmt.h +++ b/include/net/bluetooth/mgmt.h @@ -199,6 +199,16 @@ struct mgmt_cp_remove_remote_oob_data { #define MGMT_OP_STOP_DISCOVERY 0x001C +#define MGMT_OP_BLOCK_DEVICE 0x001D +struct mgmt_cp_block_device { + bdaddr_t bdaddr; +} __packed; + +#define MGMT_OP_UNBLOCK_DEVICE 0x001E +struct mgmt_cp_unblock_device { + bdaddr_t bdaddr; +} __packed; + #define MGMT_EV_CMD_COMPLETE 0x0001 struct mgmt_ev_cmd_complete { __le16 opcode; diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c index d19208903be4..64c0418a6221 100644 --- a/net/bluetooth/mgmt.c +++ b/net/bluetooth/mgmt.c @@ -1666,6 +1666,70 @@ failed: return err; } +static int block_device(struct sock *sk, u16 index, unsigned char *data, + u16 len) +{ + struct hci_dev *hdev; + struct mgmt_cp_block_device *cp; + int err; + + BT_DBG("hci%u", index); + + cp = (void *) data; + + if (len != sizeof(*cp)) + return cmd_status(sk, index, MGMT_OP_BLOCK_DEVICE, + EINVAL); + + hdev = hci_dev_get(index); + if (!hdev) + return cmd_status(sk, index, MGMT_OP_BLOCK_DEVICE, + ENODEV); + + err = hci_blacklist_add(hdev, &cp->bdaddr); + + if (err < 0) + err = cmd_status(sk, index, MGMT_OP_BLOCK_DEVICE, -err); + else + err = cmd_complete(sk, index, MGMT_OP_BLOCK_DEVICE, + NULL, 0); + hci_dev_put(hdev); + + return err; +} + +static int unblock_device(struct sock *sk, u16 index, unsigned char *data, + u16 len) +{ + struct hci_dev *hdev; + struct mgmt_cp_unblock_device *cp; + int err; + + BT_DBG("hci%u", index); + + cp = (void *) data; + + if (len != sizeof(*cp)) + return cmd_status(sk, index, MGMT_OP_UNBLOCK_DEVICE, + EINVAL); + + hdev = hci_dev_get(index); + if (!hdev) + return cmd_status(sk, index, MGMT_OP_UNBLOCK_DEVICE, + ENODEV); + + err = hci_blacklist_del(hdev, &cp->bdaddr); + + if (err < 0) + err = cmd_status(sk, index, MGMT_OP_UNBLOCK_DEVICE, -err); + else + err = cmd_complete(sk, index, MGMT_OP_UNBLOCK_DEVICE, + NULL, 0); + hci_dev_put(hdev); + + return err; +} + int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen) { unsigned char *buf; @@ -1780,6 +1844,12 @@ int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen) case MGMT_OP_STOP_DISCOVERY: err = stop_discovery(sk, index); break; + case MGMT_OP_BLOCK_DEVICE: + err = block_device(sk, index, buf + sizeof(*hdr), len); + break; + case MGMT_OP_UNBLOCK_DEVICE: + err = unblock_device(sk, index, buf + sizeof(*hdr), len); + break; default: BT_DBG("Unknown op %u", opcode); err = cmd_status(sk, index, opcode, 0x01);