diff -rU 6 --exclude-from=kernelexcludes.txt --exclude='*hci*' --exclude='af_bluetooth.*' /home/fchevalier/tmp/linux-2.6.16.16-orig/include/net/bluetooth/sco.h /home/fchevalier/tmp/linux-2.6.16.16/include/net/bluetooth/sco.h --- /home/fchevalier/tmp/linux-2.6.16.16-orig/include/net/bluetooth/sco.h 2006-05-11 03:56:24.000000000 +0200 +++ /home/fchevalier/tmp/linux-2.6.16.16/include/net/bluetooth/sco.h 2006-05-28 17:36:49.000000000 +0200 @@ -23,18 +23,13 @@ */ #ifndef __SCO_H #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 { sa_family_t sco_family; bdaddr_t sco_bdaddr; }; @@ -48,12 +43,15 @@ #define SCO_CONNINFO 0x02 struct sco_conninfo { __u16 hci_handle; __u8 dev_class[3]; }; +#define SCO_TXBUFS 0x03 +#define SCO_RXBUFS 0x04 + /* ---- SCO connections ---- */ struct sco_conn { struct hci_conn *hcon; bdaddr_t *dst; bdaddr_t *src; diff -rU 6 --exclude-from=kernelexcludes.txt --exclude='*hci*' --exclude='af_bluetooth.*' /home/fchevalier/tmp/linux-2.6.16.16-orig/net/bluetooth/sco.c /home/fchevalier/tmp/linux-2.6.16.16/net/bluetooth/sco.c --- /home/fchevalier/tmp/linux-2.6.16.16-orig/net/bluetooth/sco.c 2006-05-11 03:56:24.000000000 +0200 +++ /home/fchevalier/tmp/linux-2.6.16.16/net/bluetooth/sco.c 2006-05-30 13:58:51.000000000 +0200 @@ -51,28 +51,48 @@ #ifndef CONFIG_BT_SCO_DEBUG #undef BT_DBG #define BT_DBG(D...) #endif -#define VERSION "0.5" +#define VERSION "0.6" + +#define MAX_SCO_TXBUFS 200 +#define MAX_SCO_RXBUFS 200 + +#define DEFAULT_SCO_TXBUFS 5 +#define DEFAULT_SCO_RXBUFS 5 + +static unsigned int tx_quality_tune; +static unsigned int rx_quality_tune; static const struct proto_ops sco_sock_ops; static struct bt_sock_list sco_sk_list = { .lock = RW_LOCK_UNLOCKED }; +/* Local functions declaration */ + static void __sco_chan_add(struct sco_conn *conn, struct sock *sk, struct sock *parent); static void sco_chan_del(struct sock *sk, int err); static int sco_conn_del(struct hci_conn *conn, int err); static void sco_sock_close(struct sock *sk); static void sco_sock_kill(struct sock *sk); +static void sco_sock_wfree(struct sk_buff *skb); +static void sco_sock_rfree(struct sk_buff *skb); +static long sock_wait_for_wmem(struct sock * sk, long timeo); + +static struct sk_buff *sco_skb_send_alloc(struct sock *sk, unsigned long len, + int nb, int *errcode); + +static int sco_sock_queue_rcv_skb(struct sock *sk, struct sk_buff *skb); + /* ---- SCO timers ---- */ static void sco_sock_timeout(unsigned long arg) { struct sock *sk = (struct sock *) arg; BT_DBG("sock %p state %d", sk, sk->sk_state); @@ -230,58 +250,30 @@ done: hci_dev_unlock_bh(hdev); hci_dev_put(hdev); return err; } -static inline int sco_send_frame(struct sock *sk, struct msghdr *msg, int len) -{ - struct sco_conn *conn = sco_pi(sk)->conn; - struct sk_buff *skb; - int err, count; - - /* Check outgoing MTU */ - if (len > conn->mtu) - return -EINVAL; - - BT_DBG("sk %p len %d", sk, len); - - count = min_t(unsigned int, conn->mtu, len); - if (!(skb = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err))) - return err; - - if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) { - err = -EFAULT; - goto fail; - } - - if ((err = hci_send_sco(conn->hcon, skb)) < 0) - goto fail; - - return count; - -fail: - kfree_skb(skb); - return err; -} - static inline void sco_recv_frame(struct sco_conn *conn, struct sk_buff *skb) { struct sock *sk = sco_chan_get(conn); if (!sk) goto drop; BT_DBG("sk %p len %d", sk, skb->len); if (sk->sk_state != BT_CONNECTED) goto drop; - if (!sock_queue_rcv_skb(sk, skb)) + if (sco_sock_queue_rcv_skb(sk, skb) == 0) { return; - + } + else if(rx_quality_tune) { + printk(KERN_INFO "rx_quality_tune: audio cut !\n"); + } drop: kfree_skb(skb); return; } /* -------- Socket interface ---------- */ @@ -328,13 +320,30 @@ static void sco_sock_destruct(struct sock *sk) { BT_DBG("sk %p", sk); skb_queue_purge(&sk->sk_receive_queue); - skb_queue_purge(&sk->sk_write_queue); +} + +static void sco_sock_write_space(struct sock *sk) +{ + read_lock(&sk->sk_callback_lock); + + /* Unlock writers ASAP + */ + if((atomic_read(&sk->sk_wmem_alloc)) < sk->sk_sndbuf) { + if (sk->sk_sleep && waitqueue_active(sk->sk_sleep)) + wake_up_interruptible(sk->sk_sleep); + + /* Should agree with poll, otherwise some programs break */ + if (sock_writeable(sk)) + sk_wake_async(sk, 2, POLL_OUT); + } + + read_unlock(&sk->sk_callback_lock); } static void sco_sock_cleanup_listen(struct sock *parent) { struct sock *sk; @@ -360,12 +369,19 @@ BT_DBG("sk %p state %d", sk, sk->sk_state); /* Kill poor orphan */ bt_sock_unlink(&sco_sk_list, sk); sock_set_flag(sk, SOCK_DEAD); + + /* We cannot purge write queue in socket destructor as + it was done before, as skb in this queue hold a reference + to the socket itself */ + skb_queue_purge(&sk->sk_write_queue); + + /* release socket */ sock_put(sk); } /* Close socket. * Must be called on unlocked socket. */ @@ -376,13 +392,13 @@ sco_sock_clear_timer(sk); lock_sock(sk); conn = sco_pi(sk)->conn; - BT_DBG("sk %p state %d conn %p socket %p", sk, sk->sk_state, conn, sk->sk_socket); + BT_DBG("sk %p state %d conn %p socket %p refcnt %d", sk, sk->sk_state, conn, sk->sk_socket, atomic_read(&sk->sk_refcnt)); switch (sk->sk_state) { case BT_LISTEN: sco_sock_cleanup_listen(sk); break; @@ -426,12 +442,21 @@ return NULL; sock_init_data(sock, sk); INIT_LIST_HEAD(&bt_sk(sk)->accept_q); sk->sk_destruct = sco_sock_destruct; + sk->sk_write_space = sco_sock_write_space; + + /* Put sensible values for a voice link (i.e. not too big), + as sysctl_rmem_default & sysctl_wmem_default are + really not designed for that -- In our case we use sk_**buf to + store a count of SCO packets, not a number of bytes as most of other type of + sockets do */ + sk->sk_sndbuf = DEFAULT_SCO_TXBUFS; + sk->sk_rcvbuf = DEFAULT_SCO_RXBUFS; sk->sk_sndtimeo = SCO_CONN_TIMEOUT; sock_reset_flag(sk, SOCK_ZAPPED); sk->sk_protocol = proto; sk->sk_state = BT_OPEN; @@ -631,43 +656,100 @@ static int sco_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len) { struct sock *sk = sock->sk; int err = 0; + struct sk_buff * skb; + struct sco_conn *conn = 0; - BT_DBG("sock %p, sk %p", sock, sk); + BT_DBG("sock %p, sk %p, len %d", sock, sk, len); err = sock_error(sk); if (err) return err; if (msg->msg_flags & MSG_OOB) return -EOPNOTSUPP; + lock_sock(sk); - if (sk->sk_state == BT_CONNECTED) - err = sco_send_frame(sk, msg, len); - else - err = -ENOTCONN; + conn = sco_pi(sk)->conn; + + /* Check outgoing MTU and that this is a 16 bytes multiple . + 16 bytes multiple is required by the new SCO scheduler algorithm */ + if (len <= conn->mtu && (len % 16) == 0) { + if (sk->sk_state == BT_CONNECTED) { + /* This is the call that will put us to sleep if socket send queue is full */ + skb = sco_skb_send_alloc(sk, len, + msg->msg_flags & MSG_DONTWAIT, &err); + + if (skb) { + err = memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len); + if (err == 0) { + + BT_DBG("skb %p, size %d", skb, len); + + skb_queue_tail(&sk->sk_write_queue, skb); + hci_stream_sco(conn->hcon); + err = len; + } + else { + kfree_skb(skb); + } + } + } + else { + err = -ENOTCONN; + } + } + else { + err = -EINVAL; + } release_sock(sk); + return err; } static int sco_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, int optlen) { struct sock *sk = sock->sk; + u32 opt; int err = 0; BT_DBG("sk %p", sk); lock_sock(sk); switch (optname) { + case SCO_TXBUFS: + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + if(opt > MAX_SCO_TXBUFS) { + err = -EINVAL; + break; + } + + sk->sk_sndbuf = opt; + break; + case SCO_RXBUFS: + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + if(opt > MAX_SCO_RXBUFS) { + err = -EINVAL; + break; + } + + sk->sk_rcvbuf = opt; + break; default: err = -ENOPROTOOPT; break; } release_sock(sk); @@ -676,22 +758,41 @@ static int sco_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen) { struct sock *sk = sock->sk; struct sco_options opts; struct sco_conninfo cinfo; - int len, err = 0; + int len, err = 0; + int val; BT_DBG("sk %p", sk); if (get_user(len, optlen)) return -EFAULT; lock_sock(sk); switch (optname) { + case SCO_RXBUFS: + val = sk->sk_rcvbuf; + + len = min_t(unsigned int, len, sizeof(val)); + if (copy_to_user(optval, (char *)&val, len)) + err = -EFAULT; + + break; + + case SCO_TXBUFS: + val = sk->sk_sndbuf; + + len = min_t(unsigned int, len, sizeof(val)); + if (copy_to_user(optval, (char *)&val, len)) + err = -EFAULT; + + break; + case SCO_OPTIONS: if (sk->sk_state != BT_CONNECTED) { err = -ENOTCONN; break; } @@ -890,12 +991,169 @@ drop: kfree_skb(skb); return 0; } +struct sk_buff* sco_get_scodata(struct hci_conn *hcon) +{ + struct sco_conn *conn = hcon->sco_data; + struct sk_buff* skb = NULL; + + BT_DBG("conn %p", conn); + + if (conn) { + struct sock *sk = sco_chan_get(conn); + if(sk) { + skb = skb_dequeue(&sk->sk_write_queue); + } + } + + if(skb == NULL && tx_quality_tune) { + printk(KERN_INFO "tx_quality_tune: audio cut !\n"); + } + + return skb; +} + +/* Generic socket manipulations functions, adapted for SCO */ + +static void sco_sock_wfree(struct sk_buff *skb) +{ + struct sock *sk = skb->sk; + + atomic_sub(1, &sk->sk_wmem_alloc); + if (!sock_flag(sk, SOCK_USE_WRITE_QUEUE)) + sk->sk_write_space(sk); + sock_put(sk); +} + +static void sco_sock_rfree(struct sk_buff *skb) +{ + struct sock *sk = skb->sk; + + atomic_sub(1, &sk->sk_rmem_alloc); +} + + +static long sock_wait_for_wmem(struct sock * sk, long timeo) +{ + DEFINE_WAIT(wait); + + clear_bit(SOCK_ASYNC_NOSPACE, &sk->sk_socket->flags); + for (;;) { + if (!timeo) + break; + if (signal_pending(current)) + break; + set_bit(SOCK_NOSPACE, &sk->sk_socket->flags); + prepare_to_wait(sk->sk_sleep, &wait, TASK_INTERRUPTIBLE); + if (atomic_read(&sk->sk_wmem_alloc) < sk->sk_sndbuf) + break; + if (sk->sk_shutdown & SEND_SHUTDOWN) + break; + if (sk->sk_err) + break; + timeo = schedule_timeout(timeo); + } + finish_wait(sk->sk_sleep, &wait); + return timeo; +} + +static struct sk_buff *sco_skb_send_alloc(struct sock *sk, unsigned long len, + int nb, int *errcode) +{ + struct sk_buff *skb; + gfp_t gfp_mask; + int err; + long timeo = nb ? 0 : MAX_SCHEDULE_TIMEOUT; + + gfp_mask = sk->sk_allocation; + if (gfp_mask & __GFP_WAIT) + gfp_mask |= __GFP_REPEAT; + + while (1) { + err = sock_error(sk); + if (err != 0) + goto failure; + + err = -EPIPE; + if (sk->sk_shutdown & SEND_SHUTDOWN) + goto failure; + + if (atomic_read(&sk->sk_wmem_alloc) < sk->sk_sndbuf) { + skb = alloc_skb(len + BT_SKB_RESERVE, sk->sk_allocation); + if (skb) { + /* Full success... */ + break; + } + err = -ENOBUFS; + goto failure; + } + set_bit(SOCK_ASYNC_NOSPACE, &sk->sk_socket->flags); + set_bit(SOCK_NOSPACE, &sk->sk_socket->flags); + err = -EAGAIN; + if (!timeo) + goto failure; + if (signal_pending(current)) + goto interrupted; + timeo = sock_wait_for_wmem(sk, timeo); + } + + sock_hold(sk); + skb->sk = sk; + skb->destructor = sco_sock_wfree; + atomic_add(1, &sk->sk_wmem_alloc); + skb_reserve(skb, BT_SKB_RESERVE); + bt_cb(skb)->incoming = 0; + + return skb; + +interrupted: + err = sock_intr_errno(timeo); +failure: + *errcode = err; + return NULL; +} + +static int sco_sock_queue_rcv_skb(struct sock *sk, struct sk_buff *skb) +{ + int err = 0; + int skb_len; + + /* Cast skb->rcvbuf to unsigned... It's pointless, but reduces + number of warnings when compiling with -W --ANK + */ + if (atomic_read(&sk->sk_rmem_alloc) >= + (unsigned)sk->sk_rcvbuf) { + err = -ENOMEM; + goto out; + } + + skb->dev = NULL; + skb->sk = sk; + skb->destructor = sco_sock_rfree; + atomic_add(1, &sk->sk_rmem_alloc); + + /* Cache the SKB length before we tack it onto the receive + * queue. Once it is added it no longer belongs to us and + * may be freed by other threads of control pulling packets + * from the queue. + */ + skb_len = skb->len; + + skb_queue_tail(&sk->sk_receive_queue, skb); + + if (!sock_flag(sk, SOCK_DEAD)) + sk->sk_data_ready(sk, skb_len); +out: + return err; +} + +/* ------- Others ------- */ + static ssize_t sco_sysfs_show(struct class *dev, char *buf) { struct sock *sk; struct hlist_node *node; char *str = buf; @@ -943,13 +1201,14 @@ static struct hci_proto sco_hci_proto = { .name = "SCO", .id = HCI_PROTO_SCO, .connect_ind = sco_connect_ind, .connect_cfm = sco_connect_cfm, .disconn_ind = sco_disconn_ind, - .recv_scodata = sco_recv_scodata + .recv_scodata = sco_recv_scodata, + .get_scodata = sco_get_scodata }; static int __init sco_init(void) { int err; @@ -995,11 +1254,17 @@ proto_unregister(&sco_proto); } module_init(sco_init); module_exit(sco_exit); +module_param(tx_quality_tune, bool, 0644); +MODULE_PARM_DESC(tx_quality_tune, "Print warning message when an audio cut will happen in tx"); + +module_param(rx_quality_tune, bool, 0644); +MODULE_PARM_DESC(rx_quality_tune, "Print warning message when an audio cut will happen in rx"); + MODULE_AUTHOR("Maxim Krasnyansky , Marcel Holtmann "); MODULE_DESCRIPTION("Bluetooth SCO ver " VERSION); MODULE_VERSION(VERSION); MODULE_LICENSE("GPL"); MODULE_ALIAS("bt-proto-2");