From 8ac6c7a54ed1b98d142dce24b11c6de6a1e239a5 Mon Sep 17 00:00:00 2001
From: hc <hc@nodka.com>
Date: Tue, 22 Oct 2024 10:36:11 +0000
Subject: [PATCH] 修改4g拨号为QMI,需要在系统里后台执行quectel-CM
---
kernel/net/bluetooth/sco.c | 130 ++++++++++++++++++++++++++++--------------
1 files changed, 86 insertions(+), 44 deletions(-)
diff --git a/kernel/net/bluetooth/sco.c b/kernel/net/bluetooth/sco.c
index 1e0a1c0..8244d3a 100644
--- a/kernel/net/bluetooth/sco.c
+++ b/kernel/net/bluetooth/sco.c
@@ -68,6 +68,7 @@
bdaddr_t dst;
__u32 flags;
__u16 setting;
+ __u8 cmsg_mask;
struct sco_conn *conn;
};
@@ -279,12 +280,10 @@
return err;
}
-static int sco_send_frame(struct sock *sk, void *buf, int len,
- unsigned int msg_flags)
+static int sco_send_frame(struct sock *sk, struct sk_buff *skb)
{
struct sco_conn *conn = sco_pi(sk)->conn;
- struct sk_buff *skb;
- int err;
+ int len = skb->len;
/* Check outgoing MTU */
if (len > conn->mtu)
@@ -292,11 +291,6 @@
BT_DBG("sk %p len %d", sk, len);
- skb = bt_skb_send_alloc(sk, len, msg_flags & MSG_DONTWAIT, &err);
- if (!skb)
- return err;
-
- memcpy(skb_put(skb, len), buf, len);
hci_send_sco(conn->hcon, skb);
return len;
@@ -451,6 +445,15 @@
release_sock(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);
@@ -459,6 +462,8 @@
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;
}
}
@@ -523,11 +528,11 @@
struct sock *sk = sock->sk;
int err = 0;
- BT_DBG("sk %p %pMR", sk, &sa->sco_bdaddr);
-
if (!addr || addr_len < sizeof(struct sockaddr_sco) ||
addr->sa_family != AF_BLUETOOTH)
return -EINVAL;
+
+ BT_DBG("sk %p %pMR", sk, &sa->sco_bdaddr);
lock_sock(sk);
@@ -563,18 +568,23 @@
addr->sa_family != AF_BLUETOOTH)
return -EINVAL;
- if (sk->sk_state != BT_OPEN && sk->sk_state != BT_BOUND)
- return -EBADFD;
+ lock_sock(sk);
+ if (sk->sk_state != BT_OPEN && sk->sk_state != BT_BOUND) {
+ err = -EBADFD;
+ goto done;
+ }
- if (sk->sk_type != SOCK_SEQPACKET)
- return -EINVAL;
+ if (sk->sk_type != SOCK_SEQPACKET) {
+ err = -EINVAL;
+ goto done;
+ }
hdev = hci_get_route(&sa->sco_bdaddr, &sco_pi(sk)->src, BDADDR_BREDR);
- if (!hdev)
- return -EHOSTUNREACH;
+ if (!hdev) {
+ err = -EHOSTUNREACH;
+ goto done;
+ }
hci_dev_lock(hdev);
-
- lock_sock(sk);
/* Set destination address and psm */
bacpy(&sco_pi(sk)->dst, &sa->sco_bdaddr);
@@ -710,7 +720,7 @@
size_t len)
{
struct sock *sk = sock->sk;
- void *buf;
+ struct sk_buff *skb;
int err;
BT_DBG("sock %p, sk %p", sock, sk);
@@ -722,24 +732,21 @@
if (msg->msg_flags & MSG_OOB)
return -EOPNOTSUPP;
- buf = kmalloc(len, GFP_KERNEL);
- if (!buf)
- return -ENOMEM;
-
- if (memcpy_from_msg(buf, msg, len)) {
- kfree(buf);
- return -EFAULT;
- }
+ skb = bt_skb_sendmsg(sk, msg, len, len, 0, 0);
+ if (IS_ERR(skb))
+ return PTR_ERR(skb);
lock_sock(sk);
if (sk->sk_state == BT_CONNECTED)
- err = sco_send_frame(sk, buf, len, msg->msg_flags);
+ err = sco_send_frame(sk, skb);
else
err = -ENOTCONN;
release_sock(sk);
- kfree(buf);
+
+ if (err < 0)
+ kfree_skb(skb);
return err;
}
@@ -815,7 +822,7 @@
}
static int sco_sock_setsockopt(struct socket *sock, int level, int optname,
- char __user *optval, unsigned int optlen)
+ sockptr_t optval, unsigned int optlen)
{
struct sock *sk = sock->sk;
int len, err = 0;
@@ -834,7 +841,7 @@
break;
}
- if (get_user(opt, (u32 __user *) optval)) {
+ if (copy_from_sockptr(&opt, optval, sizeof(u32))) {
err = -EFAULT;
break;
}
@@ -855,7 +862,7 @@
voice.setting = sco_pi(sk)->setting;
len = min_t(unsigned int, sizeof(voice), optlen);
- if (copy_from_user((char *)&voice, optval, len)) {
+ if (copy_from_sockptr(&voice, optval, len)) {
err = -EFAULT;
break;
}
@@ -868,6 +875,18 @@
}
sco_pi(sk)->setting = voice.setting;
+ break;
+
+ case BT_PKT_STATUS:
+ if (copy_from_sockptr(&opt, optval, sizeof(u32))) {
+ 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:
@@ -946,6 +965,8 @@
struct sock *sk = sock->sk;
int len, err = 0;
struct bt_voice voice;
+ u32 phys;
+ int pkt_status;
BT_DBG("sk %p", sk);
@@ -978,6 +999,36 @@
if (copy_to_user(optval, (char *)&voice, len))
err = -EFAULT;
+ break;
+
+ case BT_PHY:
+ if (sk->sk_state != BT_CONNECTED) {
+ err = -ENOTCONN;
+ break;
+ }
+
+ phys = hci_conn_get_phy(sco_pi(sk)->conn->hcon);
+
+ if (put_user(phys, (u32 __user *) optval))
+ 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;
+
+ case BT_SNDMTU:
+ case BT_RCVMTU:
+ if (sk->sk_state != BT_CONNECTED) {
+ err = -ENOTCONN;
+ break;
+ }
+
+ if (put_user(sco_pi(sk)->conn->mtu, (u32 __user *)optval))
+ err = -EFAULT;
break;
default:
@@ -1197,17 +1248,7 @@
return 0;
}
-static int sco_debugfs_open(struct inode *inode, struct file *file)
-{
- return single_open(file, sco_debugfs_show, inode->i_private);
-}
-
-static const struct file_operations sco_debugfs_fops = {
- .open = sco_debugfs_open,
- .read = seq_read,
- .llseek = seq_lseek,
- .release = single_release,
-};
+DEFINE_SHOW_ATTRIBUTE(sco_debugfs);
static struct dentry *sco_debugfs;
@@ -1224,6 +1265,7 @@
.recvmsg = sco_sock_recvmsg,
.poll = bt_sock_poll,
.ioctl = bt_sock_ioctl,
+ .gettstamp = sock_gettstamp,
.mmap = sock_no_mmap,
.socketpair = sock_no_socketpair,
.shutdown = sco_sock_shutdown,
--
Gitblit v1.6.2