2 files changed, 111 insertions, 82 deletions

diff --git a/net/bluetooth/hidp/core.c b/net/bluetooth/hidp/core.c
index 56a51f91591a..d8029cfcd452 100644
--- a/net/bluetooth/hidp/core.c
+++ b/net/bluetooth/hidp/core.c
@@ -623,9 +623,15 @@ static struct device *hidp_get_device(struct hidp_session *session)
 static int hidp_setup_input(struct hidp_session *session,
                                 struct hidp_connadd_req *req)
 {
-        struct input_dev *input = session->input;
+        struct input_dev *input;
         int i;
 
+        input = input_allocate_device();
+        if (!input)
+                return -ENOMEM;
+
+        session->input = input;
+
         input_set_drvdata(input, session);
 
         input->name = "Bluetooth HID Boot Protocol Device";
@@ -698,55 +704,117 @@ static void hidp_setup_quirks(struct hid_device *hid)
                         hid->quirks = hidp_blacklist[n].quirks;
 }
 
+static int hidp_parse(struct hid_device *hid)
+{
+        struct hidp_session *session = hid->driver_data;
+        struct hidp_connadd_req *req = session->req;
+        unsigned char *buf;
+        int ret;
+
+        buf = kmalloc(req->rd_size, GFP_KERNEL);
+        if (!buf)
+                return -ENOMEM;
+
+        if (copy_from_user(buf, req->rd_data, req->rd_size)) {
+                kfree(buf);
+                return -EFAULT;
+        }
+
+        ret = hid_parse_report(session->hid, buf, req->rd_size);
+
+        kfree(buf);
+
+        if (ret)
+                return ret;
+
+        session->req = NULL;
+
+        hidp_setup_quirks(hid);
+        return 0;
+}
+
+static int hidp_start(struct hid_device *hid)
+{
+        struct hidp_session *session = hid->driver_data;
+        struct hid_report *report;
+
+        list_for_each_entry(report, &hid->report_enum[HID_INPUT_REPORT].
+                        report_list, list)
+                hidp_send_report(session, report);
+
+        list_for_each_entry(report, &hid->report_enum[HID_FEATURE_REPORT].
+                        report_list, list)
+                hidp_send_report(session, report);
+
+        if (hidinput_connect(hid) == 0)
+                hid->claimed |= HID_CLAIMED_INPUT;
+
+        return 0;
+}
+
+static void hidp_stop(struct hid_device *hid)
+{
+        struct hidp_session *session = hid->driver_data;
+
+        skb_queue_purge(&session->ctrl_transmit);
+        skb_queue_purge(&session->intr_transmit);
+
+        if (hid->claimed & HID_CLAIMED_INPUT)
+                hidinput_disconnect(hid);
+        hid->claimed = 0;
+}
+
+static struct hid_ll_driver hidp_hid_driver = {
+        .parse = hidp_parse,
+        .start = hidp_start,
+        .stop = hidp_stop,
+        .open  = hidp_open,
+        .close = hidp_close,
+        .hidinput_input_event = hidp_hidinput_event,
+};
+
 static int hidp_setup_hid(struct hidp_session *session,
                                 struct hidp_connadd_req *req)
 {
-        struct hid_device *hid = session->hid;
-        struct hid_report *report;
+        struct hid_device *hid;
         bdaddr_t src, dst;
         int ret;
 
-        baswap(&src, &bt_sk(session->ctrl_sock->sk)->src);
-        baswap(&dst, &bt_sk(session->ctrl_sock->sk)->dst);
+        hid = hid_allocate_device();
+        if (IS_ERR(hid)) {
+                ret = PTR_ERR(session->hid);
+                goto err;
+        }
 
+        session->hid = hid;
+        session->req = req;
         hid->driver_data = session;
 
-        hid->country = req->country;
+        baswap(&src, &bt_sk(session->ctrl_sock->sk)->src);
+        baswap(&dst, &bt_sk(session->ctrl_sock->sk)->dst);
 
         hid->bus     = BUS_BLUETOOTH;
         hid->vendor  = req->vendor;
         hid->product = req->product;
         hid->version = req->version;
+        hid->country = req->country;
 
         strncpy(hid->name, req->name, 128);
         strncpy(hid->phys, batostr(&src), 64);
         strncpy(hid->uniq, batostr(&dst), 64);
 
         hid->dev.parent = hidp_get_device(session);
-
-        hid->hid_open  = hidp_open;
-        hid->hid_close = hidp_close;
-
-        hid->hidinput_input_event = hidp_hidinput_event;
-
-        hidp_setup_quirks(hid);
-
-        list_for_each_entry(report, &hid->report_enum[HID_INPUT_REPORT].report_list, list)
-                hidp_send_report(session, report);
-
-        list_for_each_entry(report, &hid->report_enum[HID_FEATURE_REPORT].report_list, list)
-                hidp_send_report(session, report);
-
-        if (hidinput_connect(hid) == 0)
-                hid->claimed |= HID_CLAIMED_INPUT;
+        hid->ll_driver = &hidp_hid_driver;
 
         ret = hid_add_device(hid);
-        if (ret) {
-                if (hid->claimed & HID_CLAIMED_INPUT)
-                        hidinput_disconnect(hid);
-                skb_queue_purge(&session->intr_transmit);
-        }
+        if (ret)
+                goto err_hid;
 
+        return 0;
+err_hid:
+        hid_destroy_device(hid);
+        session->hid = NULL;
+err:
         return ret;
 }
 
