forked from ~ljy/RK356X_SDK_RELEASE

blame | history | patch | commit | commitdiff
hc
2024-05-14 bedbef8ad3e75a304af6361af235302bcc61d06b 
 kernel/net/bluetooth/sco.c
....@@ -68,6 +68,7 @@
6868 	bdaddr_t	dst;
6969 	__u32		flags;
7070 	__u16		setting;
71
+	__u8		cmsg_mask;
7172 	struct sco_conn	*conn;
7273 };
7374 
....@@ -279,12 +280,10 @@
279280 	return err;
280281 }
281282 
282
-static int sco_send_frame(struct sock *sk, void *buf, int len,
283
-			  unsigned int msg_flags)
283
+static int sco_send_frame(struct sock *sk, struct sk_buff *skb)
284284 {
285285 	struct sco_conn *conn = sco_pi(sk)->conn;
286
-	struct sk_buff *skb;
287
-	int err;
286
+	int len = skb->len;
288287 
289288 	/* Check outgoing MTU */
290289 	if (len > conn->mtu)
....@@ -292,11 +291,6 @@
292291 
293292 	BT_DBG("sk %p len %d", sk, len);
294293 
295
-	skb = bt_skb_send_alloc(sk, len, msg_flags & MSG_DONTWAIT, &err);
296
-	if (!skb)
297
-		return err;
298
-
299
-	memcpy(skb_put(skb, len), buf, len);
300294 	hci_send_sco(conn->hcon, skb);
301295 
302296 	return len;
....@@ -451,6 +445,15 @@
451445 	release_sock(sk);
452446 }
453447 
448
+static void sco_skb_put_cmsg(struct sk_buff *skb, struct msghdr *msg,
449
+			     struct sock *sk)
450
+{
451
+	if (sco_pi(sk)->cmsg_mask & SCO_CMSG_PKT_STATUS)
452
+		put_cmsg(msg, SOL_BLUETOOTH, BT_SCM_PKT_STATUS,
453
+			 sizeof(bt_cb(skb)->sco.pkt_status),
454
+			 &bt_cb(skb)->sco.pkt_status);
455
+}
456
+
454457 static void sco_sock_init(struct sock *sk, struct sock *parent)
455458 {
456459 	BT_DBG("sk %p", sk);
....@@ -459,6 +462,8 @@
459462 		sk->sk_type = parent->sk_type;
460463 		bt_sk(sk)->flags = bt_sk(parent)->flags;
461464 		security_sk_clone(parent, sk);
465
+	} else {
466
+		bt_sk(sk)->skb_put_cmsg = sco_skb_put_cmsg;
462467 	}
463468 }
464469 
....@@ -523,11 +528,11 @@
523528 	struct sock *sk = sock->sk;
524529 	int err = 0;
525530 
526
-	BT_DBG("sk %p %pMR", sk, &sa->sco_bdaddr);
527
-
528531 	if (!addr || addr_len < sizeof(struct sockaddr_sco) ||
529532 	    addr->sa_family != AF_BLUETOOTH)
530533 		return -EINVAL;
534
+
535
+	BT_DBG("sk %p %pMR", sk, &sa->sco_bdaddr);
531536 
532537 	lock_sock(sk);
533538 
....@@ -563,18 +568,23 @@
563568 	    addr->sa_family != AF_BLUETOOTH)
564569 		return -EINVAL;
565570 
566
-	if (sk->sk_state != BT_OPEN && sk->sk_state != BT_BOUND)
567
-		return -EBADFD;
571
+	lock_sock(sk);
572
+	if (sk->sk_state != BT_OPEN && sk->sk_state != BT_BOUND) {
573
+		err = -EBADFD;
574
+		goto done;
575
+	}
568576 
569
-	if (sk->sk_type != SOCK_SEQPACKET)
570
-		return -EINVAL;
577
+	if (sk->sk_type != SOCK_SEQPACKET) {
578
+		err = -EINVAL;
579
+		goto done;
580
+	}
571581 
572582 	hdev = hci_get_route(&sa->sco_bdaddr, &sco_pi(sk)->src, BDADDR_BREDR);
573
-	if (!hdev)
574
-		return -EHOSTUNREACH;
583
+	if (!hdev) {
584
+		err = -EHOSTUNREACH;
585
+		goto done;
586
+	}
575587 	hci_dev_lock(hdev);
576
-
577
-	lock_sock(sk);
578588 
579589 	/* Set destination address and psm */
580590 	bacpy(&sco_pi(sk)->dst, &sa->sco_bdaddr);
....@@ -710,7 +720,7 @@
710720 			    size_t len)
711721 {
712722 	struct sock *sk = sock->sk;
713
-	void *buf;
723
+	struct sk_buff *skb;
714724 	int err;
715725 
716726 	BT_DBG("sock %p, sk %p", sock, sk);
....@@ -722,24 +732,21 @@
722732 	if (msg->msg_flags & MSG_OOB)
723733 		return -EOPNOTSUPP;
724734 
725
-	buf = kmalloc(len, GFP_KERNEL);
726
-	if (!buf)
727
-		return -ENOMEM;
728
-
729
-	if (memcpy_from_msg(buf, msg, len)) {
730
-		kfree(buf);
731
-		return -EFAULT;
732
-	}
735
+	skb = bt_skb_sendmsg(sk, msg, len, len, 0, 0);
736
+	if (IS_ERR(skb))
737
+		return PTR_ERR(skb);
733738 
734739 	lock_sock(sk);
735740 
736741 	if (sk->sk_state == BT_CONNECTED)
737
-		err = sco_send_frame(sk, buf, len, msg->msg_flags);
742
+		err = sco_send_frame(sk, skb);
738743 	else
739744 		err = -ENOTCONN;
740745 
741746 	release_sock(sk);
742
-	kfree(buf);
747
+
748
+	if (err < 0)
749
+		kfree_skb(skb);
743750 	return err;
744751 }
745752 
....@@ -815,7 +822,7 @@
815822 }
816823 
817824 static int sco_sock_setsockopt(struct socket *sock, int level, int optname,
818
-			       char __user *optval, unsigned int optlen)
825
+			       sockptr_t optval, unsigned int optlen)
819826 {
820827 	struct sock *sk = sock->sk;
821828 	int len, err = 0;
....@@ -834,7 +841,7 @@
834841 			break;
835842 		}
836843 
837
-		if (get_user(opt, (u32 __user *) optval)) {
844
+		if (copy_from_sockptr(&opt, optval, sizeof(u32))) {
838845 			err = -EFAULT;
839846 			break;
840847 		}
....@@ -855,7 +862,7 @@
855862 		voice.setting = sco_pi(sk)->setting;
856863 
857864 		len = min_t(unsigned int, sizeof(voice), optlen);
858
-		if (copy_from_user((char *)&voice, optval, len)) {
865
+		if (copy_from_sockptr(&voice, optval, len)) {
859866 			err = -EFAULT;
860867 			break;
861868 		}
....@@ -868,6 +875,18 @@
868875 		}
869876 
870877 		sco_pi(sk)->setting = voice.setting;
878
+		break;
879
+
880
+	case BT_PKT_STATUS:
881
+		if (copy_from_sockptr(&opt, optval, sizeof(u32))) {
882
+			err = -EFAULT;
883
+			break;
884
+		}
885
+
886
+		if (opt)
887
+			sco_pi(sk)->cmsg_mask |= SCO_CMSG_PKT_STATUS;
888
+		else
889
+			sco_pi(sk)->cmsg_mask &= SCO_CMSG_PKT_STATUS;
871890 		break;
872891 
873892 	default:
....@@ -946,6 +965,8 @@
946965 	struct sock *sk = sock->sk;
947966 	int len, err = 0;
948967 	struct bt_voice voice;
968
+	u32 phys;
969
+	int pkt_status;
949970 
950971 	BT_DBG("sk %p", sk);
951972 
....@@ -978,6 +999,36 @@
978999 		if (copy_to_user(optval, (char *)&voice, len))
9791000 			err = -EFAULT;
9801001 
1002
+		break;
1003
+
1004
+	case BT_PHY:
1005
+		if (sk->sk_state != BT_CONNECTED) {
1006
+			err = -ENOTCONN;
1007
+			break;
1008
+		}
1009
+
1010
+		phys = hci_conn_get_phy(sco_pi(sk)->conn->hcon);
1011
+
1012
+		if (put_user(phys, (u32 __user *) optval))
1013
+			err = -EFAULT;
1014
+		break;
1015
+
1016
+	case BT_PKT_STATUS:
1017
+		pkt_status = (sco_pi(sk)->cmsg_mask & SCO_CMSG_PKT_STATUS);
1018
+
1019
+		if (put_user(pkt_status, (int __user *)optval))
1020
+			err = -EFAULT;
1021
+		break;
1022
+
1023
+	case BT_SNDMTU:
1024
+	case BT_RCVMTU:
1025
+		if (sk->sk_state != BT_CONNECTED) {
1026
+			err = -ENOTCONN;
1027
+			break;
1028
+		}
1029
+
1030
+		if (put_user(sco_pi(sk)->conn->mtu, (u32 __user *)optval))
1031
+			err = -EFAULT;
9811032 		break;
9821033 
9831034 	default:
....@@ -1197,17 +1248,7 @@
11971248 	return 0;
11981249 }
11991250 
1200
-static int sco_debugfs_open(struct inode *inode, struct file *file)
1201
-{
1202
-	return single_open(file, sco_debugfs_show, inode->i_private);
1203
-}
1204
-
1205
-static const struct file_operations sco_debugfs_fops = {
1206
-	.open		= sco_debugfs_open,
1207
-	.read		= seq_read,
1208
-	.llseek		= seq_lseek,
1209
-	.release	= single_release,
1210
-};
1251
+DEFINE_SHOW_ATTRIBUTE(sco_debugfs);
12111252 
12121253 static struct dentry *sco_debugfs;
12131254 
....@@ -1224,6 +1265,7 @@
12241265 	.recvmsg	= sco_sock_recvmsg,
12251266 	.poll		= bt_sock_poll,
12261267 	.ioctl		= bt_sock_ioctl,
1268
+	.gettstamp	= sock_gettstamp,
12271269 	.mmap		= sock_no_mmap,
12281270 	.socketpair	= sock_no_socketpair,
12291271 	.shutdown	= sco_sock_shutdown,