Attachment #124667 for bug #91558

Lines 25-30 Link Here

(-)a/include/net/bluetooth/hci_core.h (-3 / +5 lines)
25	#ifndef __HCI_CORE_H	25	#ifndef __HCI_CORE_H
26	#define __HCI_CORE_H	26	#define __HCI_CORE_H
27		27
		28	#include <linux/hrtimer.h>
		29
28	#include <net/bluetooth/hci.h>	30	#include <net/bluetooth/hci.h>
29		31
30	/* HCI upper protocols */	32	/* HCI upper protocols */
Lines 89-95 struct hci_dev { Link Here
89		91
90	atomic_t cmd_cnt;	92	atomic_t cmd_cnt;
91	unsigned int acl_cnt;	93	unsigned int acl_cnt;
92	unsigned int sco_cnt;	94	atomic_t sco_cnt;
93		95
94	unsigned int acl_mtu;	96	unsigned int acl_mtu;
95	unsigned int sco_mtu;	97	unsigned int sco_mtu;
Lines 145-151 struct hci_conn { Link Here
145	struct list_head list;	147	struct list_head list;
146		148
147	atomic_t refcnt;	149	atomic_t refcnt;
148	spinlock_t lock;
149		150
150	bdaddr_t dst;	151	bdaddr_t dst;
151	__u16 handle;	152	__u16 handle;
Lines 162-171 struct hci_conn { Link Here
162	__u8 power_save;	163	__u8 power_save;
163	unsigned long pend;	164	unsigned long pend;
164		165
165	unsigned int sent;	166	atomic_t sent;
166		167
167	struct sk_buff_head data_q;	168	struct sk_buff_head data_q;
168		169
		170	struct hrtimer tx_timer;
169	struct timer_list disc_timer;	171	struct timer_list disc_timer;
170	struct timer_list idle_timer;	172	struct timer_list idle_timer;
171		173




#define __SCO_H

/* SCO defaults */
#define SCO_DEFAULT_MTU		500
#define SCO_DEFAULT_FLUSH_TO	0xFFFF

#define SCO_CONN_TIMEOUT	(HZ * 40)
#define SCO_DISCONN_TIMEOUT	(HZ * 2)
#define SCO_CONN_IDLE_TIMEOUT	(HZ * 60)

/* SCO socket address */
struct sockaddr_sco {




	hci_conn_enter_sniff_mode(conn);
}

static  enum hrtimer_restart hci_sco_tx_timer(struct hrtimer *timer)
{
	struct hci_conn *conn = container_of(timer, struct hci_conn, tx_timer);
#ifdef CONFIG_BT_HCI_CORE_DEBUG
	ktime_t now = timer->base->get_time();

	BT_DBG("%s, conn %p, time %5lu.%06lu", conn->hdev->name, conn,
		(unsigned long) now.tv64, 
		do_div(now.tv64, NSEC_PER_SEC) / 1000);
#endif

	if (atomic_read(&conn->sent) > 0) {
		atomic_dec(&conn->sent);
		atomic_inc(&conn->hdev->sco_cnt);
		hci_sched_tx(conn->hdev);
	}
	return HRTIMER_NORESTART;
}
 

struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst)
{
	struct hci_conn *conn;


	skb_queue_head_init(&conn->data_q);

	hrtimer_init(&conn->tx_timer, CLOCK_MONOTONIC, HRTIMER_NORESTART);

	if(type == SCO_LINK)
		conn->tx_timer.function = hci_sco_tx_timer;

	init_timer(&conn->disc_timer);
	conn->disc_timer.function = hci_conn_timeout;
	conn->disc_timer.data = (unsigned long) conn;

	conn->idle_timer.data = (unsigned long) conn;

	atomic_set(&conn->refcnt, 0);
	atomic_set(&conn->sent, 0);

	hci_dev_hold(hdev);



	del_timer(&conn->disc_timer);

	hrtimer_cancel(&conn->tx_timer);

	if (conn->type == SCO_LINK) {
		struct hci_conn *acl = conn->link;
		if (acl) {

			sco->link = NULL;

		/* Unacked frames */
		hdev->acl_cnt += atomic_read(&conn->sent);
	}

	tasklet_disable(&hdev->tx_task);




		hdev->flush(hdev);

	atomic_set(&hdev->cmd_cnt, 1);
	atomic_set(&hdev->sco_cnt, 0);
	hdev->acl_cnt = 0;

	if (!test_bit(HCI_RAW, &hdev->flags))
		ret = __hci_request(hdev, hci_reset_req, 0,

{
	struct hci_dev *hdev = conn->hdev;
	struct hci_sco_hdr hdr;
	ktime_t now;

	BT_DBG("%s len %d", hdev->name, skb->len);


		return -EINVAL;
	}

	now = conn->tx_timer.base->get_time();

	/* force a clean start for 100 ms or later underrun */
	if (conn->tx_timer.expires.tv64 + NSEC_PER_SEC / 10 <= now.tv64) {
		conn->tx_timer.expires = now;
	}

	hdr.handle = cpu_to_le16(conn->handle);
	hdr.dlen   = skb->len;



/* ---- HCI TX task (outgoing data) ---- */

/* HCI ACL Connection scheduler */
static inline struct hci_conn *hci_low_sent_acl(struct hci_dev *hdev, int *quote)
{
	struct hci_conn_hash *h = &hdev->conn_hash;
	struct hci_conn  *conn = NULL;
	unsigned int num = 0, min = ~0;
	struct list_head *p;

	/* We don't have to lock device here. Connections are always

		struct hci_conn *c;
		c = list_entry(p, struct hci_conn, list);

		BT_DBG("c->type %d c->state %d len(c->data_q) %d min %d c->sent %d", 
			c->type, c->state, skb_queue_len(&c->data_q), min, atomic_read(&c->sent));

		if (c->type != ACL_LINK || c->state != BT_CONNECTED
				|| skb_queue_empty(&c->data_q))
			continue;
		num++;

		if (atomic_read(&c->sent) < min) {
			min  = atomic_read(&c->sent);
			conn = c;
		}
	}

	if (conn) {
		int q = hdev->acl_cnt / num;
		int q = cnt / num;
		*quote = q ? q : 1;
	} else
		*quote = 0;

	/* Kill stalled connections */
	list_for_each(p, &h->list) {
		c = list_entry(p, struct hci_conn, list);
		if (c->type == ACL_LINK && atomic_read(&c->sent)) {
			BT_ERR("%s killing stalled ACL connection %s",
				hdev->name, batostr(&c->dst));
			hci_acl_disconn(c, 0x13);

			hci_acl_tx_to(hdev);
	}

	while (hdev->acl_cnt && (conn = hci_low_sent_acl(hdev, &quote))) {
		while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
			BT_DBG("skb %p len %d", skb, skb->len);


			hdev->acl_last_tx = jiffies;

			hdev->acl_cnt--;
			atomic_inc(&conn->sent);
		}
	}
}

/* HCI SCO Connection scheduler */

static inline void hci_sched_sco(struct hci_dev *hdev)
{
	struct hci_conn_hash *h = &hdev->conn_hash;
	struct sk_buff *skb;
	struct list_head *p;
	struct hci_conn *c;
	ktime_t now, pkt_time;
	
	BT_DBG("%s", hdev->name);
  
	/* We don't have to lock device here. Connections are always 
	   added and removed with TX task disabled. */
	list_for_each(p, &h->list) {
		c = list_entry(p, struct hci_conn, list);
  
		/* SCO scheduling algorithm makes sure there is never more than
		   1 outstanding packet for each connection */

		if (c->type != SCO_LINK)
			continue;

		if (atomic_read(&c->sent) >= 1)
			continue;

		if (c->state != BT_CONNECTED)
			continue;

		if (atomic_read(&hdev->sco_cnt) <= 0)
			continue;

		if ((skb = skb_dequeue(&c->data_q)) == NULL)
			continue;

		hci_send_frame(skb);
 
		atomic_inc(&c->sent);			
		atomic_dec(&hdev->sco_cnt);
					
		pkt_time = ktime_set(0, NSEC_PER_SEC / 16000 * (skb->len - HCI_SCO_HDR_SIZE));
		now = c->tx_timer.base->get_time();

		c->tx_timer.expires.tv64 += pkt_time.tv64;
		if (c->tx_timer.expires.tv64 > now.tv64) {
			hrtimer_restart(&c->tx_timer);
		} else {
			/* Timer is to expire in the past - force timer expiration.
			   this can happen if timer base precision is less than pkt_time */
			c->tx_timer.function(&c->tx_timer);
		}
	}
}


	read_lock(&hci_task_lock);

	BT_DBG("%s acl %d sco %d", hdev->name, hdev->acl_cnt, atomic_read(&hdev->sco_cnt));

	/* Schedule queues and send stuff to HCI driver */

	hci_sched_acl(hdev);

	hci_sched_sco(hdev);

	hci_sched_acl(hdev);

	/* Send next queued raw (unknown type) packet */
	while ((skb = skb_dequeue(&hdev->raw_q)))
		hci_send_frame(skb);




		lv = (struct hci_rp_read_loc_version *) skb->data;

		if (lv->status) {
			BT_DBG("%s READ_LOCAL_VERSION failed %d", hdev->name, lv->status);
			break;
		}


		}

		hdev->acl_cnt = hdev->acl_pkts;
		atomic_set(&hdev->sco_cnt, hdev->sco_pkts);

		BT_DBG("%s mtu: acl %d, sco %d max_pkt: acl %d, sco %d", hdev->name,
			hdev->acl_mtu, hdev->sco_mtu, hdev->acl_pkts, hdev->sco_pkts);


		conn = hci_conn_hash_lookup_handle(hdev, handle);
		if (conn) {
			atomic_sub(count, &conn->sent);

			if (conn->type == ACL_LINK) {
				if ((hdev->sco_cnt += count) > hdev->sco_pkts)
					hdev->sco_cnt = hdev->sco_pkts;
			} else {
				if ((hdev->acl_cnt += count) > hdev->acl_pkts)
					hdev->acl_cnt = hdev->acl_pkts;
			}

Lines 53-59 #undef BT_DBG Link Here

(-)a/net/bluetooth/sco.c (-16 / +120 lines)
53	#define BT_DBG(D...)	53	#define BT_DBG(D...)
54	#endif	54	#endif
55		55
56	#define VERSION "0.5"	56	#define VERSION "0.6"
		57
		58	#define MAX_SCO_TXBUFS 200
		59	#define MAX_SCO_RXBUFS 200
		60
		61	#define DEFAULT_SCO_TXBUFS 5
		62	#define DEFAULT_SCO_RXBUFS 5
57		63
58	static const struct proto_ops sco_sock_ops;	64	static const struct proto_ops sco_sock_ops;
59		65
Lines 69-74 static int sco_conn_del(struct hci_conn Link Here
69	static void sco_sock_close(struct sock *sk);	75	static void sco_sock_close(struct sock *sk);
70	static void sco_sock_kill(struct sock *sk);	76	static void sco_sock_kill(struct sock *sk);
71		77
		78	/*
		79	* Write buffer destructor automatically called from kfree_skb.
		80	*/
		81	void sco_sock_wfree(struct sk_buff *skb)
		82	{
		83	struct sock *sk = skb->sk;
		84
		85	atomic_dec(&sk->sk_wmem_alloc);
		86	sk->sk_write_space(sk);
		87	sock_put(sk);
		88	}
		89
		90	static void sco_sock_write_space(struct sock *sk)
		91	{
		92	read_lock(&sk->sk_callback_lock);
		93
		94	if (atomic_read(&sk->sk_wmem_alloc) < sk->sk_sndbuf) {
		95	if (sk->sk_sleep && waitqueue_active(sk->sk_sleep))
		96	wake_up_interruptible(sk->sk_sleep);
		97
		98	if (sock_writeable(sk))
		99	sk_wake_async(sk, 2, POLL_OUT);
		100	}
		101
		102	read_unlock(&sk->sk_callback_lock);
		103	}
		104
72	/* ---- SCO timers ---- */	105	/* ---- SCO timers ---- */
73	static void sco_sock_timeout(unsigned long arg)	106	static void sco_sock_timeout(unsigned long arg)
74	{	107	{
Lines 234-266 static inline int sco_send_frame(struct Link Here
234	{	267	{
235	struct sco_conn *conn = sco_pi(sk)->conn;	268	struct sco_conn *conn = sco_pi(sk)->conn;
236	struct sk_buff *skb;	269	struct sk_buff *skb;
237	int err, count;	270	int err;
238
239	/* Check outgoing MTU */
240	if (len > conn->mtu)
241	return -EINVAL;
242		271
243	BT_DBG("sk %p len %d", sk, len);	272	BT_DBG("sk %p len %d", sk, len);
244		273
245	count = min_t(unsigned int, conn->mtu, len);	274	if (!(skb = bt_skb_send_alloc(sk, len, msg->msg_flags & MSG_DONTWAIT, &err)))
246	if (!(skb = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err)))
247	return err;	275	return err;
248		276
249	if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {	277	/* fix sk_wmem_alloc value : by default it is increased by skb->truesize, but
		278	we want it only increased by 1 */
		279	atomic_sub(skb->truesize - 1, &sk->sk_wmem_alloc);
		280	/* fix destructor */
		281	skb->destructor = sco_sock_wfree;
		282
		283	if (memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len)) {
250	err = -EFAULT;	284	err = -EFAULT;
251	goto fail;	285	goto fail;
252	}	286	}
253		287
254	if ((err = hci_send_sco(conn->hcon, skb)) < 0)	288	err = hci_send_sco(conn->hcon, skb);
255	return err;	289
		290	if (err < 0)
		291	goto fail;
256		292
257	return count;	293	return len;
258		294
259	fail:	295	fail:
260	kfree_skb(skb);	296	kfree_skb(skb);
261	return err;	297	return err;
262	}	298	}
263		299
		300	static int sco_sock_queue_rcv_skb(struct sock sk, struct sk_buff skb)
		301	{
		302	BT_DBG("sock %p, sk_rcvbuf %d, qlen %d", sk, sk->sk_rcvbuf, skb_queue_len(&sk->sk_receive_queue));
		303
		304	if (skb_queue_len(&sk->sk_receive_queue) + 1 > (unsigned)sk->sk_rcvbuf)
		305	return -ENOMEM;
		306
		307	skb->dev = NULL;
		308	skb->sk = sk;
		309	skb->destructor = NULL;
		310
		311	skb_queue_tail(&sk->sk_receive_queue, skb);
		312
		313	if (!sock_flag(sk, SOCK_DEAD))
		314	sk->sk_data_ready(sk, 1);
		315
		316	return 0;
		317	}
		318
264	static inline void sco_recv_frame(struct sco_conn conn, struct sk_buff skb)	319	static inline void sco_recv_frame(struct sco_conn conn, struct sk_buff skb)
265	{	320	{
266	struct sock *sk = sco_chan_get(conn);	321	struct sock *sk = sco_chan_get(conn);
Lines 273-279 static inline void sco_recv_frame(struct Link Here
273	if (sk->sk_state != BT_CONNECTED)	328	if (sk->sk_state != BT_CONNECTED)
274	goto drop;	329	goto drop;
275		330
276	if (!sock_queue_rcv_skb(sk, skb))	331	if (!sco_sock_queue_rcv_skb(sk, skb))
277	return;	332	return;
278		333
279	drop:	334	drop:
Lines 328-334 static void sco_sock_destruct(struct soc Link Here
328	BT_DBG("sk %p", sk);	383	BT_DBG("sk %p", sk);
329		384
330	skb_queue_purge(&sk->sk_receive_queue);	385	skb_queue_purge(&sk->sk_receive_queue);
331	skb_queue_purge(&sk->sk_write_queue);
332	}	386	}
333		387
334	static void sco_sock_cleanup_listen(struct sock *parent)	388	static void sco_sock_cleanup_listen(struct sock *parent)
Lines 426-431 static struct sock *sco_sock_alloc(struc Link Here
426	INIT_LIST_HEAD(&bt_sk(sk)->accept_q);	480	INIT_LIST_HEAD(&bt_sk(sk)->accept_q);
427		481
428	sk->sk_destruct = sco_sock_destruct;	482	sk->sk_destruct = sco_sock_destruct;
		483	sk->sk_write_space = sco_sock_write_space;
		484
		485	sk->sk_sndbuf = DEFAULT_SCO_TXBUFS;
		486	sk->sk_rcvbuf = DEFAULT_SCO_RXBUFS;
429	sk->sk_sndtimeo = SCO_CONN_TIMEOUT;	487	sk->sk_sndtimeo = SCO_CONN_TIMEOUT;
430		488
431	sock_reset_flag(sk, SOCK_ZAPPED);	489	sock_reset_flag(sk, SOCK_ZAPPED);
Lines 656-661 static int sco_sock_sendmsg(struct kiocb Link Here
656	static int sco_sock_setsockopt(struct socket sock, int level, int optname, char __user optval, int optlen)	714	static int sco_sock_setsockopt(struct socket sock, int level, int optname, char __user optval, int optlen)
657	{	715	{
658	struct sock *sk = sock->sk;	716	struct sock *sk = sock->sk;
		717	u32 opt;
659	int err = 0;	718	int err = 0;
660		719
661	BT_DBG("sk %p", sk);	720	BT_DBG("sk %p", sk);
Lines 663-668 static int sco_sock_setsockopt(struct so Link Here
663	lock_sock(sk);	722	lock_sock(sk);
664		723
665	switch (optname) {	724	switch (optname) {
		725	case SO_SNDBUF:
		726	if (get_user(opt, (u32 __user *) optval)) {
		727	err = -EFAULT;
		728	break;
		729	}
		730	if (opt > MAX_SCO_TXBUFS) {
		731	err = -EINVAL;
		732	break;
		733	}
		734
		735	sk->sk_sndbuf = opt;
		736	/* Wake up sending tasks if we upped the value */
		737	sk->sk_write_space(sk);
		738	break;
		739	case SO_RCVBUF:
		740	if (get_user(opt, (u32 __user *) optval)) {
		741	err = -EFAULT;
		742	break;
		743	}
		744	if (opt > MAX_SCO_RXBUFS) {
		745	err = -EINVAL;
		746	break;
		747	}
		748
		749	sk->sk_rcvbuf = opt;
		750	break;
666	default:	751	default:
667	err = -ENOPROTOOPT;	752	err = -ENOPROTOOPT;
668	break;	753	break;
Lines 678-683 static int sco_sock_getsockopt(struct so Link Here
678	struct sco_options opts;	763	struct sco_options opts;
679	struct sco_conninfo cinfo;	764	struct sco_conninfo cinfo;
680	int len, err = 0;	765	int len, err = 0;
		766	int val;
681		767
682	BT_DBG("sk %p", sk);	768	BT_DBG("sk %p", sk);
683		769
Lines 687-692 static int sco_sock_getsockopt(struct so Link Here
687	lock_sock(sk);	773	lock_sock(sk);
688		774
689	switch (optname) {	775	switch (optname) {
		776	case SO_RCVBUF:
		777	val = sk->sk_rcvbuf;
		778
		779	len = min_t(unsigned int, len, sizeof(val));
		780	if (copy_to_user(optval, (char *) &val, len))
		781	err = -EFAULT;
		782
		783	break;
		784
		785	case SO_SNDBUF:
		786	val = sk->sk_sndbuf;
		787
		788	len = min_t(unsigned int, len, sizeof(val));
		789	if (copy_to_user(optval, (char *) &val, len))
		790	err = -EFAULT;
		791
		792	break;
		793
690	case SCO_OPTIONS:	794	case SCO_OPTIONS:
691	if (sk->sk_state != BT_CONNECTED) {	795	if (sk->sk_state != BT_CONNECTED) {
692	err = -ENOTCONN;	796	err = -ENOTCONN;
Lines 698-704 static int sco_sock_getsockopt(struct so Link Here
698	BT_DBG("mtu %d", opts.mtu);	802	BT_DBG("mtu %d", opts.mtu);
699		803
700	len = min_t(unsigned int, len, sizeof(opts));	804	len = min_t(unsigned int, len, sizeof(opts));
701	if (copy_to_user(optval, (char *)&opts, len))	805	if (copy_to_user(optval, (char *) &opts, len))
702	err = -EFAULT;	806	err = -EFAULT;
703		807
704	break;	808	break;
Lines 713-719 static int sco_sock_getsockopt(struct so Link Here
713	memcpy(cinfo.dev_class, sco_pi(sk)->conn->hcon->dev_class, 3);	817	memcpy(cinfo.dev_class, sco_pi(sk)->conn->hcon->dev_class, 3);
714		818
715	len = min_t(unsigned int, len, sizeof(cinfo));	819	len = min_t(unsigned int, len, sizeof(cinfo));
716	if (copy_to_user(optval, (char *)&cinfo, len))	820	if (copy_to_user(optval, (char *) &cinfo, len))
717	err = -EFAULT;	821	err = -EFAULT;
718		822
719	break;	823	break;

Return to bug 91558

Lines 620-626 int hci_dev_reset(__u16 dev) Link Here

(-)a/net/bluetooth/hci_core.c (-26 / +70 lines)
620	hdev->flush(hdev);	620	hdev->flush(hdev);
621		621
622	atomic_set(&hdev->cmd_cnt, 1);	622	atomic_set(&hdev->cmd_cnt, 1);
623	hdev->acl_cnt = 0; hdev->sco_cnt = 0;	623	atomic_set(&hdev->sco_cnt, 0);
		624	hdev->acl_cnt = 0;
624		625
625	if (!test_bit(HCI_RAW, &hdev->flags))	626	if (!test_bit(HCI_RAW, &hdev->flags))
626	ret = __hci_request(hdev, hci_reset_req, 0,	627	ret = __hci_request(hdev, hci_reset_req, 0,
Lines 1132-1137 int hci_send_sco(struct hci_conn *conn, Link Here
1132	{	1133	{
1133	struct hci_dev *hdev = conn->hdev;	1134	struct hci_dev *hdev = conn->hdev;
1134	struct hci_sco_hdr hdr;	1135	struct hci_sco_hdr hdr;
		1136	ktime_t now;
1135		1137
1136	BT_DBG("%s len %d", hdev->name, skb->len);	1138	BT_DBG("%s len %d", hdev->name, skb->len);
1137		1139
Lines 1140-1145 int hci_send_sco(struct hci_conn *conn, Link Here
1140	return -EINVAL;	1142	return -EINVAL;
1141	}	1143	}
1142		1144
		1145	now = conn->tx_timer.base->get_time();
		1146
		1147	/* force a clean start for 100 ms or later underrun */
		1148	if (conn->tx_timer.expires.tv64 + NSEC_PER_SEC / 10 <= now.tv64) {
		1149	conn->tx_timer.expires = now;
		1150	}
		1151
1143	hdr.handle = cpu_to_le16(conn->handle);	1152	hdr.handle = cpu_to_le16(conn->handle);
1144	hdr.dlen = skb->len;	1153	hdr.dlen = skb->len;
1145		1154
Lines 1156-1167 EXPORT_SYMBOL(hci_send_sco); Link Here
1156		1165
1157	/* ---- HCI TX task (outgoing data) ---- */	1166	/* ---- HCI TX task (outgoing data) ---- */
1158		1167
1159	/* HCI Connection scheduler */	1168	/* HCI ACL Connection scheduler */
1160	static inline struct hci_conn hci_low_sent(struct hci_dev hdev, __u8 type, int *quote)	1169	static inline struct hci_conn hci_low_sent_acl(struct hci_dev hdev, int *quote)
1161	{	1170	{
1162	struct hci_conn_hash *h = &hdev->conn_hash;	1171	struct hci_conn_hash *h = &hdev->conn_hash;
1163	struct hci_conn *conn = NULL;	1172	struct hci_conn *conn = NULL;
1164	int num = 0, min = ~0;	1173	unsigned int num = 0, min = ~0;
1165	struct list_head *p;	1174	struct list_head *p;
1166		1175
1167	/* We don't have to lock device here. Connections are always	1176	/* We don't have to lock device here. Connections are always
Lines 1170-1189 static inline struct hci_conn *hci_low_s Link Here
1170	struct hci_conn *c;	1179	struct hci_conn *c;
1171	c = list_entry(p, struct hci_conn, list);	1180	c = list_entry(p, struct hci_conn, list);
1172		1181
1173	if (c->type != type \|\| c->state != BT_CONNECTED	1182	BT_DBG("c->type %d c->state %d len(c->data_q) %d min %d c->sent %d",
		1183	c->type, c->state, skb_queue_len(&c->data_q), min, atomic_read(&c->sent));
		1184
		1185	if (c->type != ACL_LINK \|\| c->state != BT_CONNECTED
1174	\|\| skb_queue_empty(&c->data_q))	1186	\|\| skb_queue_empty(&c->data_q))
1175	continue;	1187	continue;
1176	num++;	1188	num++;
1177		1189
1178	if (c->sent < min) {	1190	if (atomic_read(&c->sent) < min) {
1179	min = c->sent;	1191	min = atomic_read(&c->sent);
1180	conn = c;	1192	conn = c;
1181	}	1193	}
1182	}	1194	}
1183		1195
1184	if (conn) {	1196	if (conn) {
1185	int cnt = (type == ACL_LINK ? hdev->acl_cnt : hdev->sco_cnt);	1197	int q = hdev->acl_cnt / num;
1186	int q = cnt / num;
1187	*quote = q ? q : 1;	1198	*quote = q ? q : 1;
1188	} else	1199	} else
1189	*quote = 0;	1200	*quote = 0;
Lines 1203-1209 static inline void hci_acl_tx_to(struct Link Here
1203	/* Kill stalled connections */	1214	/* Kill stalled connections */
1204	list_for_each(p, &h->list) {	1215	list_for_each(p, &h->list) {
1205	c = list_entry(p, struct hci_conn, list);	1216	c = list_entry(p, struct hci_conn, list);
1206	if (c->type == ACL_LINK && c->sent) {	1217	if (c->type == ACL_LINK && atomic_read(&c->sent)) {
1207	BT_ERR("%s killing stalled ACL connection %s",	1218	BT_ERR("%s killing stalled ACL connection %s",
1208	hdev->name, batostr(&c->dst));	1219	hdev->name, batostr(&c->dst));
1209	hci_acl_disconn(c, 0x13);	1220	hci_acl_disconn(c, 0x13);
Lines 1226-1232 static inline void hci_sched_acl(struct Link Here
1226	hci_acl_tx_to(hdev);	1237	hci_acl_tx_to(hdev);
1227	}	1238	}
1228		1239
1229	while (hdev->acl_cnt && (conn = hci_low_sent(hdev, ACL_LINK, &quote))) {	1240	while (hdev->acl_cnt && (conn = hci_low_sent_acl(hdev, &quote))) {
1230	while (quote-- && (skb = skb_dequeue(&conn->data_q))) {	1241	while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
1231	BT_DBG("skb %p len %d", skb, skb->len);	1242	BT_DBG("skb %p len %d", skb, skb->len);
1232		1243
Lines 1236-1263 static inline void hci_sched_acl(struct Link Here
1236	hdev->acl_last_tx = jiffies;	1247	hdev->acl_last_tx = jiffies;
1237		1248
1238	hdev->acl_cnt--;	1249	hdev->acl_cnt--;
1239	conn->sent++;	1250	atomic_inc(&conn->sent);
1240	}	1251	}
1241	}	1252	}
1242	}	1253	}
1243		1254
1244	/* Schedule SCO */	1255	/* HCI SCO Connection scheduler */
		1256
1245	static inline void hci_sched_sco(struct hci_dev *hdev)	1257	static inline void hci_sched_sco(struct hci_dev *hdev)
1246	{	1258	{
1247	struct hci_conn *conn;	1259	struct hci_conn_hash *h = &hdev->conn_hash;
1248	struct sk_buff *skb;	1260	struct sk_buff *skb;
1249	int quote;	1261	struct list_head *p;
1250		1262	struct hci_conn *c;
		1263	ktime_t now, pkt_time;
		1264
1251	BT_DBG("%s", hdev->name);	1265	BT_DBG("%s", hdev->name);
		1266
		1267	/* We don't have to lock device here. Connections are always
		1268	added and removed with TX task disabled. */
		1269	list_for_each(p, &h->list) {
		1270	c = list_entry(p, struct hci_conn, list);
		1271
		1272	/* SCO scheduling algorithm makes sure there is never more than
		1273	1 outstanding packet for each connection */
1252		1274
1253	while (hdev->sco_cnt && (conn = hci_low_sent(hdev, SCO_LINK, &quote))) {	1275	if (c->type != SCO_LINK)
1254	while (quote-- && (skb = skb_dequeue(&conn->data_q))) {	1276	continue;
1255	BT_DBG("skb %p len %d", skb, skb->len);	1277
1256	hci_send_frame(skb);	1278	if (atomic_read(&c->sent) >= 1)
		1279	continue;
		1280
		1281	if (c->state != BT_CONNECTED)
		1282	continue;
		1283
		1284	if (atomic_read(&hdev->sco_cnt) <= 0)
		1285	continue;
1257		1286
1258	conn->sent++;	1287	if ((skb = skb_dequeue(&c->data_q)) == NULL)
1259	if (conn->sent == ~0)	1288	continue;
1260	conn->sent = 0;	1289
		1290	hci_send_frame(skb);
		1291
		1292	atomic_inc(&c->sent);
		1293	atomic_dec(&hdev->sco_cnt);
		1294
		1295	pkt_time = ktime_set(0, NSEC_PER_SEC / 16000 * (skb->len - HCI_SCO_HDR_SIZE));
		1296	now = c->tx_timer.base->get_time();
		1297
		1298	c->tx_timer.expires.tv64 += pkt_time.tv64;
		1299	if (c->tx_timer.expires.tv64 > now.tv64) {
		1300	hrtimer_restart(&c->tx_timer);
		1301	} else {
		1302	/* Timer is to expire in the past - force timer expiration.
		1303	this can happen if timer base precision is less than pkt_time */
		1304	c->tx_timer.function(&c->tx_timer);
1261	}	1305	}
1262	}	1306	}
1263	}	1307	}
Lines 1269-1282 static void hci_tx_task(unsigned long ar Link Here
1269		1313
1270	read_lock(&hci_task_lock);	1314	read_lock(&hci_task_lock);
1271		1315
1272	BT_DBG("%s acl %d sco %d", hdev->name, hdev->acl_cnt, hdev->sco_cnt);	1316	BT_DBG("%s acl %d sco %d", hdev->name, hdev->acl_cnt, atomic_read(&hdev->sco_cnt));
1273		1317
1274	/* Schedule queues and send stuff to HCI driver */	1318	/* Schedule queues and send stuff to HCI driver */
1275		1319
1276	hci_sched_acl(hdev);
1277
1278	hci_sched_sco(hdev);	1320	hci_sched_sco(hdev);
1279		1321
		1322	hci_sched_acl(hdev);
		1323
1280	/* Send next queued raw (unknown type) packet */	1324	/* Send next queued raw (unknown type) packet */
1281	while ((skb = skb_dequeue(&hdev->raw_q)))	1325	while ((skb = skb_dequeue(&hdev->raw_q)))
1282	hci_send_frame(skb);	1326	hci_send_frame(skb);

Lines 26-37 #ifndef __SCO_H Link Here

(-)a/include/net/bluetooth/sco.h (-5 lines)
26	#define __SCO_H	26	#define __SCO_H
27		27
28	/* SCO defaults */	28	/* SCO defaults */
29	#define SCO_DEFAULT_MTU 500
30	#define SCO_DEFAULT_FLUSH_TO 0xFFFF
31
32	#define SCO_CONN_TIMEOUT (HZ * 40)	29	#define SCO_CONN_TIMEOUT (HZ * 40)
33	#define SCO_DISCONN_TIMEOUT (HZ * 2)
34	#define SCO_CONN_IDLE_TIMEOUT (HZ * 60)
35		30
36	/* SCO socket address */	31	/* SCO socket address */
37	struct sockaddr_sco {	32	struct sockaddr_sco {

Lines 165-170 static void hci_conn_idle(unsigned long Link Here

(-)a/net/bluetooth/hci_conn.c (-1 / +29 lines)
165	hci_conn_enter_sniff_mode(conn);	165	hci_conn_enter_sniff_mode(conn);
166	}	166	}
167		167
		168	static enum hrtimer_restart hci_sco_tx_timer(struct hrtimer *timer)
		169	{
		170	struct hci_conn *conn = container_of(timer, struct hci_conn, tx_timer);
		171	#ifdef CONFIG_BT_HCI_CORE_DEBUG
		172	ktime_t now = timer->base->get_time();
		173
		174	BT_DBG("%s, conn %p, time %5lu.%06lu", conn->hdev->name, conn,
		175	(unsigned long) now.tv64,
		176	do_div(now.tv64, NSEC_PER_SEC) / 1000);
		177	#endif
		178
		179	if (atomic_read(&conn->sent) > 0) {
		180	atomic_dec(&conn->sent);
		181	atomic_inc(&conn->hdev->sco_cnt);
		182	hci_sched_tx(conn->hdev);
		183	}
		184	return HRTIMER_NORESTART;
		185	}
		186
		187
168	struct hci_conn hci_conn_add(struct hci_dev hdev, int type, bdaddr_t *dst)	188	struct hci_conn hci_conn_add(struct hci_dev hdev, int type, bdaddr_t *dst)
169	{	189	{
170	struct hci_conn *conn;	190	struct hci_conn *conn;
Lines 185-190 struct hci_conn *hci_conn_add(struct hci Link Here
185		205
186	skb_queue_head_init(&conn->data_q);	206	skb_queue_head_init(&conn->data_q);
187		207
		208	hrtimer_init(&conn->tx_timer, CLOCK_MONOTONIC, HRTIMER_NORESTART);
		209
		210	if(type == SCO_LINK)
		211	conn->tx_timer.function = hci_sco_tx_timer;
		212
188	init_timer(&conn->disc_timer);	213	init_timer(&conn->disc_timer);
189	conn->disc_timer.function = hci_conn_timeout;	214	conn->disc_timer.function = hci_conn_timeout;
190	conn->disc_timer.data = (unsigned long) conn;	215	conn->disc_timer.data = (unsigned long) conn;
Lines 194-199 struct hci_conn *hci_conn_add(struct hci Link Here
194	conn->idle_timer.data = (unsigned long) conn;	219	conn->idle_timer.data = (unsigned long) conn;
195		220
196	atomic_set(&conn->refcnt, 0);	221	atomic_set(&conn->refcnt, 0);
		222	atomic_set(&conn->sent, 0);
197		223
198	hci_dev_hold(hdev);	224	hci_dev_hold(hdev);
199		225
Lines 220-225 int hci_conn_del(struct hci_conn *conn) Link Here
220		246
221	del_timer(&conn->disc_timer);	247	del_timer(&conn->disc_timer);
222		248
		249	hrtimer_cancel(&conn->tx_timer);
		250
223	if (conn->type == SCO_LINK) {	251	if (conn->type == SCO_LINK) {
224	struct hci_conn *acl = conn->link;	252	struct hci_conn *acl = conn->link;
225	if (acl) {	253	if (acl) {
Lines 232-238 int hci_conn_del(struct hci_conn *conn) Link Here
232	sco->link = NULL;	260	sco->link = NULL;
233		261
234	/* Unacked frames */	262	/* Unacked frames */
235	hdev->acl_cnt += conn->sent;	263	hdev->acl_cnt += atomic_read(&conn->sent);
236	}	264	}
237		265
238	tasklet_disable(&hdev->tx_task);	266	tasklet_disable(&hdev->tx_task);

Lines 319-325 static void hci_cc_info_param(struct hci Link Here

(-)a/net/bluetooth/hci_event.c (-7 / +4 lines)
319	lv = (struct hci_rp_read_loc_version *) skb->data;	319	lv = (struct hci_rp_read_loc_version *) skb->data;
320		320
321	if (lv->status) {	321	if (lv->status) {
322	BT_DBG("%s READ_LOCAL_VERSION failed %d", hdev->name, lf->status);	322	BT_DBG("%s READ_LOCAL_VERSION failed %d", hdev->name, lv->status);
323	break;	323	break;
324	}	324	}
325		325
Lines 381-387 static void hci_cc_info_param(struct hci Link Here
381	}	381	}
382		382
383	hdev->acl_cnt = hdev->acl_pkts;	383	hdev->acl_cnt = hdev->acl_pkts;
384	hdev->sco_cnt = hdev->sco_pkts;	384	atomic_set(&hdev->sco_cnt, hdev->sco_pkts);
385		385
386	BT_DBG("%s mtu: acl %d, sco %d max_pkt: acl %d, sco %d", hdev->name,	386	BT_DBG("%s mtu: acl %d, sco %d max_pkt: acl %d, sco %d", hdev->name,
387	hdev->acl_mtu, hdev->sco_mtu, hdev->acl_pkts, hdev->sco_pkts);	387	hdev->acl_mtu, hdev->sco_mtu, hdev->acl_pkts, hdev->sco_pkts);
Lines 879-890 static inline void hci_num_comp_pkts_evt Link Here
879		879
880	conn = hci_conn_hash_lookup_handle(hdev, handle);	880	conn = hci_conn_hash_lookup_handle(hdev, handle);
881	if (conn) {	881	if (conn) {
882	conn->sent -= count;	882	atomic_sub(count, &conn->sent);
883		883
884	if (conn->type == SCO_LINK) {	884	if (conn->type == ACL_LINK) {
885	if ((hdev->sco_cnt += count) > hdev->sco_pkts)
886	hdev->sco_cnt = hdev->sco_pkts;
887	} else {
888	if ((hdev->acl_cnt += count) > hdev->acl_pkts)	885	if ((hdev->acl_cnt += count) > hdev->acl_pkts)
889	hdev->acl_cnt = hdev->acl_pkts;	886	hdev->acl_cnt = hdev->acl_pkts;
890	}	887	}