Bluetooth: Add support for BT_PKT_STATUS CMSG data for SCO connections

This change adds support for reporting the BT_PKT_STATUS to the socket
CMSG data to allow the implementation of a packet loss correction on
erroneous data received on the SCO socket.

The patch was partially developed by Marcel Holtmann and validated by
Hsin-yu Chao.

Signed-off-by: Alain Michaud <alainm@chromium.org>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>

diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h
index 1819005..7ee8041 100644 (file)
--- a/include/net/bluetooth/bluetooth.h
+++ b/include/net/bluetooth/bluetooth.h
@@ -147,6 +147,10 @@ struct bt_voice {
 #define BT_MODE_LE_FLOWCTL     0x03
 #define BT_MODE_EXT_FLOWCTL    0x04
 
+#define BT_PKT_STATUS          16
+
+#define BT_SCM_PKT_STATUS      0x03
+
 __printf(1, 2)
 void bt_info(const char *fmt, ...);
 __printf(1, 2)
@@ -286,6 +290,7 @@ struct bt_sock {
        struct sock *parent;
        unsigned long flags;
        void (*skb_msg_name)(struct sk_buff *, void *, int *);
+       void (*skb_put_cmsg)(struct sk_buff *, struct msghdr *, struct sock *);
 };
 
 enum {
@@ -335,6 +340,10 @@ struct l2cap_ctrl {
        struct l2cap_chan *chan;
 };
 
+struct sco_ctrl {
+       u8      pkt_status;
+};
+
 struct hci_dev;
 
 typedef void (*hci_req_complete_t)(struct hci_dev *hdev, u8 status, u16 opcode);
@@ -361,6 +370,7 @@ struct bt_skb_cb {
        u8 incoming:1;
        union {
                struct l2cap_ctrl l2cap;
+               struct sco_ctrl sco;
                struct hci_ctrl hci;
        };
 };

diff --git a/include/net/bluetooth/sco.h b/include/net/bluetooth/sco.h
index f40ddb4..1aa2e14 100644 (file)
--- a/include/net/bluetooth/sco.h
+++ b/include/net/bluetooth/sco.h
@@ -46,4 +46,6 @@ struct sco_conninfo {
        __u8  dev_class[3];
 };
 
+#define SCO_CMSG_PKT_STATUS    0x01
+
 #endif /* __SCO_H */

diff --git a/net/bluetooth/af_bluetooth.c b/net/bluetooth/af_bluetooth.c
index b751a7c..4ef6a54 100644 (file)
--- a/net/bluetooth/af_bluetooth.c
+++ b/net/bluetooth/af_bluetooth.c
@@ -286,6 +286,9 @@ int bt_sock_recvmsg(struct socket *sock, struct msghdr *msg, size_t len,
                if (msg->msg_name && bt_sk(sk)->skb_msg_name)
                        bt_sk(sk)->skb_msg_name(skb, msg->msg_name,
                                                &msg->msg_namelen);
+
+               if (bt_sk(sk)->skb_put_cmsg)
+                       bt_sk(sk)->skb_put_cmsg(skb, msg, sk);
        }
 
        skb_free_datagram(sk, skb);

diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 83ce665..00458a8 100644 (file)
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -4554,6 +4554,7 @@ static void hci_scodata_packet(struct hci_dev *hdev, struct sk_buff *skb)
 
        if (conn) {
                /* Send to upper protocol */
+               bt_cb(skb)->sco.pkt_status = flags & 0x03;
                sco_recv_scodata(conn, skb);
                return;
        } else {

diff --git a/net/bluetooth/sco.c b/net/bluetooth/sco.c
index c8c3d38..83a4886 100644 (file)
--- a/net/bluetooth/sco.c
+++ b/net/bluetooth/sco.c
@@ -66,6 +66,7 @@ struct sco_pinfo {
        bdaddr_t        dst;
        __u32           flags;
        __u16           setting;
+       __u8            cmsg_mask;
        struct sco_conn *conn;
 };
 
@@ -449,6 +450,15 @@ static void sco_sock_close(struct sock *sk)
        sco_sock_kill(sk);
 }
 
+static void sco_skb_put_cmsg(struct sk_buff *skb, struct msghdr *msg,
+                            struct sock *sk)
+{
+       if (sco_pi(sk)->cmsg_mask & SCO_CMSG_PKT_STATUS)
+               put_cmsg(msg, SOL_BLUETOOTH, BT_SCM_PKT_STATUS,
+                        sizeof(bt_cb(skb)->sco.pkt_status),
+                        &bt_cb(skb)->sco.pkt_status);
+}
+
 static void sco_sock_init(struct sock *sk, struct sock *parent)
 {
        BT_DBG("sk %p", sk);
@@ -457,6 +467,8 @@ static void sco_sock_init(struct sock *sk, struct sock *parent)
                sk->sk_type = parent->sk_type;
                bt_sk(sk)->flags = bt_sk(parent)->flags;
                security_sk_clone(parent, sk);
+       } else {
+               bt_sk(sk)->skb_put_cmsg = sco_skb_put_cmsg;
        }
 }
 
@@ -846,6 +858,18 @@ static int sco_sock_setsockopt(struct socket *sock, int level, int optname,
                sco_pi(sk)->setting = voice.setting;
                break;
 
+       case BT_PKT_STATUS:
+               if (get_user(opt, (u32 __user *)optval)) {
+                       err = -EFAULT;
+                       break;
+               }
+
+               if (opt)
+                       sco_pi(sk)->cmsg_mask |= SCO_CMSG_PKT_STATUS;
+               else
+                       sco_pi(sk)->cmsg_mask &= SCO_CMSG_PKT_STATUS;
+               break;
+
        default:
                err = -ENOPROTOOPT;
                break;
@@ -923,6 +947,7 @@ static int sco_sock_getsockopt(struct socket *sock, int level, int optname,
        int len, err = 0;
        struct bt_voice voice;
        u32 phys;
+       int pkt_status;
 
        BT_DBG("sk %p", sk);
 
@@ -969,6 +994,13 @@ static int sco_sock_getsockopt(struct socket *sock, int level, int optname,
                        err = -EFAULT;
                break;
 
+       case BT_PKT_STATUS:
+               pkt_status = (sco_pi(sk)->cmsg_mask & SCO_CMSG_PKT_STATUS);
+
+               if (put_user(pkt_status, (int __user *)optval))
+                       err = -EFAULT;
+               break;
+
        default:
                err = -ENOPROTOOPT;
                break;