[PATCH 1/7] Bluetooth: Add RSSI Monitor commands

[PATCH 1/7] Bluetooth: Add RSSI Monitor commands

[anderson.briglia]

From: Anderson Briglia <anderson.briglia@openbossa.org>

This patch adds two new Management commands: Enable/Disable RSSI Monitor.
These commands are used to handle a list of RSSI monitors, that are
responsible to watch RSSI values from a connection. It is useful for
Proximity profile implementation.

Signed-off-by: Anderson Briglia <anderson.briglia@openbossa.org>
---
 include/net/bluetooth/mgmt.h |   12 ++++++++++++
 net/bluetooth/mgmt.c         |   22 ++++++++++++++++++++++
 2 files changed, 34 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/mgmt.h b/include/net/bluetooth/mgmt.h
index 5428fd3..ef3b636 100644
--- a/include/net/bluetooth/mgmt.h
+++ b/include/net/bluetooth/mgmt.h
@@ -211,6 +211,18 @@ struct mgmt_cp_unblock_device {
 	bdaddr_t bdaddr;
 } __packed;
 
+#define MGMT_OP_ENABLE_RSSI_MONITOR	0x001F
+struct mgmt_cp_enable_rssi_monitor {
+	bdaddr_t bdaddr;
+	__s8 low_alert_trigger;
+	__s8 high_alert_trigger;
+} __packed;
+
+#define MGMT_OP_DISABLE_RSSI_MONITOR	0x0020
+struct mgmt_cp_disable_rssi_monitor {
+	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 7c0f4f6..36ed168 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -1884,6 +1884,22 @@ static int unblock_device(struct sock *sk, u16 index, unsigned char *data,
 	return err;
 }
 
+static int enable_rssi_monitor(struct sock *sk, u16 index,
+					unsigned char *data, u16 len)
+{
+	BT_DBG("hci%u", index);
+
+	return -ENOSYS;
+}
+
+static int disable_rssi_monitor(struct sock *sk, u16 index,
+					unsigned char *data, u16 len)
+{
+	BT_DBG("hci%u", index);
+
+	return -ENOSYS;
+}
+
 int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
 {
 	unsigned char *buf;
@@ -2004,6 +2020,12 @@ int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
 	case MGMT_OP_UNBLOCK_DEVICE:
 		err = unblock_device(sk, index, buf + sizeof(*hdr), len);
 		break;
+	case MGMT_OP_ENABLE_RSSI_MONITOR:
+		err = enable_rssi_monitor(sk, index, buf + sizeof(*hdr), len);
+		break;
+	case MGMT_OP_DISABLE_RSSI_MONITOR:
+		err = disable_rssi_monitor(sk, index, buf + sizeof(*hdr), len);
+		break;
 	default:
 		BT_DBG("Unknown op %u", opcode);
 		err = cmd_status(sk, index, opcode, 0x01);
-- 
1.7.4.1

[PATCH 2/7] Bluetooth: Implement Enable RSSI Monitor

[anderson.briglia]

From: Anderson Briglia <anderson.briglia@openbossa.org>

This patch implements Enable RSSI Monitor Management command. This
command is responsible to add monitors into a list. This list stores all
RSSI monitors, one for each monitored connection.

Signed-off-by: Anderson Briglia <anderson.briglia@openbossa.org>
---
 include/net/bluetooth/hci_core.h |    2 +
 net/bluetooth/hci_core.c         |    2 +
 net/bluetooth/mgmt.c             |   84 +++++++++++++++++++++++++++++++++++++-
 3 files changed, 87 insertions(+), 1 deletions(-)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 1bc1c3a..b1e3e615 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -211,6 +211,8 @@ struct hci_dev {
 
 	struct timer_list	le_scan_timer;
 
+	struct list_head	rssi_monitors;
+
 	struct hci_dev_stats	stat;
 
 	struct sk_buff_head	driver_init;
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 8f9ed8c..52414d4 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -1521,6 +1521,8 @@ int hci_register_dev(struct hci_dev *hdev)
 	setup_timer(&hdev->le_scan_timer, hci_disable_le_scan,
 						(unsigned long) hdev);
 
+	INIT_LIST_HEAD(&hdev->rssi_monitors);
+
 	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);
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index 36ed168..410f614 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -55,6 +55,14 @@ struct pending_cmd {
 
 static LIST_HEAD(cmd_list);
 
+struct rssi_monitor {
+	struct list_head list;
+	bdaddr_t bdaddr;
+	s8 low_trigger;
+	s8 high_trigger;
+	u8 last_alert;
+};
+
 static int cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status)
 {
 	struct sk_buff *skb;
@@ -1884,12 +1892,86 @@ static int unblock_device(struct sock *sk, u16 index, unsigned char *data,
 	return err;
 }
 
+static struct rssi_monitor *rssi_monitor_find(u16 index, bdaddr_t *bdaddr)
+{
+	struct rssi_monitor *tmp, *rm;
+	struct hci_dev *hdev;
+
+	hdev = hci_dev_get(index);
+	if (!hdev)
+		return NULL;
+
+	rm = NULL;
+
+	list_for_each_entry(tmp, &hdev->rssi_monitors, list) {
+		if (bacmp(&tmp->bdaddr, bdaddr) == 0) {
+			rm = tmp;
+			break;
+		}
+	}
+
+	hci_dev_put(hdev);
+
+	return rm;
+}
+
+static int rssi_check_trigger_values(s8 low_trigger, s8 high_trigger)
+{
+	if (low_trigger < high_trigger ||
+			((high_trigger < -127 || low_trigger > 128)))
+		return -EINVAL;
+
+	return 0;
+}
+
+static int rssi_monitor_add(u16 index, bdaddr_t *bdaddr, s8 low_trigger,
+							s8 high_trigger)
+{
+	struct rssi_monitor *rm;
+	struct hci_dev *hdev;
+
+	if (rssi_check_trigger_values(low_trigger, high_trigger))
+		return -EINVAL;
+
+	if (rssi_monitor_find(index, bdaddr))
+		return -EEXIST;
+
+	rm = kzalloc(sizeof(*rm), GFP_ATOMIC);
+	if (!rm)
+		return -ENOMEM;
+
+	bacpy(&rm->bdaddr, bdaddr);
+	rm->low_trigger = low_trigger;
+	rm->high_trigger = high_trigger;
+
+	hdev = hci_dev_get(index);
+	if (!hdev) {
+		kfree(rm);
+		return -EINVAL;
+	}
+
+	list_add(&rm->list, &hdev->rssi_monitors);
+
+	hci_dev_put(hdev);
+
+	return 0;
+}
+
 static int enable_rssi_monitor(struct sock *sk, u16 index,
 					unsigned char *data, u16 len)
 {
+	struct mgmt_cp_enable_rssi_monitor *cp;
+
 	BT_DBG("hci%u", index);
 
-	return -ENOSYS;
+	cp = (void *) data;
+
+	if (len != sizeof(*cp))
+		return cmd_status(sk, index,
+					MGMT_OP_ENABLE_RSSI_MONITOR, EINVAL);
+
+	return rssi_monitor_add(index, &cp->bdaddr,
+			cp->low_alert_trigger, cp->high_alert_trigger);
 }
 
 static int disable_rssi_monitor(struct sock *sk, u16 index,
-- 
1.7.4.1

[PATCH 3/7] Bluetooth: Implement Disable RSSI Monitor

[anderson.briglia]

From: Anderson Briglia <anderson.briglia@openbossa.org>

This patch implements Disable RSSI Monitor command. It is used to remove
elements from RSSI monitor list.

Signed-off-by: Anderson Briglia <anderson.briglia@openbossa.org>
---
 net/bluetooth/mgmt.c |   24 +++++++++++++++++++++++-
 1 files changed, 23 insertions(+), 1 deletions(-)

diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index 410f614..a4a2927 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -1957,6 +1957,20 @@ static int rssi_monitor_add(u16 index, bdaddr_t *bdaddr, s8 low_trigger,
 	return 0;
 }
 
+static int rssi_monitor_remove(u16 index, bdaddr_t *bdaddr)
+{
+	struct rssi_monitor *rm;
+
+	rm = rssi_monitor_find(index, bdaddr);
+	if (!rm)
+		return -EINVAL;
+
+	list_del(&rm->list);
+	kfree(rm);
+
+	return 0;
+}
+
 static int enable_rssi_monitor(struct sock *sk, u16 index,
 					unsigned char *data, u16 len)
 {
@@ -1977,9 +1991,17 @@ static int enable_rssi_monitor(struct sock *sk, u16 index,
 static int disable_rssi_monitor(struct sock *sk, u16 index,
 					unsigned char *data, u16 len)
 {
+	struct mgmt_cp_disable_rssi_monitor *cp;
+
 	BT_DBG("hci%u", index);
 
-	return -ENOSYS;
+	cp = (void *) data;
+
+	if (len != sizeof(*cp))
+		return cmd_status(sk, index,
+				MGMT_OP_DISABLE_RSSI_MONITOR, EINVAL);
+
+	return rssi_monitor_remove(index, &cp->bdaddr);
 }
 
 int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
-- 
1.7.4.1

[PATCH 4/7] Bluetooth: Implement RSSI Monitor Alert event

[anderson.briglia]

From: Anderson Briglia <anderson.briglia@openbossa.org>

This patch implements functions to calculate and send a Management event
according to RSSI value from a monitored connection. The event was
divided in three types: high, in range and low. The RSSI Monitor Alert
event is sent when a monitored RSSI value (stored into RSSI Monitor
list), reaches one of the two thresholds (low and high).

Signed-off-by: Anderson Briglia <anderson.briglia@openbossa.org>
---
 include/net/bluetooth/hci_core.h |    1 +
 include/net/bluetooth/mgmt.h     |    6 ++++++
 net/bluetooth/mgmt.c             |   32 ++++++++++++++++++++++++++++++++
 3 files changed, 39 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index b1e3e615..df3b943 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -870,6 +870,7 @@ int mgmt_has_pending_stop_discov(u16 index);
 int mgmt_cancel_discovery(u16 index);
 int mgmt_is_interleaved_discovery(u16 index);
 int mgmt_do_interleaved_discovery(u16 index);
+int mgmt_rssi_monitor_send_alert(u16 index, bdaddr_t *bdaddr, s8 rssi);
 
 /* 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 ef3b636..f254f40 100644
--- a/include/net/bluetooth/mgmt.h
+++ b/include/net/bluetooth/mgmt.h
@@ -313,3 +313,9 @@ struct mgmt_ev_remote_name {
 } __packed;
 
 #define MGMT_EV_DISCOVERING		0x0014
+
+#define MGMT_EV_RSSI_MONITOR_ALERT	0x0015
+struct mgmt_ev_rssi_monitor_alert {
+	bdaddr_t bdaddr;
+	__u8 alert_type;
+} __packed;
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index a4a2927..635ab8c 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -44,6 +44,10 @@ enum bt_device_type {
 #define BREDR_LE_SCAN_TIMEOUT 5120 /* TGAP(100)/2 */
 #define LE_ONLY_SCAN_TIMEOUT 10240 /* TGAP(gen_disc_scan_min) */
 
+#define RSSI_MONITOR_ALERT_IN_RANGE	0x00
+#define RSSI_MONITOR_ALERT_LOW		0x01
+#define RSSI_MONITOR_ALERT_HIGH		0x02
+
 struct pending_cmd {
 	struct list_head list;
 	__u16 opcode;
@@ -1943,6 +1947,7 @@ static int rssi_monitor_add(u16 index, bdaddr_t *bdaddr, s8 low_trigger,
 	bacpy(&rm->bdaddr, bdaddr);
 	rm->low_trigger = low_trigger;
 	rm->high_trigger = high_trigger;
+	rm->last_alert = RSSI_MONITOR_ALERT_HIGH;
 
 	hdev = hci_dev_get(index);
 	if (!hdev) {
@@ -2004,6 +2009,33 @@ static int disable_rssi_monitor(struct sock *sk, u16 index,
 	return rssi_monitor_remove(index, &cp->bdaddr);
 }
 
+int mgmt_rssi_monitor_send_alert(u16 index, bdaddr_t *bdaddr, s8 rssi)
+{
+	struct rssi_monitor *rm;
+	struct mgmt_ev_rssi_monitor_alert ev;
+	u8 alert;
+
+	rm = rssi_monitor_find(index, bdaddr);
+	if (!rm)
+		return -EINVAL;
+
+	if (rssi <= rm->high_trigger)
+		alert = RSSI_MONITOR_ALERT_HIGH;
+	else if (rssi <= rm->low_trigger)
+		alert = RSSI_MONITOR_ALERT_LOW;
+	else
+		alert = RSSI_MONITOR_ALERT_IN_RANGE;
+
+	if (rm->last_alert == alert)
+		return 0;
+
+	bacpy(&ev.bdaddr, bdaddr);
+	ev.alert_type = alert;
+	rm->last_alert = alert;
+	return mgmt_event(MGMT_EV_RSSI_MONITOR_ALERT, index, &ev,
+							sizeof(ev), NULL);
+}
+
 int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
 {
 	unsigned char *buf;
-- 
1.7.4.1

[PATCH 5/7] Bluetooth: Implement Read RSSI command

[anderson.briglia]

From: Anderson Briglia <anderson.briglia@openbossa.org>

This command is used for RSSI Monitoring since the controller did not
have this feature.
When Read RSSI Command completes, the RSSI alert level is calculated for
monitored connections and if the alert type has changed, an alert event
is sent to user space according to configured threshold values.

Signed-off-by: Anderson Briglia <anderson.briglia@openbossa.org>
---
 include/net/bluetooth/hci.h |   10 ++++++++++
 net/bluetooth/hci_event.c   |   23 +++++++++++++++++++++++
 net/bluetooth/mgmt.c        |   23 +++++++++++++++++++++++
 3 files changed, 56 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index c4fdeeb..276be6e 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -550,6 +550,16 @@ struct hci_cp_set_event_flt {
 	__u8     condition[0];
 } __packed;
 
+#define HCI_OP_READ_RSSI		0x1405
+struct hci_cp_read_rssi {
+	__le16	handle;
+} __packed;
+struct hci_rp_read_rssi {
+	__u8	status;
+	__le16	handle;
+	__s8	rssi;
+} __packed;
+
 /* Filter types */
 #define HCI_FLT_CLEAR_ALL	0x00
 #define HCI_FLT_INQ_RESULT	0x01
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 55872ff..b21da65 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -982,6 +982,25 @@ static inline void hci_cc_write_le_host_supported(struct hci_dev *hdev,
 	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_EXT_FEATURES, sizeof(cp), &cp);
 }
 
+static void hci_cc_read_rssi(struct hci_dev *hdev, struct sk_buff *skb)
+{
+	struct hci_rp_read_rssi *rp = (void *) skb->data;
+	struct hci_conn *conn;
+
+	BT_DBG("%s status 0x%x", hdev->name, rp->status);
+
+	if (!test_bit(HCI_MGMT, &hdev->flags))
+		return;
+
+	conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(rp->handle));
+	if (!conn) {
+		BT_DBG("%s read rssi failed", hdev->name);
+		return;
+	}
+
+	mgmt_rssi_monitor_send_alert(hdev->id, &conn->dst, rp->rssi);
+}
+
 static inline void hci_cs_inquiry(struct hci_dev *hdev, __u8 status)
 {
 	BT_DBG("%s status 0x%x", hdev->name, status);
@@ -2012,6 +2031,10 @@ static inline void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *sk
 		hci_cc_write_le_host_supported(hdev, skb);
 		break;
 
+	case HCI_OP_READ_RSSI:
+		hci_cc_read_rssi(hdev, skb);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%x", hdev->name, opcode);
 		break;
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index 635ab8c..bda2b23 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -2706,3 +2706,26 @@ int mgmt_do_interleaved_discovery(u16 index)
 
 	return err;
 }
+
+static int mgmt_read_rssi(struct hci_dev *hdev, bdaddr_t *bdaddr)
+{
+	struct hci_cp_read_rssi rs;
+	struct hci_conn *conn;
+
+	BT_DBG("hci%u", hdev->id);
+
+	if (!test_bit(HCI_UP, &hdev->flags))
+		return -ENETDOWN;
+
+	conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, bdaddr);
+	if (!conn)
+		conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, bdaddr);
+
+	if (!conn)
+		return -ENOTCONN;
+
+	put_unaligned_le16(conn->handle, &rs.handle);
+
+	return hci_send_cmd(hdev, HCI_OP_READ_RSSI, sizeof(rs), &rs);
+}
+
-- 
1.7.4.1

[PATCH 6/7] Bluetooth: Add RSSI Monitor timer and list lock

[anderson.briglia]

From: Anderson Briglia <anderson.briglia@openbossa.org>

This is the polling mechanism to send HCI Read RSSI commands. The
command responses are used by the RSSI Monitor to get the current RSSI
value and calculate the current alert level for a given monitored
connection.

This patch also adds read and write locks on rssi_monitor_list avoiding
race conditions.

Signed-off-by: Anderson Briglia <anderson.briglia@openbossa.org>
---
 include/net/bluetooth/hci_core.h |    2 +
 net/bluetooth/hci_core.c         |   11 +++++
 net/bluetooth/mgmt.c             |   77 +++++++++++++++++++++++++++++++++----
 3 files changed, 81 insertions(+), 9 deletions(-)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index df3b943..668dab2 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -212,6 +212,7 @@ struct hci_dev {
 	struct timer_list	le_scan_timer;
 
 	struct list_head	rssi_monitors;
+	struct timer_list	rssi_timer;
 
 	struct hci_dev_stats	stat;
 
@@ -871,6 +872,7 @@ int mgmt_cancel_discovery(u16 index);
 int mgmt_is_interleaved_discovery(u16 index);
 int mgmt_do_interleaved_discovery(u16 index);
 int mgmt_rssi_monitor_send_alert(u16 index, bdaddr_t *bdaddr, s8 rssi);
+void mgmt_monitor_rssi(struct hci_dev *hdev);
 
 /* HCI info for socket */
 #define hci_pi(sk) ((struct hci_pinfo *) sk)
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 52414d4..0ae7997 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -1448,6 +1448,14 @@ static void hci_disable_le_scan(unsigned long arg)
 	hci_send_cmd(hdev, HCI_OP_LE_SET_SCAN_ENABLE, sizeof(cp), &cp);
 }
 
+static void hci_read_rssi(unsigned long arg)
+{
+	struct hci_dev *hdev = (void *) arg;
+
+	if (test_bit(HCI_UP, &hdev->flags))
+		mgmt_monitor_rssi(hdev);
+}
+
 /* Register HCI device */
 int hci_register_dev(struct hci_dev *hdev)
 {
@@ -1522,6 +1530,8 @@ int hci_register_dev(struct hci_dev *hdev)
 						(unsigned long) hdev);
 
 	INIT_LIST_HEAD(&hdev->rssi_monitors);
+	setup_timer(&hdev->rssi_timer, hci_read_rssi,
+						(unsigned long) hdev);
 
 	INIT_WORK(&hdev->power_on, hci_power_on);
 	INIT_WORK(&hdev->power_off, hci_power_off);
@@ -1605,6 +1615,7 @@ int hci_unregister_dev(struct hci_dev *hdev)
 	hci_del_off_timer(hdev);
 	del_timer(&hdev->adv_timer);
 	del_timer(&hdev->le_scan_timer);
+	del_timer(&hdev->rssi_timer);
 
 	destroy_workqueue(hdev->workqueue);
 
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index bda2b23..9c90090 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -48,6 +48,8 @@ enum bt_device_type {
 #define RSSI_MONITOR_ALERT_LOW		0x01
 #define RSSI_MONITOR_ALERT_HIGH		0x02
 
+#define RSSI_MONITOR_TIMEOUT		2000
+
 struct pending_cmd {
 	struct list_head list;
 	__u16 opcode;
@@ -67,6 +69,9 @@ struct rssi_monitor {
 	u8 last_alert;
 };
 
+static spinlock_t rssi_monitor_list_lock;
+static DEFINE_SPINLOCK(rssi_monitor_list_lock);
+
 static int cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status)
 {
 	struct sk_buff *skb;
@@ -1933,16 +1938,23 @@ static int rssi_monitor_add(u16 index, bdaddr_t *bdaddr, s8 low_trigger,
 {
 	struct rssi_monitor *rm;
 	struct hci_dev *hdev;
+	int err = 0;
 
 	if (rssi_check_trigger_values(low_trigger, high_trigger))
 		return -EINVAL;
 
-	if (rssi_monitor_find(index, bdaddr))
-		return -EEXIST;
+	spin_lock_bh(&rssi_monitor_list_lock);
+
+	if (rssi_monitor_find(index, bdaddr)) {
+		err = -EEXIST;
+		goto out;
+	}
 
 	rm = kzalloc(sizeof(*rm), GFP_ATOMIC);
-	if (!rm)
-		return -ENOMEM;
+	if (!rm) {
+		err = -ENOMEM;
+		goto out;
+	}
 
 	bacpy(&rm->bdaddr, bdaddr);
 	rm->low_trigger = low_trigger;
@@ -1952,28 +1964,54 @@ static int rssi_monitor_add(u16 index, bdaddr_t *bdaddr, s8 low_trigger,
 	hdev = hci_dev_get(index);
 	if (!hdev) {
 		kfree(rm);
-		return -EINVAL;
+		err = -EINVAL;
+		goto out;
 	}
 
 	list_add(&rm->list, &hdev->rssi_monitors);
 
+	mod_timer(&hdev->rssi_timer, jiffies +
+			msecs_to_jiffies(RSSI_MONITOR_TIMEOUT));
+
 	hci_dev_put(hdev);
 
-	return 0;
+out:
+	spin_unlock_bh(&rssi_monitor_list_lock);
+
+	return err;
 }
 
 static int rssi_monitor_remove(u16 index, bdaddr_t *bdaddr)
 {
 	struct rssi_monitor *rm;
+	struct hci_dev *hdev;
+	int err = 0;
+
+	spin_lock_bh(&rssi_monitor_list_lock);
 
 	rm = rssi_monitor_find(index, bdaddr);
-	if (!rm)
-		return -EINVAL;
+	if (!rm) {
+		err = -EINVAL;
+		goto out;
+	}
 
 	list_del(&rm->list);
 	kfree(rm);
 
-	return 0;
+	hdev = hci_dev_get(index);
+	if (!hdev) {
+		err = -EINVAL;
+		goto out;
+	}
+
+	if (list_empty(&hdev->rssi_monitors))
+		del_timer(&hdev->rssi_timer);
+
+	hci_dev_put(hdev);
+
+out:
+	spin_unlock_bh(&rssi_monitor_list_lock);
+	return err;
 }
 
 static int enable_rssi_monitor(struct sock *sk, u16 index,
@@ -2015,7 +2053,11 @@ int mgmt_rssi_monitor_send_alert(u16 index, bdaddr_t *bdaddr, s8 rssi)
 	struct mgmt_ev_rssi_monitor_alert ev;
 	u8 alert;
 
+	spin_lock(&rssi_monitor_list_lock);
+
 	rm = rssi_monitor_find(index, bdaddr);
+
+	spin_unlock(&rssi_monitor_list_lock);
 	if (!rm)
 		return -EINVAL;
 
@@ -2729,3 +2771,20 @@ static int mgmt_read_rssi(struct hci_dev *hdev, bdaddr_t *bdaddr)
 	return hci_send_cmd(hdev, HCI_OP_READ_RSSI, sizeof(rs), &rs);
 }
 
+void mgmt_monitor_rssi(struct hci_dev *hdev)
+{
+	struct rssi_monitor *rm;
+
+	spin_lock(&rssi_monitor_list_lock);
+
+	list_for_each_entry(rm, &hdev->rssi_monitors, list)
+		mgmt_read_rssi(hdev, &rm->bdaddr);
+
+	if (list_empty(&hdev->rssi_monitors))
+		return;
+
+	mod_timer(&hdev->rssi_timer, jiffies +
+			msecs_to_jiffies(RSSI_MONITOR_TIMEOUT));
+
+	spin_unlock(&rssi_monitor_list_lock);
+}
-- 
1.7.4.1

[PATCH 7/7] Bluetooth: Remove RSSI monitor on disconnection

[anderson.briglia]

From: Anderson Briglia <anderson.briglia@openbossa.org>

Remove RSSI monitor from list when remote has been disconnected.

Signed-off-by: Anderson Briglia <anderson.briglia@openbossa.org>
---
 net/bluetooth/mgmt.c |    2 ++
 1 files changed, 2 insertions(+), 0 deletions(-)

diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index 9c90090..0174637 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -2376,6 +2376,8 @@ int mgmt_disconnected(u16 index, bdaddr_t *bdaddr)
 	if (sk)
 		sock_put(sk);
 
+	err = rssi_monitor_remove(index, bdaddr);
+
 	return err;
 }
 
-- 
1.7.4.1