From 0ddd89caf1b3a98a0d547855c9c5c42b34223d24 Mon Sep 17 00:00:00 2001 From: Sudha Bheemanna Date: Wed, 24 Aug 2016 17:21:15 +0530 Subject: [PATCH] Bluetooth: Add RSSI Monitor feature Added feature support for monitoring the RSSI value. Commands and events for enabling, disabling and setting rssi threshold values are added. Change-Id: I850643a9228afc017e54217a11826b9c6a68a96b Signed-off-by: Sudha Bheemanna Signed-off-by: Seung-Woo Kim Signed-off-by: Amit Purwar --- include/net/bluetooth/hci.h | 54 +++ include/net/bluetooth/hci_core.h | 80 +++++ include/net/bluetooth/mgmt_tizen.h | 63 ++++ net/bluetooth/hci_event.c | 32 ++ net/bluetooth/mgmt.c | 686 +++++++++++++++++++++++++++++++++++++ 5 files changed, 915 insertions(+) diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h index 47a8194..a5c140e 100644 --- a/include/net/bluetooth/hci.h +++ b/include/net/bluetooth/hci.h @@ -1674,6 +1674,60 @@ struct hci_cp_le_set_adv_set_rand_addr { bdaddr_t bdaddr; } __packed; +#ifdef TIZEN_BT +/* + * Vendor Specific HCI Command + * Vendor: Broadcom + * Purpose: This HCI is used to enable RSSI monitoring and setting + * Threshold Values for LE Link + */ +#define HCI_OP_ENABLE_RSSI 0xfce9 + +struct hci_cp_set_enable_rssi { + __u8 hci_le_ext_opcode; + __u8 le_enable_cs_Features; + __u8 data[3]; +} __packed; + +struct hci_cp_set_rssi_threshold { + __u8 hci_le_ext_opcode; + __u8 mode; + __le16 conn_handle; + __u8 alert_mask; + __u8 low_th; + __u8 in_range_th; + __u8 high_th; +} __packed; + +struct hci_cc_rsp_enable_rssi { + __u8 status; + __u8 le_ext_opcode; +} __packed; + +struct hci_ev_vendor_specific_rssi_alert { + __le16 conn_handle; + __s8 alert_type; + __s8 rssi_dbm; +} __packed; + +/* + * Vendor Specific HCI Command + * Vendor: Broadcom + * Purpose: This HCI is used to get Raw RSSI value for a Link + */ +#define HCI_OP_GET_RAW_RSSI 0xfc48 + +struct hci_cp_get_raw_rssi { + __le16 conn_handle; +} __packed; + +struct hci_cc_rp_get_raw_rssi { + __u8 status; + __le16 conn_handle; + __s8 rssi_dbm; +} __packed; +#endif + /* ---- HCI Events ---- */ #define HCI_EV_INQUIRY_COMPLETE 0x01 diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h index 29522e6..33a567d 100644 --- a/include/net/bluetooth/hci_core.h +++ b/include/net/bluetooth/hci_core.h @@ -517,6 +517,9 @@ struct hci_conn { void *sco_data; struct amp_mgr *amp_mgr; +#ifdef TIZEN_BT + bool rssi_monitored; +#endif struct hci_conn *link; void (*connect_cfm_cb) (struct hci_conn *conn, u8 status); @@ -895,6 +898,70 @@ static inline struct hci_conn *hci_lookup_le_connect(struct hci_dev *hdev) return NULL; } +#ifdef TIZEN_BT +static inline bool hci_conn_rssi_state_set(struct hci_dev *hdev, + __u8 type, bdaddr_t *ba, bool value) +{ + struct hci_conn_hash *h = &hdev->conn_hash; + struct hci_conn *c; + __u8 conn_type; + + if (type == 0x01) + conn_type = LE_LINK; + else + conn_type = ACL_LINK; + + rcu_read_lock(); + + list_for_each_entry_rcu(c, &h->list, list) { + if (c->type == conn_type && !bacmp(&c->dst, ba)) { + c->rssi_monitored = value; + rcu_read_unlock(); + return true; + } + } + + rcu_read_unlock(); + return false; +} + +static inline void hci_conn_rssi_unset_all(struct hci_dev *hdev, + __u8 type) +{ + struct hci_conn_hash *h = &hdev->conn_hash; + struct hci_conn *c; + __u8 conn_type; + + if (type == 0x01) + conn_type = LE_LINK; + else + conn_type = ACL_LINK; + + rcu_read_lock(); + list_for_each_entry_rcu(c, &h->list, list) { + if (c->type == conn_type) + c->rssi_monitored = false; + } + rcu_read_unlock(); +} + +static inline int hci_conn_hash_lookup_rssi_count(struct hci_dev *hdev) +{ + struct hci_conn_hash *h = &hdev->conn_hash; + struct hci_conn *c; + int count = 0; + + rcu_read_lock(); + list_for_each_entry_rcu(c, &h->list, list) { + if (c->rssi_monitored == true) + ++count; + } + rcu_read_unlock(); + + return count; +} +#endif + int hci_disconnect(struct hci_conn *conn, __u8 reason); bool hci_setup_sync(struct hci_conn *conn, __u16 handle); void hci_sco_setup(struct hci_conn *conn, __u8 status); @@ -1578,6 +1645,19 @@ void mgmt_advertising_removed(struct sock *sk, struct hci_dev *hdev, u8 instance); int mgmt_phy_configuration_changed(struct hci_dev *hdev, struct sock *skip); +#ifdef TIZEN_BT +void mgmt_rssi_enable_success(struct sock *sk, struct hci_dev *hdev, + void *data, struct hci_cc_rsp_enable_rssi *rp, int success); +void mgmt_rssi_disable_success(struct sock *sk, struct hci_dev *hdev, + void *data, struct hci_cc_rsp_enable_rssi *rp, int success); +int mgmt_set_rssi_threshold(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len); +void mgmt_rssi_alert_evt(struct hci_dev *hdev, struct sk_buff *skb); +void mgmt_raw_rssi_response(struct hci_dev *hdev, + struct hci_cc_rp_get_raw_rssi *rp, int success); +void mgmt_enable_rssi_cc(struct hci_dev *hdev, void *response, u8 status); +#endif + u8 hci_le_conn_update(struct hci_conn *conn, u16 min, u16 max, u16 latency, u16 to_multiplier); void hci_le_start_enc(struct hci_conn *conn, __le16 ediv, __le64 rand, diff --git a/include/net/bluetooth/mgmt_tizen.h b/include/net/bluetooth/mgmt_tizen.h index 6dc8e36..46f652b 100644 --- a/include/net/bluetooth/mgmt_tizen.h +++ b/include/net/bluetooth/mgmt_tizen.h @@ -63,4 +63,67 @@ struct mgmt_cp_remove_dev_from_white_list { #define MGMT_OP_CLEAR_DEV_WHITE_LIST (TIZEN_OP_CODE_BASE + 0x06) #define MGMT_OP_CLEAR_DEV_WHITE_LIST_SIZE 0 +/* For RSSI monitoring */ +#define MGMT_OP_SET_RSSI_ENABLE (TIZEN_OP_CODE_BASE + 0x07) +#define MGMT_SET_RSSI_ENABLE_SIZE 10 + +struct mgmt_cp_set_enable_rssi { + __s8 low_th; + __s8 in_range_th; + __s8 high_th; + bdaddr_t bdaddr; + __s8 link_type; +} __packed; + +struct mgmt_cc_rsp_enable_rssi { + __u8 status; + __u8 le_ext_opcode; + bdaddr_t bt_address; + __s8 link_type; +} __packed; + +#define MGMT_OP_GET_RAW_RSSI (TIZEN_OP_CODE_BASE + 0x08) +#define MGMT_GET_RAW_RSSI_SIZE 7 + +struct mgmt_cp_get_raw_rssi { + bdaddr_t bt_address; + __u8 link_type; +} __packed; + +#define MGMT_OP_SET_RSSI_DISABLE (TIZEN_OP_CODE_BASE + 0x09) +#define MGMT_SET_RSSI_DISABLE_SIZE 7 +struct mgmt_cp_disable_rssi { + bdaddr_t bdaddr; + __u8 link_type; +} __packed; +struct mgmt_cc_rp_disable_rssi { + __u8 status; + __u8 le_ext_opcode; + bdaddr_t bt_address; + __s8 link_type; +} __packed; +/* RSSI monitoring */ + +/* For handling of RSSI Events */ +#define MGMT_EV_RSSI_ALERT (TIZEN_EV_BASE + 0x04) +struct mgmt_ev_vendor_specific_rssi_alert { + bdaddr_t bdaddr; + __s8 link_type; + __s8 alert_type; + __s8 rssi_dbm; +} __packed; + +#define MGMT_EV_RAW_RSSI (TIZEN_EV_BASE + 0x05) +struct mgmt_cc_rp_get_raw_rssi { + __u8 status; + __s8 rssi_dbm; + __u8 link_type; + bdaddr_t bt_address; +} __packed; + +#define MGMT_EV_RSSI_ENABLED (TIZEN_EV_BASE + 0x06) + +#define MGMT_EV_RSSI_DISABLED (TIZEN_EV_BASE + 0x07) +/* Handling of RSSI Events */ + #endif /* __MGMT_TIZEN_H */ diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index 3e7badb..f7616f7 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -1604,6 +1604,30 @@ static void hci_cc_set_ext_adv_param(struct hci_dev *hdev, struct sk_buff *skb) hci_dev_unlock(hdev); } +#ifdef TIZEN_BT +static void hci_cc_enable_rssi(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_cc_rsp_enable_rssi *rp = (void *)skb->data; + + BT_DBG("hci_cc_enable_rssi - %s status 0x%2.2x Event_LE_ext_Opcode 0x%2.2x", + hdev->name, rp->status, rp->le_ext_opcode); + + mgmt_enable_rssi_cc(hdev, rp, rp->status); +} + +static void hci_cc_get_raw_rssi(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_cc_rp_get_raw_rssi *rp = (void *)skb->data; + + BT_DBG("hci_cc_get_raw_rssi- %s Get Raw Rssi Response[%2.2x %4.4x %2.2X]", + hdev->name, rp->status, rp->conn_handle, rp->rssi_dbm); + + mgmt_raw_rssi_response(hdev, rp, rp->status); +} +#endif + static void hci_cc_read_rssi(struct hci_dev *hdev, struct sk_buff *skb) { struct hci_rp_read_rssi *rp = (void *) skb->data; @@ -3342,7 +3366,15 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb, case HCI_OP_LE_SET_ADV_SET_RAND_ADDR: hci_cc_le_set_adv_set_random_addr(hdev, skb); break; +#ifdef TIZEN_BT + case HCI_OP_ENABLE_RSSI: + hci_cc_enable_rssi(hdev, skb); + break; + case HCI_OP_GET_RAW_RSSI: + hci_cc_get_raw_rssi(hdev, skb); + break; +#endif default: BT_DBG("%s opcode 0x%4.4x", hdev->name, *opcode); break; diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c index c8081c1..f63f7c0 100644 --- a/net/bluetooth/mgmt.c +++ b/net/bluetooth/mgmt.c @@ -5528,6 +5528,689 @@ unlocked: return err; } + +static void set_rssi_threshold_complete(struct hci_dev *hdev, + u8 status, u16 opcode) +{ + struct mgmt_pending_cmd *cmd; + + BT_DBG("status 0x%02x", status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_SET_RSSI_ENABLE, hdev); + if (!cmd) + goto unlock; + + if (status) + mgmt_cmd_status(cmd->sk, hdev->id, MGMT_OP_SET_RSSI_ENABLE, + mgmt_status(status)); + else + mgmt_cmd_complete(cmd->sk, hdev->id, MGMT_OP_SET_RSSI_ENABLE, 0, + NULL, 0); + + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +static void set_rssi_disable_complete(struct hci_dev *hdev, + u8 status, u16 opcode) +{ + struct mgmt_pending_cmd *cmd; + + BT_DBG("status 0x%02x", status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_SET_RSSI_DISABLE, hdev); + if (!cmd) + goto unlock; + + if (status) + mgmt_cmd_status(cmd->sk, hdev->id, MGMT_OP_SET_RSSI_DISABLE, + mgmt_status(status)); + else + mgmt_cmd_complete(cmd->sk, hdev->id, MGMT_OP_SET_RSSI_DISABLE, + 0, NULL, 0); + + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +int mgmt_set_rssi_threshold(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + int err = 0; + struct hci_cp_set_rssi_threshold th = { 0, }; + struct mgmt_cp_set_enable_rssi *cp = data; + struct hci_conn *conn; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + __u8 dest_type; + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_SET_RSSI_ENABLE, hdev); + if (!cmd) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_RSSI_ENABLE, + MGMT_STATUS_FAILED); + goto unlocked; + } + + if (!lmp_le_capable(hdev)) { + mgmt_pending_remove(cmd); + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_RSSI_ENABLE, + MGMT_STATUS_NOT_SUPPORTED); + goto unlocked; + } + + if (!hdev_is_powered(hdev)) { + BT_DBG("%s", hdev->name); + mgmt_pending_remove(cmd); + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_RSSI_ENABLE, + MGMT_STATUS_NOT_POWERED); + goto unlocked; + } + + if (cp->link_type == 0x01) + dest_type = LE_LINK; + else + dest_type = ACL_LINK; + + /* Get LE/ACL link handle info */ + conn = hci_conn_hash_lookup_ba(hdev, + dest_type, &cp->bdaddr); + + if (!conn) { + err = mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_SET_RSSI_ENABLE, 1, NULL, 0); + mgmt_pending_remove(cmd); + goto unlocked; + } + + hci_req_init(&req, hdev); + + th.hci_le_ext_opcode = 0x0B; + th.mode = 0x01; + th.conn_handle = conn->handle; + th.alert_mask = 0x07; + th.low_th = cp->low_th; + th.in_range_th = cp->in_range_th; + th.high_th = cp->high_th; + + hci_req_add(&req, HCI_OP_ENABLE_RSSI, sizeof(th), &th); + err = hci_req_run(&req, set_rssi_threshold_complete); + + if (err < 0) { + mgmt_pending_remove(cmd); + BT_ERR("Error in requesting hci_req_run"); + goto unlocked; + } + +unlocked: + hci_dev_unlock(hdev); + return err; +} + +void mgmt_rssi_enable_success(struct sock *sk, struct hci_dev *hdev, + void *data, struct hci_cc_rsp_enable_rssi *rp, int success) +{ + struct mgmt_cc_rsp_enable_rssi mgmt_rp = { 0, }; + struct mgmt_cp_set_enable_rssi *cp = data; + struct mgmt_pending_cmd *cmd; + + if (!cp || !rp) + goto remove_cmd; + + mgmt_rp.status = rp->status; + mgmt_rp.le_ext_opcode = rp->le_ext_opcode; + mgmt_rp.bt_address = cp->bdaddr; + mgmt_rp.link_type = cp->link_type; + + mgmt_cmd_complete(sk, hdev->id, MGMT_OP_SET_RSSI_ENABLE, + MGMT_STATUS_SUCCESS, &mgmt_rp, + sizeof(struct mgmt_cc_rsp_enable_rssi)); + + mgmt_event(MGMT_EV_RSSI_ENABLED, hdev, &mgmt_rp, + sizeof(struct mgmt_cc_rsp_enable_rssi), NULL); + + hci_conn_rssi_unset_all(hdev, mgmt_rp.link_type); + hci_conn_rssi_state_set(hdev, mgmt_rp.link_type, + &mgmt_rp.bt_address, true); + +remove_cmd: + hci_dev_lock(hdev); + cmd = pending_find(MGMT_OP_SET_RSSI_ENABLE, hdev); + if (cmd) + mgmt_pending_remove(cmd); + + hci_dev_unlock(hdev); +} + +void mgmt_rssi_disable_success(struct sock *sk, struct hci_dev *hdev, + void *data, struct hci_cc_rsp_enable_rssi *rp, int success) +{ + struct mgmt_cc_rp_disable_rssi mgmt_rp = { 0, }; + struct mgmt_cp_disable_rssi *cp = data; + struct mgmt_pending_cmd *cmd; + + if (!cp || !rp) + goto remove_cmd; + + mgmt_rp.status = rp->status; + mgmt_rp.le_ext_opcode = rp->le_ext_opcode; + mgmt_rp.bt_address = cp->bdaddr; + mgmt_rp.link_type = cp->link_type; + + mgmt_cmd_complete(sk, hdev->id, MGMT_OP_SET_RSSI_DISABLE, + MGMT_STATUS_SUCCESS, &mgmt_rp, + sizeof(struct mgmt_cc_rsp_enable_rssi)); + + mgmt_event(MGMT_EV_RSSI_DISABLED, hdev, &mgmt_rp, + sizeof(struct mgmt_cc_rsp_enable_rssi), NULL); + + hci_conn_rssi_state_set(hdev, mgmt_rp.link_type, + &mgmt_rp.bt_address, false); + +remove_cmd: + hci_dev_lock(hdev); + cmd = pending_find(MGMT_OP_SET_RSSI_DISABLE, hdev); + if (cmd) + mgmt_pending_remove(cmd); + + hci_dev_unlock(hdev); +} + +static int mgmt_set_disable_rssi(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_pending_cmd *cmd; + struct hci_request req; + struct hci_cp_set_enable_rssi cp_en = { 0, }; + int err; + + BT_DBG("Set Disable RSSI."); + + cp_en.hci_le_ext_opcode = 0x01; + cp_en.le_enable_cs_Features = 0x00; + cp_en.data[0] = 0x00; + cp_en.data[1] = 0x00; + cp_en.data[2] = 0x00; + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_SET_RSSI_DISABLE, hdev); + if (!cmd) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_RSSI_DISABLE, + MGMT_STATUS_FAILED); + goto unlocked; + } + + if (!lmp_le_capable(hdev)) { + mgmt_pending_remove(cmd); + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_RSSI_DISABLE, + MGMT_STATUS_NOT_SUPPORTED); + goto unlocked; + } + + if (!hdev_is_powered(hdev)) { + BT_DBG("%s", hdev->name); + mgmt_pending_remove(cmd); + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_RSSI_DISABLE, + MGMT_STATUS_NOT_POWERED); + goto unlocked; + } + + hci_req_init(&req, hdev); + + BT_DBG("Enable Len: %zu [%2.2X %2.2X %2.2X %2.2X %2.2X]", + sizeof(struct hci_cp_set_enable_rssi), + cp_en.hci_le_ext_opcode, cp_en.le_enable_cs_Features, + cp_en.data[0], cp_en.data[1], cp_en.data[2]); + + hci_req_add(&req, HCI_OP_ENABLE_RSSI, sizeof(cp_en), &cp_en); + err = hci_req_run(&req, set_rssi_disable_complete); + + if (err < 0) { + mgmt_pending_remove(cmd); + BT_ERR("Error in requesting hci_req_run"); + goto unlocked; + } + +unlocked: + hci_dev_unlock(hdev); + return err; +} + +void mgmt_enable_rssi_cc(struct hci_dev *hdev, void *response, u8 status) +{ + struct hci_cc_rsp_enable_rssi *rp = response; + struct mgmt_pending_cmd *cmd_enable = NULL; + struct mgmt_pending_cmd *cmd_disable = NULL; + struct mgmt_cp_set_enable_rssi *cp_en; + struct mgmt_cp_disable_rssi *cp_dis; + + hci_dev_lock(hdev); + cmd_enable = pending_find(MGMT_OP_SET_RSSI_ENABLE, hdev); + cmd_disable = pending_find(MGMT_OP_SET_RSSI_DISABLE, hdev); + hci_dev_unlock(hdev); + + if (cmd_enable) + BT_DBG("Enable Request"); + + if (cmd_disable) + BT_DBG("Disable Request"); + + if (cmd_enable) { + cp_en = cmd_enable->param; + + if (status != 0x00) + return; + + switch (rp->le_ext_opcode) { + case 0x01: + BT_DBG("RSSI enabled.. Setting Threshold..."); + mgmt_set_rssi_threshold(cmd_enable->sk, hdev, + cp_en, sizeof(*cp_en)); + break; + + case 0x0B: + BT_DBG("Sending RSSI enable success"); + mgmt_rssi_enable_success(cmd_enable->sk, hdev, + cp_en, rp, rp->status); + break; + } + + } else if (cmd_disable) { + cp_dis = cmd_disable->param; + + if (status != 0x00) + return; + + switch (rp->le_ext_opcode) { + case 0x01: + BT_DBG("Sending RSSI disable success"); + mgmt_rssi_disable_success(cmd_disable->sk, hdev, + cp_dis, rp, rp->status); + break; + + case 0x0B: + /* + * Only unset RSSI Threshold values for the Link if + * RSSI is monitored for other BREDR or LE Links + */ + if (hci_conn_hash_lookup_rssi_count(hdev) > 1) { + BT_DBG("Unset Threshold. Other links being monitored"); + mgmt_rssi_disable_success(cmd_disable->sk, hdev, + cp_dis, rp, rp->status); + } else { + BT_DBG("Unset Threshold. Disabling..."); + mgmt_set_disable_rssi(cmd_disable->sk, hdev, + cp_dis, sizeof(*cp_dis)); + } + break; + } + } +} + +static void set_rssi_enable_complete(struct hci_dev *hdev, u8 status, + u16 opcode) +{ + struct mgmt_pending_cmd *cmd; + + BT_DBG("status 0x%02x", status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_SET_RSSI_ENABLE, hdev); + if (!cmd) + goto unlock; + + if (status) + mgmt_cmd_status(cmd->sk, hdev->id, MGMT_OP_SET_RSSI_ENABLE, + mgmt_status(status)); + else + mgmt_cmd_complete(cmd->sk, hdev->id, MGMT_OP_SET_RSSI_ENABLE, 0, + NULL, 0); + + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +static int set_enable_rssi(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_pending_cmd *cmd; + struct hci_request req; + struct mgmt_cp_set_enable_rssi *cp = data; + struct hci_cp_set_enable_rssi cp_en = { 0, }; + int err; + + BT_DBG("Set Enable RSSI."); + + cp_en.hci_le_ext_opcode = 0x01; + cp_en.le_enable_cs_Features = 0x04; + cp_en.data[0] = 0x00; + cp_en.data[1] = 0x00; + cp_en.data[2] = 0x00; + + hci_dev_lock(hdev); + + if (!lmp_le_capable(hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_RSSI_ENABLE, + MGMT_STATUS_NOT_SUPPORTED); + goto unlocked; + } + + if (!hdev_is_powered(hdev)) { + BT_DBG("%s", hdev->name); + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_RSSI_ENABLE, + MGMT_STATUS_NOT_POWERED); + goto unlocked; + } + + if (pending_find(MGMT_OP_SET_RSSI_ENABLE, hdev)) { + BT_DBG("%s", hdev->name); + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_RSSI_ENABLE, + MGMT_STATUS_BUSY); + goto unlocked; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_RSSI_ENABLE, hdev, cp, + sizeof(*cp)); + if (!cmd) { + BT_DBG("%s", hdev->name); + err = -ENOMEM; + goto unlocked; + } + + /* If RSSI is already enabled directly set Threshold values */ + if (hci_conn_hash_lookup_rssi_count(hdev) > 0) { + hci_dev_unlock(hdev); + BT_DBG("RSSI Enabled. Directly set Threshold"); + err = mgmt_set_rssi_threshold(sk, hdev, cp, sizeof(*cp)); + return err; + } + + hci_req_init(&req, hdev); + + BT_DBG("Enable Len: %zu [%2.2X %2.2X %2.2X %2.2X %2.2X]", + sizeof(struct hci_cp_set_enable_rssi), + cp_en.hci_le_ext_opcode, cp_en.le_enable_cs_Features, + cp_en.data[0], cp_en.data[1], cp_en.data[2]); + + hci_req_add(&req, HCI_OP_ENABLE_RSSI, sizeof(cp_en), &cp_en); + err = hci_req_run(&req, set_rssi_enable_complete); + + if (err < 0) { + mgmt_pending_remove(cmd); + BT_ERR("Error in requesting hci_req_run"); + goto unlocked; + } + +unlocked: + hci_dev_unlock(hdev); + + return err; +} + +static void get_raw_rssi_complete(struct hci_dev *hdev, u8 status, u16 opcode) +{ + struct mgmt_pending_cmd *cmd; + + BT_DBG("status 0x%02x", status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_GET_RAW_RSSI, hdev); + if (!cmd) + goto unlock; + + mgmt_cmd_complete(cmd->sk, hdev->id, MGMT_OP_GET_RAW_RSSI, + MGMT_STATUS_SUCCESS, &status, 1); + + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +static int get_raw_rssi(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_pending_cmd *cmd; + struct hci_request req; + struct mgmt_cp_get_raw_rssi *cp = data; + struct hci_cp_get_raw_rssi hci_cp; + + struct hci_conn *conn; + int err; + __u8 dest_type; + + BT_DBG("Get Raw RSSI."); + + hci_dev_lock(hdev); + + if (!lmp_le_capable(hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_GET_RAW_RSSI, + MGMT_STATUS_NOT_SUPPORTED); + goto unlocked; + } + + if (cp->link_type == 0x01) + dest_type = LE_LINK; + else + dest_type = ACL_LINK; + + /* Get LE/BREDR link handle info */ + conn = hci_conn_hash_lookup_ba(hdev, + dest_type, &cp->bt_address); + if (!conn) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_GET_RAW_RSSI, + MGMT_STATUS_NOT_CONNECTED); + goto unlocked; + } + hci_cp.conn_handle = conn->handle; + + if (!hdev_is_powered(hdev)) { + BT_DBG("%s", hdev->name); + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_GET_RAW_RSSI, + MGMT_STATUS_NOT_POWERED); + goto unlocked; + } + + if (pending_find(MGMT_OP_GET_RAW_RSSI, hdev)) { + BT_DBG("%s", hdev->name); + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_GET_RAW_RSSI, + MGMT_STATUS_BUSY); + goto unlocked; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_GET_RAW_RSSI, hdev, data, len); + if (!cmd) { + BT_DBG("%s", hdev->name); + err = -ENOMEM; + goto unlocked; + } + + hci_req_init(&req, hdev); + + BT_DBG("Connection Handle [%d]", hci_cp.conn_handle); + hci_req_add(&req, HCI_OP_GET_RAW_RSSI, sizeof(hci_cp), &hci_cp); + err = hci_req_run(&req, get_raw_rssi_complete); + + if (err < 0) { + mgmt_pending_remove(cmd); + BT_ERR("Error in requesting hci_req_run"); + } + +unlocked: + hci_dev_unlock(hdev); + + return err; +} + +void mgmt_raw_rssi_response(struct hci_dev *hdev, + struct hci_cc_rp_get_raw_rssi *rp, int success) +{ + struct mgmt_cc_rp_get_raw_rssi mgmt_rp = { 0, }; + struct hci_conn *conn; + + mgmt_rp.status = rp->status; + mgmt_rp.rssi_dbm = rp->rssi_dbm; + + conn = hci_conn_hash_lookup_handle(hdev, rp->conn_handle); + if (!conn) + return; + + bacpy(&mgmt_rp.bt_address, &conn->dst); + if (conn->type == LE_LINK) + mgmt_rp.link_type = 0x01; + else + mgmt_rp.link_type = 0x00; + + mgmt_event(MGMT_EV_RAW_RSSI, hdev, &mgmt_rp, + sizeof(struct mgmt_cc_rp_get_raw_rssi), NULL); +} + +static void set_disable_threshold_complete(struct hci_dev *hdev, + u8 status, u16 opcode) +{ + struct mgmt_pending_cmd *cmd; + + BT_DBG("status 0x%02x", status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_SET_RSSI_DISABLE, hdev); + if (!cmd) + goto unlock; + + mgmt_cmd_complete(cmd->sk, hdev->id, MGMT_OP_SET_RSSI_DISABLE, + MGMT_STATUS_SUCCESS, &status, 1); + + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +/** Removes monitoring for a link*/ +static int set_disable_threshold(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + int err = 0; + struct hci_cp_set_rssi_threshold th = { 0, }; + struct mgmt_cp_disable_rssi *cp = data; + struct hci_conn *conn; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + __u8 dest_type; + + BT_DBG("Set Disable RSSI."); + + hci_dev_lock(hdev); + + if (!lmp_le_capable(hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_RSSI_DISABLE, + MGMT_STATUS_NOT_SUPPORTED); + goto unlocked; + } + + /* Get LE/ACL link handle info*/ + if (cp->link_type == 0x01) + dest_type = LE_LINK; + else + dest_type = ACL_LINK; + + conn = hci_conn_hash_lookup_ba(hdev, dest_type, &cp->bdaddr); + if (!conn) { + err = mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_SET_RSSI_DISABLE, 1, NULL, 0); + goto unlocked; + } + + th.hci_le_ext_opcode = 0x0B; + th.mode = 0x01; + th.conn_handle = conn->handle; + th.alert_mask = 0x00; + th.low_th = 0x00; + th.in_range_th = 0x00; + th.high_th = 0x00; + + if (!hdev_is_powered(hdev)) { + BT_DBG("%s", hdev->name); + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_SET_RSSI_DISABLE, + 0, data, len); + goto unlocked; + } + + if (pending_find(MGMT_OP_SET_RSSI_DISABLE, hdev)) { + BT_DBG("%s", hdev->name); + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_RSSI_DISABLE, + MGMT_STATUS_BUSY); + goto unlocked; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_RSSI_DISABLE, hdev, cp, + sizeof(*cp)); + if (!cmd) { + BT_DBG("%s", hdev->name); + err = -ENOMEM; + goto unlocked; + } + + hci_req_init(&req, hdev); + + hci_req_add(&req, HCI_OP_ENABLE_RSSI, sizeof(th), &th); + err = hci_req_run(&req, set_disable_threshold_complete); + if (err < 0) { + mgmt_pending_remove(cmd); + BT_ERR("Error in requesting hci_req_run"); + goto unlocked; + } + +unlocked: + hci_dev_unlock(hdev); + + return err; +} + +void mgmt_rssi_alert_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_vendor_specific_rssi_alert *ev = (void *)skb->data; + struct mgmt_ev_vendor_specific_rssi_alert mgmt_ev; + struct hci_conn *conn; + + BT_DBG("RSSI alert [%2.2X %2.2X %2.2X]", + ev->conn_handle, ev->alert_type, ev->rssi_dbm); + + conn = hci_conn_hash_lookup_handle(hdev, ev->conn_handle); + + if (!conn) { + BT_ERR("RSSI alert Error: Device not found for handle"); + return; + } + bacpy(&mgmt_ev.bdaddr, &conn->dst); + + if (conn->type == LE_LINK) + mgmt_ev.link_type = 0x01; + else + mgmt_ev.link_type = 0x00; + + mgmt_ev.alert_type = ev->alert_type; + mgmt_ev.rssi_dbm = ev->rssi_dbm; + + mgmt_event(MGMT_EV_RSSI_ALERT, hdev, &mgmt_ev, + sizeof(struct mgmt_ev_vendor_specific_rssi_alert), + NULL); +} #endif /* TIZEN_BT */ static bool ltk_is_valid(struct mgmt_ltk_info *key) @@ -7394,6 +8077,9 @@ static const struct hci_mgmt_handler tizen_mgmt_handlers[] = { { add_white_list, MGMT_ADD_DEV_WHITE_LIST_SIZE }, { remove_from_white_list, MGMT_REMOVE_DEV_FROM_WHITE_LIST_SIZE }, { clear_white_list, MGMT_OP_CLEAR_DEV_WHITE_LIST_SIZE }, + { set_enable_rssi, MGMT_SET_RSSI_ENABLE_SIZE }, + { get_raw_rssi, MGMT_GET_RAW_RSSI_SIZE }, + { set_disable_threshold, MGMT_SET_RSSI_DISABLE_SIZE }, }; #endif -- 2.7.4