[Bluetooth] Add automatic sniff mode support

This patch introduces the automatic sniff mode feature. This allows
the host to switch idle connections into sniff mode to safe power.

Signed-off-by: Ulisses Furquim <ulissesf@gmail.com>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>

1 files changed, 166 insertions, 26 deletions

diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index f41cf1a6c11a..3896dabab11d 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -83,6 +83,8 @@ static void hci_cc_link_policy(struct hci_dev *hdev, __u16 ocf, struct sk_buff *
 {
         struct hci_conn *conn;
         struct hci_rp_role_discovery *rd;
+        struct hci_rp_write_link_policy *lp;
+        void *sent;
 
         BT_DBG("%s ocf 0x%x", hdev->name, ocf);
 
@@ -106,6 +108,27 @@ static void hci_cc_link_policy(struct hci_dev *hdev, __u16 ocf, struct sk_buff *
                 hci_dev_unlock(hdev);
                 break;
 
+        case OCF_WRITE_LINK_POLICY:
+                sent = hci_sent_cmd_data(hdev, OGF_LINK_POLICY, OCF_WRITE_LINK_POLICY);
+                if (!sent)
+                        break;
+
+                lp = (struct hci_rp_write_link_policy *) skb->data;
+
+                if (lp->status)
+                        break;
+
+                hci_dev_lock(hdev);
+
+                conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(lp->handle));
+                if (conn) {
+                        __le16 policy = get_unaligned((__le16 *) (sent + 2));
+                        conn->link_policy = __le16_to_cpu(policy);
+                }
+
+                hci_dev_unlock(hdev);
+                break;
+
         default:
                 BT_DBG("%s: Command complete: ogf LINK_POLICY ocf %x", 
                                 hdev->name, ocf);
@@ -274,7 +297,7 @@ static void hci_cc_host_ctl(struct hci_dev *hdev, __u16 ocf, struct sk_buff *skb
 /* Command Complete OGF INFO_PARAM  */
 static void hci_cc_info_param(struct hci_dev *hdev, __u16 ocf, struct sk_buff *skb)
 {
-        struct hci_rp_read_loc_features *lf;
+        struct hci_rp_read_local_features *lf;
         struct hci_rp_read_buffer_size *bs;
         struct hci_rp_read_bd_addr *ba;
 
@@ -282,7 +305,7 @@ static void hci_cc_info_param(struct hci_dev *hdev, __u16 ocf, struct sk_buff *s
 
         switch (ocf) {
         case OCF_READ_LOCAL_FEATURES:
-                lf = (struct hci_rp_read_loc_features *) skb->data;
+                lf = (struct hci_rp_read_local_features *) skb->data;
 
                 if (lf->status) {
                         BT_DBG("%s READ_LOCAL_FEATURES failed %d", hdev->name, lf->status);
@@ -447,8 +470,46 @@ static void hci_cs_link_policy(struct hci_dev *hdev, __u16 ocf, __u8 status)
         BT_DBG("%s ocf 0x%x", hdev->name, ocf);
 
         switch (ocf) {
+        case OCF_SNIFF_MODE:
+                if (status) {
+                        struct hci_conn *conn;
+                        struct hci_cp_sniff_mode *cp = hci_sent_cmd_data(hdev, OGF_LINK_POLICY, OCF_SNIFF_MODE);
+
+                        if (!cp)
+                                break;
+
+                        hci_dev_lock(hdev);
+
+                        conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle));
+                        if (conn) {
+                                clear_bit(HCI_CONN_MODE_CHANGE_PEND, &conn->pend);
+                        }
+
+                        hci_dev_unlock(hdev);
+                }
+                break;
+
+        case OCF_EXIT_SNIFF_MODE:
+                if (status) {
+                        struct hci_conn *conn;
+                        struct hci_cp_exit_sniff_mode *cp = hci_sent_cmd_data(hdev, OGF_LINK_POLICY, OCF_EXIT_SNIFF_MODE);
+
+                        if (!cp)
+                                break;
+
+                        hci_dev_lock(hdev);
+
+                        conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle));
+                        if (conn) {
+                                clear_bit(HCI_CONN_MODE_CHANGE_PEND, &conn->pend);
+                        }
+
+                        hci_dev_unlock(hdev);
+                }
+                break;
+
         default:
-                BT_DBG("%s Command status: ogf HOST_POLICY ocf %x", hdev->name, ocf);
+                BT_DBG("%s Command status: ogf LINK_POLICY ocf %x", hdev->name, ocf);
                 break;
         }
 }
@@ -630,14 +691,16 @@ static inline void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *sk
                 else
                         cp.role = 0x01; /* Remain slave */
 
-                hci_send_cmd(hdev, OGF_LINK_CTL, OCF_ACCEPT_CONN_REQ, sizeof(cp), &cp);
+                hci_send_cmd(hdev, OGF_LINK_CTL,
+                                OCF_ACCEPT_CONN_REQ, sizeof(cp), &cp);
         } else {
                 /* Connection rejected */
                 struct hci_cp_reject_conn_req cp;
 
                 bacpy(&cp.bdaddr, &ev->bdaddr);
                 cp.reason = 0x0f;
-                hci_send_cmd(hdev, OGF_LINK_CTL, OCF_REJECT_CONN_REQ, sizeof(cp), &cp);
+                hci_send_cmd(hdev, OGF_LINK_CTL,
+                                OCF_REJECT_CONN_REQ, sizeof(cp), &cp);
         }
 }
 
