Bluetooth: A2MP: Manage incoming connections

Handle incoming A2MP connection by creating AMP manager and
processing A2MP messages.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>

2 files changed, 31 insertions, 4 deletions

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index f1ec1b1d308f..e08ca2ac31aa 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -569,3 +569,19 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
         return mgr;
 }
+
+struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
+                                       struct sk_buff *skb)
+{
+        struct amp_mgr *mgr;
+
+        mgr = amp_mgr_create(conn);
+        if (!mgr) {
+                BT_ERR("Could not create AMP manager");
+                return NULL;
+        }
+
+        BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
+
+        return mgr->a2mp_chan;
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index fc572795497a..3daac2c6b7b4 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -37,6 +37,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/smp.h>
+#include <net/bluetooth/a2mp.h>
 
 bool disable_ertm;
 
@@ -5132,10 +5133,20 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
 
         chan = l2cap_get_chan_by_scid(conn, cid);
         if (!chan) {
-                BT_DBG("unknown cid 0x%4.4x", cid);
-                /* Drop packet and return */
-                kfree_skb(skb);
-                return 0;
+                if (cid == L2CAP_CID_A2MP) {
+                        chan = a2mp_channel_create(conn, skb);
+                        if (!chan) {
+                                kfree_skb(skb);
+                                return 0;
+                        }
+
+                        l2cap_chan_lock(chan);
+                } else {
+                        BT_DBG("unknown cid 0x%4.4x", cid);
+                        /* Drop packet and return */
+                        kfree_skb(skb);
+                        return 0;
+                }
         }
 
         BT_DBG("chan %p, len %d", chan, skb->len);