Bluetooth: set skbuffer priority based on L2CAP socket priority

This uses SO_PRIORITY to set the skbuffer priority field

Signed-off-by: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>
Signed-off-by: Gustavo F. Padovan <padovan@profusion.mobi>

4 files changed, 26 insertions, 9 deletions

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 967e18f72a38..9285a650949c 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -32,6 +32,9 @@
 #define HCI_PROTO_L2CAP 0
 #define HCI_PROTO_SCO   1
 
+/* HCI priority */
+#define HCI_PRIO_MAX    7
+
 /* HCI Core structures */
 struct inquiry_data {
         bdaddr_t        bdaddr;
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 38a561581169..c10bf1db0abb 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -747,7 +747,8 @@ struct l2cap_chan *l2cap_chan_create(struct sock *sk);
 void l2cap_chan_close(struct l2cap_chan *chan, int reason);
 void l2cap_chan_destroy(struct l2cap_chan *chan);
 int l2cap_chan_connect(struct l2cap_chan *chan);
-int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len);
+int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
+                                                                u32 priority);
 void l2cap_chan_busy(struct l2cap_chan *chan, int busy);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 76210cd6d3ea..ac2c41ada0fe 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -564,6 +564,7 @@ static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
                 flags = ACL_START;
 
         bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON;
+        skb->priority = HCI_PRIO_MAX;
 
         hci_send_acl(conn->hcon, skb, flags);
 }
@@ -1265,7 +1266,8 @@ static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb)
         struct hci_conn *hcon = chan->conn->hcon;
         u16 flags;
 
-        BT_DBG("chan %p, skb %p len %d", chan, skb, skb->len);
+        BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len,
+                                                        skb->priority);
 
         if (!test_bit(FLAG_FLUSHABLE, &chan->flags) &&
                                         lmp_no_flush_capable(hcon->hdev))
@@ -1483,6 +1485,8 @@ static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, in
                 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
                         return -EFAULT;
 
+                (*frag)->priority = skb->priority;
+
                 sent += count;
                 len  -= count;
 
@@ -1492,7 +1496,9 @@ static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, in
         return sent;
 }
 
-static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
+static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan,
+                                                struct msghdr *msg, size_t len,
+                                                u32 priority)
 {
         struct sock *sk = chan->sk;
         struct l2cap_conn *conn = chan->conn;
@@ -1500,7 +1506,7 @@ static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, struct
         int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE;
         struct l2cap_hdr *lh;
 
-        BT_DBG("sk %p len %d", sk, (int)len);
+        BT_DBG("sk %p len %d priority %u", sk, (int)len, priority);
 
         count = min_t(unsigned int, (conn->mtu - hlen), len);
         skb = bt_skb_send_alloc(sk, count + hlen,
@@ -1508,6 +1514,8 @@ static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, struct
         if (!skb)
                 return ERR_PTR(err);
 
+        skb->priority = priority;
+
         /* Create L2CAP header */
         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
         lh->cid = cpu_to_le16(chan->dcid);
@@ -1522,7 +1530,9 @@ static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, struct
         return skb;
 }
 
-static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
+static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan,
+                                                struct msghdr *msg, size_t len,
+                                                u32 priority)
 {
         struct sock *sk = chan->sk;
         struct l2cap_conn *conn = chan->conn;
@@ -1538,6 +1548,8 @@ static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, struct ms
         if (!skb)
                 return ERR_PTR(err);
 
+        skb->priority = priority;
+
         /* Create L2CAP header */
         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
         lh->cid = cpu_to_le16(chan->dcid);
@@ -1651,7 +1663,8 @@ static int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, si
         return size;
 }
 
-int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
+int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
+                                                                u32 priority)
 {
         struct sk_buff *skb;
         u32 control;
@@ -1659,7 +1672,7 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
 
         /* Connectionless channel */
         if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
-                skb = l2cap_create_connless_pdu(chan, msg, len);
+                skb = l2cap_create_connless_pdu(chan, msg, len, priority);
                 if (IS_ERR(skb))
                         return PTR_ERR(skb);
 
@@ -1674,7 +1687,7 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
                         return -EMSGSIZE;
 
                 /* Create a basic PDU */
-                skb = l2cap_create_basic_pdu(chan, msg, len);
+                skb = l2cap_create_basic_pdu(chan, msg, len, priority);
                 if (IS_ERR(skb))
                         return PTR_ERR(skb);
 
diff --git a/net/bluetooth/l2cap_sock.c b/net/bluetooth/l2cap_sock.c
index 836d12e66a38..646aefc4f1d7 100644
--- a/net/bluetooth/l2cap_sock.c
+++ b/net/bluetooth/l2cap_sock.c
@@ -721,7 +721,7 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
                 return -ENOTCONN;
         }
 
-        err = l2cap_chan_send(chan, msg, len);
+        err = l2cap_chan_send(chan, msg, len, sk->sk_priority);
 
         release_sock(sk);
         return err;