@@ -645,7 +708,7 @@ static inline void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *sk
 static inline void hci_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_conn_complete *ev = (struct hci_ev_conn_complete *) skb->data;
-        struct hci_conn *conn = NULL;
+        struct hci_conn *conn;
 
         BT_DBG("%s", hdev->name);
 
@@ -667,12 +730,21 @@ static inline void hci_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *s
                 if (test_bit(HCI_ENCRYPT, &hdev->flags))
                         conn->link_mode |= HCI_LM_ENCRYPT;
 
+                /* Get remote features */
+                if (conn->type == ACL_LINK) {
+                        struct hci_cp_read_remote_features cp;
+                        cp.handle = ev->handle;
+                        hci_send_cmd(hdev, OGF_LINK_CTL,
+                                OCF_READ_REMOTE_FEATURES, sizeof(cp), &cp);
+                }
+
                 /* Set link policy */
                 if (conn->type == ACL_LINK && hdev->link_policy) {
                         struct hci_cp_write_link_policy cp;
                         cp.handle = ev->handle;
                         cp.policy = __cpu_to_le16(hdev->link_policy);
-                        hci_send_cmd(hdev, OGF_LINK_POLICY, OCF_WRITE_LINK_POLICY, sizeof(cp), &cp);
+                        hci_send_cmd(hdev, OGF_LINK_POLICY,
+                                OCF_WRITE_LINK_POLICY, sizeof(cp), &cp);
                 }
 
                 /* Set packet type for incoming connection */
@@ -683,7 +755,8 @@ static inline void hci_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *s
                                 __cpu_to_le16(hdev->pkt_type & ACL_PTYPE_MASK):
                                 __cpu_to_le16(hdev->pkt_type & SCO_PTYPE_MASK);
 
-                        hci_send_cmd(hdev, OGF_LINK_CTL, OCF_CHANGE_CONN_PTYPE, sizeof(cp), &cp);
+                        hci_send_cmd(hdev, OGF_LINK_CTL,
+                                OCF_CHANGE_CONN_PTYPE, sizeof(cp), &cp);
                 }
         } else
                 conn->state = BT_CLOSED;
@@ -711,8 +784,7 @@ static inline void hci_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *s
 static inline void hci_disconn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_disconn_complete *ev = (struct hci_ev_disconn_complete *) skb->data;
-        struct hci_conn *conn = NULL;
-        __u16 handle = __le16_to_cpu(ev->handle);
+        struct hci_conn *conn;
 
         BT_DBG("%s status %d", hdev->name, ev->status);
 