@@ -767,46 +835,6 @@ int hidp_add_connection(struct hidp_connadd_req *req, struct socket *ctrl_sock,
 
         BT_DBG("rd_data %p rd_size %d", req->rd_data, req->rd_size);
 
-        if (req->rd_size > 0) {
-                unsigned char *buf = kmalloc(req->rd_size, GFP_KERNEL);
-
-                if (!buf) {
-                        kfree(session);
-                        return -ENOMEM;
-                }
-
-                if (copy_from_user(buf, req->rd_data, req->rd_size)) {
-                        kfree(buf);
-                        kfree(session);
-                        return -EFAULT;
-                }
-
-                session->hid = hid_allocate_device();
-                if (IS_ERR(session->hid)) {
-                        kfree(buf);
-                        kfree(session);
-                        return PTR_ERR(session->hid);
-                }
-
-                err = hid_parse_report(session->hid, buf, req->rd_size);
-
-                kfree(buf);
-
-                if (err) {
-                        hid_destroy_device(session->hid);
-                        kfree(session);
-                        return -EINVAL;
-                }
-        }
-
-        if (!session->hid) {
-                session->input = input_allocate_device();
-                if (!session->input) {
-                        kfree(session);
-                        return -ENOMEM;
-                }
-        }
-
         down_write(&hidp_session_sem);
 
         s = __hidp_get_session(&bt_sk(ctrl_sock->sk)->dst);
@@ -834,16 +862,16 @@ int hidp_add_connection(struct hidp_connadd_req *req, struct socket *ctrl_sock,
         session->flags   = req->flags & (1 << HIDP_BLUETOOTH_VENDOR_ID);
         session->idle_to = req->idle_to;
 
-        if (session->input) {
-                err = hidp_setup_input(session, req);
-                if (err < 0)
-                        goto failed;
-        }
-
-        if (session->hid) {
+        if (req->rd_size > 0) {
                 err = hidp_setup_hid(session, req);
                 if (err)
-                        goto failed;
+                        goto err_skb;
+        }
+
+        if (!session->hid) {
+                err = hidp_setup_input(session, req);
+                if (err < 0)
+                        goto err_skb;
         }
 
         __hidp_link_session(session);
@@ -871,16 +899,15 @@ unlink:
 
         __hidp_unlink_session(session);
 
-        if (session->input) {
+        if (session->input)
                 input_unregister_device(session->input);
-                session->input = NULL; /* don't try to free it here */
-        }
-
-failed:
-        up_write(&hidp_session_sem);
-
         if (session->hid)
                 hid_destroy_device(session->hid);
+err_skb:
+        skb_queue_purge(&session->ctrl_transmit);
+        skb_queue_purge(&session->intr_transmit);
+failed:
+        up_write(&hidp_session_sem);
 
         input_free_device(session->input);
         kfree(session);
diff --git a/net/bluetooth/hidp/hidp.h b/net/bluetooth/hidp/hidp.h
index 343fb0566b3e..e503c89057ad 100644
--- a/net/bluetooth/hidp/hidp.h
+++ b/net/bluetooth/hidp/hidp.h
@@ -151,6 +151,8 @@ struct hidp_session {
 
         struct sk_buff_head ctrl_transmit;
         struct sk_buff_head intr_transmit;
+
+        struct hidp_connadd_req *req;
 };
 
 static inline void hidp_schedule(struct hidp_session *session)