@@ -721,7 +793,7 @@ static inline void hci_disconn_complete_evt(struct hci_dev *hdev, struct sk_buff
 
         hci_dev_lock(hdev);
 
-        conn = hci_conn_hash_lookup_handle(hdev, handle);
+        conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle));
         if (conn) {
                 conn->state = BT_CLOSED;
                 hci_proto_disconn_ind(conn, ev->reason);
@@ -778,7 +850,7 @@ static inline void hci_num_comp_pkts_evt(struct hci_dev *hdev, struct sk_buff *s
 static inline void hci_role_change_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_role_change *ev = (struct hci_ev_role_change *) skb->data;
-        struct hci_conn *conn = NULL;
+        struct hci_conn *conn;
 
         BT_DBG("%s status %d", hdev->name, ev->status);
 
@@ -801,18 +873,43 @@ static inline void hci_role_change_evt(struct hci_dev *hdev, struct sk_buff *skb
         hci_dev_unlock(hdev);
 }
 
+/* Mode Change */
+static inline void hci_mode_change_evt(struct hci_dev *hdev, struct sk_buff *skb)
+{
+        struct hci_ev_mode_change *ev = (struct hci_ev_mode_change *) skb->data;
+        struct hci_conn *conn;
+
+        BT_DBG("%s status %d", hdev->name, ev->status);
+
+        hci_dev_lock(hdev);
+
+        conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle));
+        if (conn) {
+                conn->mode = ev->mode;
+                conn->interval = __le16_to_cpu(ev->interval);
+
+                if (!test_and_clear_bit(HCI_CONN_MODE_CHANGE_PEND, &conn->pend)) {
+                        if (conn->mode == HCI_CM_ACTIVE)
+                                conn->power_save = 1;
+                        else
+                                conn->power_save = 0;
+                }
+        }
+
+        hci_dev_unlock(hdev);
+}
+
 /* Authentication Complete */
 static inline void hci_auth_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_auth_complete *ev = (struct hci_ev_auth_complete *) skb->data;
-        struct hci_conn *conn = NULL;
-        __u16 handle = __le16_to_cpu(ev->handle);
+        struct hci_conn *conn;
 
         BT_DBG("%s status %d", hdev->name, ev->status);
 
         hci_dev_lock(hdev);
 
-        conn = hci_conn_hash_lookup_handle(hdev, handle);
+        conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle));
         if (conn) {
                 if (!ev->status)
                         conn->link_mode |= HCI_LM_AUTH;
@@ -827,8 +924,7 @@ static inline void hci_auth_complete_evt(struct hci_dev *hdev, struct sk_buff *s
                                 cp.handle  = __cpu_to_le16(conn->handle);
                                 cp.encrypt = 1;
                                 hci_send_cmd(conn->hdev, OGF_LINK_CTL,
-                                                OCF_SET_CONN_ENCRYPT,
-                                                sizeof(cp), &cp);
+                                        OCF_SET_CONN_ENCRYPT, sizeof(cp), &cp);
                         } else {
                                 clear_bit(HCI_CONN_ENCRYPT_PEND, &conn->pend);
                                 hci_encrypt_cfm(conn, ev->status, 0x00);
@@ -843,14 +939,13 @@ static inline void hci_auth_complete_evt(struct hci_dev *hdev, struct sk_buff *s
 static inline void hci_encrypt_change_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_encrypt_change *ev = (struct hci_ev_encrypt_change *) skb->data;
-        struct hci_conn *conn = NULL;
-        __u16 handle = __le16_to_cpu(ev->handle);
+        struct hci_conn *conn;
 
         BT_DBG("%s status %d", hdev->name, ev->status);
 
         hci_dev_lock(hdev);
 
-        conn = hci_conn_hash_lookup_handle(hdev, handle);
+        conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle));
         if (conn) {
                 if (!ev->status) {
                         if (ev->encrypt)
@@ -871,14 +966,13 @@ static inline void hci_encrypt_change_evt(struct hci_dev *hdev, struct sk_buff *
 static inline void hci_change_conn_link_key_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_change_conn_link_key_complete *ev = (struct hci_ev_change_conn_link_key_complete *) skb->data;
-        struct hci_conn *conn = NULL;
-        __u16 handle = __le16_to_cpu(ev->handle);
+        struct hci_conn *conn;
 
         BT_DBG("%s status %d", hdev->name, ev->status);
 
         hci_dev_lock(hdev);
 
-        conn = hci_conn_hash_lookup_handle(hdev, handle);
+        conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle));
         if (conn) {
                 if (!ev->status)
                         conn->link_mode |= HCI_LM_SECURE;
@@ -906,18 +1000,35 @@ static inline void hci_link_key_notify_evt(struct hci_dev *hdev, struct sk_buff
 {
 }
 
+/* Remote Features */
+static inline void hci_remote_features_evt(struct hci_dev *hdev, struct sk_buff *skb)
+{
+        struct hci_ev_remote_features *ev = (struct hci_ev_remote_features *) skb->data;
+        struct hci_conn *conn;
+
+        BT_DBG("%s status %d", hdev->name, ev->status);
+
+        hci_dev_lock(hdev);
+
+        conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle));
+        if (conn && !ev->status) {
+                memcpy(conn->features, ev->features, sizeof(conn->features));
+        }
+
+        hci_dev_unlock(hdev);
+}
+
 /* Clock Offset */
 static inline void hci_clock_offset_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_clock_offset *ev = (struct hci_ev_clock_offset *) skb->data;
-        struct hci_conn *conn = NULL;
-        __u16 handle = __le16_to_cpu(ev->handle);
+        struct hci_conn *conn;
 
         BT_DBG("%s status %d", hdev->name, ev->status);
 
         hci_dev_lock(hdev);
 
-        conn = hci_conn_hash_lookup_handle(hdev, handle);
+        conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle));
         if (conn && !ev->status) {
                 struct inquiry_entry *ie;
 
@@ -948,6 +1059,23 @@ static inline void hci_pscan_rep_mode_evt(struct hci_dev *hdev, struct sk_buff *
         hci_dev_unlock(hdev);
 }
 
+/* Sniff Subrate */
+static inline void hci_sniff_subrate_evt(struct hci_dev *hdev, struct sk_buff *skb)
+{
+        struct hci_ev_sniff_subrate *ev = (struct hci_ev_sniff_subrate *) skb->data;
+        struct hci_conn *conn;
+
+        BT_DBG("%s status %d", hdev->name, ev->status);
+
+        hci_dev_lock(hdev);
+
+        conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle));
+        if (conn) {
+        }
+
+        hci_dev_unlock(hdev);
+}
+
 void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_event_hdr *hdr = (struct hci_event_hdr *) skb->data;
@@ -996,6 +1124,10 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_role_change_evt(hdev, skb);
                 break;
 
+        case HCI_EV_MODE_CHANGE:
+                hci_mode_change_evt(hdev, skb);
+                break;
+
         case HCI_EV_AUTH_COMPLETE:
                 hci_auth_complete_evt(hdev, skb);
                 break;
@@ -1020,6 +1152,10 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_link_key_notify_evt(hdev, skb);
                 break;
 
+        case HCI_EV_REMOTE_FEATURES:
+                hci_remote_features_evt(hdev, skb);
+                break;
+
         case HCI_EV_CLOCK_OFFSET:
                 hci_clock_offset_evt(hdev, skb);
                 break;
@@ -1028,6 +1164,10 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_pscan_rep_mode_evt(hdev, skb);
                 break;
 
+        case HCI_EV_SNIFF_SUBRATE:
+                hci_sniff_subrate_evt(hdev, skb);
+                break;
+
         case HCI_EV_CMD_STATUS:
                 cs = (struct hci_ev_cmd_status *) skb->data;
                 skb_pull(skb, sizeof(cs));