Hi,
These 3 patches add a basic support for ERTM (without retransmission). The code
is able to send I-frames with txWindow up to 63 and send/receive RR S-frames
to acknowledge packets (first patch).
The third patch adds Segmentation and Reassembly which permits that upper
layers use MTU values greater than MPS value. To segment packets we use a
separated queue, and then append it to the end of tx_queue. To reassembly we
use a sk_buff to reconstruct the SDU on the other side.
Second patch is just a code clean up.
include/net/bluetooth/bluetooth.h | 3 +-
include/net/bluetooth/l2cap.h | 164 +++++++++++-
net/bluetooth/l2cap.c | 554 +++++++++++++++++++++++++++++++------
3 files changed, 638 insertions(+), 83 deletions(-)
Hi,
On Fri, Jul 24, 2009 at 10:03 PM, Doug
Moeller<[email protected]> wrote:
> # dbus-send --system --type=method_call --print-reply --dest=org.bluez
> /org/bluez/hci0 org.bluez.Adapter.GetMode
> Error org.freedesktop.DBus.Error.UnknownMethod: Method "GetMode" with
> signature "" on interface "org.bluez.Adapter" doesn't exist
>
>
> Bluetoothd output
>
>
> # bluetoothd -dn
> bluetoothd[18625]: Bluetooth daemon 4.38
There is no GetMode on Adapter interface, I suggest you to read the
documentation under doc/ directory, there is also some python scripts
which are useful for testing under tests/ directory. Another problem
is that you are using hardcoded path /org/bluez/hci0, this won't work
anymore.
ps: The directories are relative to BlueZ repository:
git://git.kernel.org/pub/scm/bluetooth/bluez.git
ps2: http://git.kernel.org/?p=bluetooth/bluez.git;a=blob;f=doc/adapter-api.txt;h=1e03b4e2785e33d37c9579238e6a49ac719047de;hb=HEAD
--
Luiz Augusto von Dentz
Engenheiro de Computa??o
I've been beating my head against this for hours. My end game is to get
Bluetooth audio going, but I'm stuck just trying to get the interface
discoverable. If anyone is willing to share any clues I'd really
appreciate it.
-Doug
# dbus-send --system --type=method_call --print-reply --dest=org.bluez
/org/bluez/hci0 org.bluez.Adapter.GetMode
Error org.freedesktop.DBus.Error.UnknownMethod: Method "GetMode" with
signature "" on interface "org.bluez.Adapter" doesn't exist
Bluetoothd output
# bluetoothd -dn
bluetoothd[18625]: Bluetooth daemon 4.38
bluetoothd[18625]: Enabling debug information
bluetoothd[18625]: parsing main.conf
bluetoothd[18625]: discovto=0
bluetoothd[18625]: pairto=0
bluetoothd[18625]: pageto=8192
bluetoothd[18625]: name=%h-%d
bluetoothd[18625]: class=0x000100
bluetoothd[18625]: inqmode=0
bluetoothd[18625]: Key file does not have key 'DeviceID'
bluetoothd[18625]: Starting SDP server
bluetoothd[18625]: Loading builtin plugins
bluetoothd[18625]: Loading service plugin
bluetoothd[18625]: Loading hciops plugin
bluetoothd[18625]: Loading hal plugin
bluetoothd[18625]: Loading storage plugin
bluetoothd[18625]: Loading plugins /usr/local/lib/bluetooth/plugins
bluetoothd[18625]: Loading audio plugin
bluetoothd[18625]: Loading serial plugin
bluetoothd[18625]: Loading input plugin
bluetoothd[18625]: register_interface: path /org/bluez/18625/any
bluetoothd[18625]: Registered interface org.bluez.Service on path
/org/bluez/18625/any
bluetoothd[18625]: input.conf: Key file does not have key 'IdleTimeout'
bluetoothd[18625]: Unix socket created: 7
bluetoothd[18625]: audio.conf: Key file does not have key 'AutoConnect'
bluetoothd[18625]: audio.conf: Key file does not have key 'MaxConnected'
bluetoothd[18625]: Telephony plugin initialized
bluetoothd[18625]: HFP AG features: "Ability to reject a call" "Enhanced
call status" "Extended Error Result Codes"
bluetoothd[18625]: HCI dev 0 registered
bluetoothd[18625]: child 18626 forked
bluetoothd[18625]: HCI dev 0 up
bluetoothd[18625]: Starting security manager 0
bluetoothd[18625]: periodic_inquiry_exit at adapter startup
bluetoothd[18625]: register_interface: path /org/bluez/18625/hci0
bluetoothd[18625]: Registered interface org.bluez.Service on path
/org/bluez/18625/hci0
bluetoothd[18625]: proxy_probe: path /org/bluez/18625/hci0
bluetoothd[18625]: Registered interface org.bluez.SerialProxyManager on
path /org/bluez/18625/hci0
bluetoothd[18625]: headset_server_probe: path /org/bluez/18625/hci0
bluetoothd[18625]: audio.conf: Key file does not have key 'Master'
bluetoothd[18625]: Adding record with handle 0x10000
bluetoothd[18625]: Record pattern UUID 00000003-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 00000100-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 00001002-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 00001108-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 00001112-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 00001203-0000-1000-8000-00805f9
bluetoothd[18625]: audio.conf: Key file does not have key 'SCORouting'
bluetoothd[18625]: Adding record with handle 0x10001
bluetoothd[18625]: Record pattern UUID 00000003-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 00000100-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 00001002-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 0000111e-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 0000111f-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 00001203-0000-1000-8000-00805f9
bluetoothd[18625]: a2dp_server_probe: path /org/bluez/18625/hci0
bluetoothd[18625]: audio.conf: Key file does not have key 'Disable'
bluetoothd[18625]: audio.conf: Key file does not have group 'A2DP'
bluetoothd[18625]: audio.conf: Key file does not have group 'A2DP'
bluetoothd[18625]: audio.conf: Key file does not have group 'A2DP'
bluetoothd[18625]: audio.conf: Key file does not have group 'A2DP'
bluetoothd[18625]: audio.conf: Key file does not have key 'Master'
bluetoothd[18625]: SEP 0xb9b81478 registered: type:0 codec:0 seid:1
bluetoothd[18625]: Adding record with handle 0x10002
bluetoothd[18625]: Record pattern UUID 00000019-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 00000100-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 00001002-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 0000110a-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 0000110d-0000-1000-8000-00805f9
bluetoothd[18625]: avrcp_server_probe: path /org/bluez/18625/hci0
bluetoothd[18625]: audio.conf: Key file does not have key 'Master'
bluetoothd[18625]: Adding record with handle 0x10003
bluetoothd[18625]: Record pattern UUID 00000017-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 00000100-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 00001002-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 0000110c-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 0000110e-0000-1000-8000-00805f9
bluetoothd[18625]: Adding record with handle 0x10004
bluetoothd[18625]: Record pattern UUID 00000017-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 00000100-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 00001002-0000-1000-8000-00805f9
bluetoothd[18625]: Record pattern UUID 0000110e-0000-1000-8000-00805f9
bluetoothd[18625]: Changing service classes to 0x480100
bluetoothd[18625]: Adapter /org/bluez/18625/hci0 has been enabled
bluetoothd[18625]: Changing service classes to 0x480100
bluetoothd[18625]: Entering main loop
bluetoothd[18625]: child 18626 exited
bluetoothd[18625]: Failed to access HAL
Hi Gustavo,
> This patch adds support for ERTM transfers, without retransmission, with
> txWindow up to 63 and with acknowledgement of packets received. Now the
> packets are queued before call l2cap_do_send(), so packets couldn't be
> sent at the time we call l2cap_sock_sendmsg(). They will be sent in
> an asynchronous way on later calls of l2cap_ertm_send(). Besides if an
> error occurs on calling l2cap_do_send() we disconnect the channel.
we have to fix the basic comments I gave you on IRC. Then send 2/3 first
and then we go trough this one again.
Regards
Marcel
Hi Gustavo,
> The code for sending a disconnect request was repetead several times on
> L2CAP.
this one should go in first. Please prepare a version where this applies
cleanly to bluetooth-testing.git and let us push that one.
Regards
Marcel
ERTM should use Segmentation and Reassembly to break down a SDU in many
PDUs on sending data to the other side.
On sending packets we queue all 'segments' until end of segmentation and
just the add them to the queue for sending.
On receiving we create a new skb with the SDU reassembled.
Based on a patch from Nathan Holstein <[email protected]>
Signed-off-by: Gustavo F. Padovan <[email protected]>
---
include/net/bluetooth/l2cap.h | 18 ++++
net/bluetooth/l2cap.c | 184 ++++++++++++++++++++++++++++++++++++-----
2 files changed, 182 insertions(+), 20 deletions(-)
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 95d6963..0cd7822 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -326,6 +326,10 @@ struct l2cap_pinfo {
__u8 req_seq;
__u8 expected_tx_seq;
__u8 num_to_ack;
+ __u16 sdu_len;
+ __u16 partial_sdu_len;
+ __u8 start_txseq;
+ struct sk_buff *sdu;
__u8 ident;
@@ -355,6 +359,7 @@ struct l2cap_pinfo {
#define L2CAP_CONF_MAX_CONF_RSP 2
#define L2CAP_CONN_TRANSMITTER 0x01
+#define L2CAP_CONN_SAR_SDU 0x02
static inline void l2cap_init_send_head(struct sock *sk)
{
@@ -414,6 +419,14 @@ static inline void l2cap_advance_send_head(struct sock *sk, struct sk_buff *skb)
sk->sk_send_head = l2cap_write_queue_next(sk, skb);
}
+static inline void l2cap_add_sar_queue(struct sock *sk, struct sk_buff_head *list, struct sk_buff_head *sar_queue)
+{
+ if (sk->sk_send_head == NULL)
+ sk->sk_send_head = sar_queue->next;
+
+ skb_queue_splice_tail(sar_queue, list);
+}
+
static inline int l2cap_tx_window_full(struct sock *sk)
{
struct l2cap_pinfo *pi = l2cap_pi(sk);
@@ -456,6 +469,11 @@ static inline int l2cap_is_S_frame(u16 control)
return (control & L2CAP_CONTROL_FRAME_TYPE);
}
+static inline int l2cap_sar_sdu_start(u16 control)
+{
+ return (control & L2CAP_CONTROL_SAR) == L2CAP_SAR_SDU_START;
+}
+
void l2cap_load(void);
#endif /* __L2CAP_H */
diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index 5d10277..9558561 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -1267,7 +1267,7 @@ static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, in
return sent;
}
-static int l2cap_create_sdu(struct sock *sk, struct msghdr *msg, size_t len, u16 *control, struct sk_buff **out_skb)
+static int l2cap_create_sdu(struct sock *sk, struct msghdr *msg, size_t len, u16 *control, u16 sdulen, struct sk_buff **out_skb)
{
struct l2cap_conn *conn = l2cap_pi(sk)->conn;
struct sk_buff *skb;
@@ -1276,6 +1276,9 @@ static int l2cap_create_sdu(struct sock *sk, struct msghdr *msg, size_t len, u16
hlen = (control) ? L2CAP_ERTM_HDR_SIZE : L2CAP_HDR_SIZE;
+ if (sdulen)
+ hlen +=2;
+
BT_DBG("sk %p len %d", sk, (int)len);
/* First fragment (with L2CAP header) */
@@ -1296,6 +1299,9 @@ static int l2cap_create_sdu(struct sock *sk, struct msghdr *msg, size_t len, u16
if (control)
put_unaligned(*control, (__le16 *) skb_put(skb, 2));
+ if (sdulen)
+ put_unaligned(sdulen, (__le16 *) skb_put(skb, 2));
+
if (sk->sk_type == SOCK_DGRAM)
put_unaligned(l2cap_pi(sk)->psm, (__le16 *) skb_put(skb, 2));
@@ -1310,7 +1316,58 @@ static int l2cap_create_sdu(struct sock *sk, struct msghdr *msg, size_t len, u16
static inline int l2cap_create_basic_sdu(struct sock *sk, struct msghdr *msg, size_t len, struct sk_buff **out_skb)
{
- return l2cap_create_sdu(sk, msg, len, NULL, out_skb);
+ return l2cap_create_sdu(sk, msg, len, NULL, 0, out_skb);
+}
+
+
+static inline int l2cap_sar_segment_sdu(struct sock *sk, struct msghdr *msg, size_t len)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ struct sk_buff *skb;
+ struct sk_buff_head sar_queue;
+ u16 control;
+ size_t size = 0;
+ int err;
+
+ __skb_queue_head_init(&sar_queue);
+ control |= L2CAP_SAR_SDU_START;
+ err = l2cap_create_sdu(sk, msg, pi->max_pdu_size, &control,
+ len, &skb);
+ if (unlikely(err < 0))
+ return err;
+
+ __skb_queue_tail(&sar_queue, skb);
+ len -= pi->max_pdu_size;
+ size +=pi->max_pdu_size;
+ control = 0;
+
+ while (len > 0) {
+ size_t buflen;
+
+ if (len > pi->max_pdu_size) {
+ control |= L2CAP_SAR_SDU_CONTINUE;
+ buflen = pi->max_pdu_size;
+ }
+ else {
+ control |= L2CAP_SAR_SDU_END;
+ buflen = len;
+ }
+
+ err = l2cap_create_sdu(sk, msg, buflen ,&control,
+ 0, &skb);
+ if (unlikely(err < 0)) {
+ skb_queue_purge(&sar_queue);
+ return err;
+ }
+
+ __skb_queue_tail(&sar_queue, skb);
+ len -= buflen;
+ size += buflen;
+ control = 0;
+ }
+ l2cap_add_sar_queue(sk, &pi->tx_queue, &sar_queue);
+
+ return size;
}
static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
@@ -1331,7 +1388,7 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
return -EOPNOTSUPP;
/* Check outgoing MTU */
- if ((sk->sk_type != SOCK_RAW || pi->mode == L2CAP_MODE_BASIC)
+ if ((sk->sk_type != SOCK_RAW && pi->mode == L2CAP_MODE_BASIC)
&& len > pi->omtu)
return -EINVAL;
@@ -1353,21 +1410,21 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
break;
case L2CAP_MODE_ERTM:
-
/* Entire SDU fits into one PDU */
- if (len <= pi->omtu) {
+ if (len <= pi->max_pdu_size) {
control = L2CAP_SAR_UNSEGMENTED;
- err = l2cap_create_sdu(sk, msg, len, &control, &skb);
+ err = l2cap_create_sdu(sk, msg, len, &control, 0, &skb);
+ if (err < 0)
+ goto done;
+
+ l2cap_add_tx_queue_tail(sk, skb);
}
+ /* Segment SDU into multiples PDUs */
else {
- /* Segmentation will be added later */
- err = -EINVAL;
- goto done;
+ err = l2cap_sar_segment_sdu(sk, msg, len);
+ if (unlikely(err < 0))
+ goto done;
}
- if (err < 0)
- goto done;
-
- l2cap_add_tx_queue_tail(sk, skb);
err = l2cap_ertm_send(sk);
if (!err)
@@ -1379,7 +1436,6 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
err = -EINVAL;
}
-
done:
release_sock(sk);
return err;
@@ -2770,6 +2826,89 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *sk
kfree_skb(skb);
}
+static int l2cap_sar_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control, u8 txseq)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ struct sk_buff *_skb;
+ unsigned char *to;
+ int err = -EINVAL;
+
+ switch (control & L2CAP_CONTROL_SAR) {
+ case L2CAP_SAR_UNSEGMENTED:
+ if (pi->conn_state & L2CAP_CONN_SAR_SDU)
+ goto drop2;
+
+ err = sock_queue_rcv_skb(sk, skb);
+ if (unlikely(err < 0))
+ goto drop;
+ break;
+
+ case L2CAP_SAR_SDU_START:
+ if (pi->conn_state & L2CAP_CONN_SAR_SDU)
+ goto drop2;
+
+ pi->sdu_len = get_unaligned((__le16 *) skb->data);
+ skb_pull(skb, 2);
+
+ pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
+ if (!pi->sdu)
+ goto drop;
+
+ to = skb_put(pi->sdu, skb->len);
+ memcpy(to, skb->data, skb->len);
+
+ pi->conn_state |= L2CAP_CONN_SAR_SDU;
+ pi->partial_sdu_len = skb->len;
+ pi->start_txseq = txseq;
+ kfree_skb(skb);
+ break;
+
+ case L2CAP_SAR_SDU_CONTINUE:
+ if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
+ goto drop;
+
+ to = skb_put(pi->sdu, skb->len);
+ memcpy(to, skb->data, skb->len);
+
+ pi->partial_sdu_len += skb->len;
+ if (pi->partial_sdu_len > pi->sdu_len)
+ goto drop2;
+
+ kfree_skb(skb);
+ break;
+
+ case L2CAP_SAR_SDU_END:
+ if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
+ goto drop;
+
+ to = skb_put(pi->sdu, skb->len);
+ memcpy(to, skb->data, skb->len);
+
+ pi->conn_state &= !L2CAP_CONN_SAR_SDU;
+ pi->partial_sdu_len += skb->len;
+
+ if (pi->partial_sdu_len != pi->sdu_len)
+ goto drop2;
+
+ _skb = skb_clone(pi->sdu, GFP_ATOMIC);
+ err = sock_queue_rcv_skb(sk, _skb);
+ if (unlikely(err < 0))
+ kfree_skb(_skb);
+
+ kfree_skb(pi->sdu);
+ kfree_skb(skb);
+ break;
+ }
+ return 0;
+
+drop2:
+ kfree_skb(pi->sdu);
+
+drop:
+ kfree_skb(skb);
+ return err;
+}
+
static inline int l2cap_data_channel_I_frame(struct sock *sk, u16 rx_control, struct sk_buff *skb)
{
struct l2cap_pinfo *pi = l2cap_pi(sk);
@@ -2782,11 +2921,11 @@ static inline int l2cap_data_channel_I_frame(struct sock *sk, u16 rx_control, st
if (tx_seq != pi->expected_tx_seq)
return -EINVAL;
- L2CAP_SEQ_NUM_INC(pi->expected_tx_seq);
- err = sock_queue_rcv_skb(sk, skb);
- if (err)
+ err = l2cap_sar_reassembly_sdu(sk, skb, rx_control, tx_seq);
+ if (err < 0)
return err;
+ L2CAP_SEQ_NUM_INC(pi->expected_tx_seq);
L2CAP_NUM_TO_ACK_INC(pi->num_to_ack);
if (pi->num_to_ack == L2CAP_DEFAULT_NUM_TO_ACK - 1) {
tx_control |= L2CAP_CONTROL_FRAME_TYPE;
@@ -2830,16 +2969,21 @@ static inline int l2cap_data_channel_S_frame(struct sock *sk, u16 rx_control, st
static inline int l2cap_ertm_data_channel(struct sock *sk, struct l2cap_hdr *lh, struct sk_buff *skb)
{
- u16 control, len = skb->len;
+ u16 control, len;
BT_DBG("sk %p skb %p", sk, skb);
control = get_unaligned((__le16 *) skb->data);
skb_pull(skb, 2);
- len -=2;
+
+ len = skb->len;
+
BT_DBG("control 0x%4.4x", control);
- if (l2cap_pi(sk)->imtu < skb->len)
+ if (l2cap_sar_sdu_start(control))
+ len -=2;
+
+ if (l2cap_pi(sk)->imtu < len)
goto drop;
if (l2cap_is_I_frame(control))
--
1.6.3.3
The code for sending a disconnect request was repetead several times on
L2CAP.
Signed-off-by: Gustavo F. Padovan <[email protected]>
---
net/bluetooth/l2cap.c | 53 ++++++++++++++++--------------------------------
1 files changed, 18 insertions(+), 35 deletions(-)
diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index 17e1d8b..5d10277 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -379,6 +379,17 @@ static void l2cap_do_start(struct sock *sk)
}
}
+static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct sock *sk)
+{
+ struct l2cap_disconn_req req;
+
+ req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
+ req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
+ l2cap_send_cmd(conn, l2cap_get_ident(conn),
+ L2CAP_DISCONN_REQ, sizeof(req), &req);
+ return;
+}
+
/* ---- L2CAP connections ---- */
static void l2cap_conn_start(struct l2cap_conn *conn)
{
@@ -663,15 +674,10 @@ static void __l2cap_sock_close(struct sock *sk, int reason)
case BT_CONFIG:
if (sk->sk_type == SOCK_SEQPACKET) {
struct l2cap_conn *conn = l2cap_pi(sk)->conn;
- struct l2cap_disconn_req req;
sk->sk_state = BT_DISCONN;
l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
-
- req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
- req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
- l2cap_send_cmd(conn, l2cap_get_ident(conn),
- L2CAP_DISCONN_REQ, sizeof(req), &req);
+ l2cap_send_disconn_req(conn, sk);
} else
l2cap_chan_del(sk, reason);
break;
@@ -1214,11 +1220,7 @@ static int l2cap_ertm_send(struct sock *sk)
err = l2cap_do_send(sk, tx_skb);
if (err < 0) {
- struct l2cap_disconn_req req;
- req.dcid = cpu_to_le16(pi->dcid);
- req.scid = cpu_to_le16(pi->scid);
- l2cap_send_cmd(pi->conn, l2cap_get_ident(pi->conn),
- L2CAP_DISCONN_REQ, sizeof(req), &req);
+ l2cap_send_disconn_req(pi->conn, sk);
return err;
}
@@ -1947,13 +1949,8 @@ static int l2cap_build_conf_req(struct sock *sk, void *data)
case L2CAP_MODE_STREAMING:
case L2CAP_MODE_ERTM:
pi->conf_state |= L2CAP_CONF_STATE2_DEVICE;
- if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask)) {
- struct l2cap_disconn_req req;
- req.dcid = cpu_to_le16(pi->dcid);
- req.scid = cpu_to_le16(pi->scid);
- l2cap_send_cmd(pi->conn, l2cap_get_ident(pi->conn),
- L2CAP_DISCONN_REQ, sizeof(req), &req);
- }
+ if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask))
+ l2cap_send_disconn_req(pi->conn, sk);
break;
default:
pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
@@ -2447,11 +2444,7 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
/* Complete config. */
len = l2cap_parse_conf_req(sk, rsp);
if (len < 0) {
- struct l2cap_disconn_req req;
- req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
- req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
- l2cap_send_cmd(conn, l2cap_get_ident(conn),
- L2CAP_DISCONN_REQ, sizeof(req), &req);
+ l2cap_send_disconn_req(conn, sk);
goto unlock;
}
@@ -2517,11 +2510,7 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
len = l2cap_parse_conf_rsp(sk, rsp->data,
len, req, &result);
if (len < 0) {
- struct l2cap_disconn_req req;
- req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
- req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
- l2cap_send_cmd(conn, l2cap_get_ident(conn),
- L2CAP_DISCONN_REQ, sizeof(req), &req);
+ l2cap_send_disconn_req(conn, sk);
goto done;
}
@@ -2537,13 +2526,7 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
sk->sk_state = BT_DISCONN;
sk->sk_err = ECONNRESET;
l2cap_sock_set_timer(sk, HZ * 5);
- {
- struct l2cap_disconn_req req;
- req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
- req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
- l2cap_send_cmd(conn, l2cap_get_ident(conn),
- L2CAP_DISCONN_REQ, sizeof(req), &req);
- }
+ l2cap_send_disconn_req(conn, sk);
goto done;
}
--
1.6.3.3
This patch adds support for ERTM transfers, without retransmission, with
txWindow up to 63 and with acknowledgement of packets received. Now the
packets are queued before call l2cap_do_send(), so packets couldn't be
sent at the time we call l2cap_sock_sendmsg(). They will be sent in
an asynchronous way on later calls of l2cap_ertm_send(). Besides if an
error occurs on calling l2cap_do_send() we disconnect the channel.
Based on a patch from Nathan Holstein <[email protected]>
Signed-off-by: Gustavo F. Padovan <[email protected]>
---
include/net/bluetooth/bluetooth.h | 3 +-
include/net/bluetooth/l2cap.h | 146 +++++++++++++++-
net/bluetooth/l2cap.c | 367 +++++++++++++++++++++++++++++++-----
3 files changed, 463 insertions(+), 53 deletions(-)
diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h
index 968166a..65a5cf8 100644
--- a/include/net/bluetooth/bluetooth.h
+++ b/include/net/bluetooth/bluetooth.h
@@ -138,8 +138,9 @@ struct sock *bt_accept_dequeue(struct sock *parent, struct socket *newsock);
struct bt_skb_cb {
__u8 pkt_type;
__u8 incoming;
+ __u8 tx_seq;
};
-#define bt_cb(skb) ((struct bt_skb_cb *)(skb->cb))
+#define bt_cb(skb) ((struct bt_skb_cb *)((skb)->cb))
static inline struct sk_buff *bt_skb_alloc(unsigned int len, gfp_t how)
{
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 6fc7698..95d6963 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -29,7 +29,8 @@
#define L2CAP_DEFAULT_MTU 672
#define L2CAP_DEFAULT_MIN_MTU 48
#define L2CAP_DEFAULT_FLUSH_TO 0xffff
-#define L2CAP_DEFAULT_TX_WINDOW 1
+#define L2CAP_DEFAULT_TX_WINDOW 63
+#define L2CAP_DEFAULT_NUM_TO_ACK (L2CAP_DEFAULT_TX_WINDOW/5)
#define L2CAP_DEFAULT_MAX_RECEIVE 1
#define L2CAP_DEFAULT_RETRANS_TO 300 /* 300 milliseconds */
#define L2CAP_DEFAULT_MONITOR_TO 1000 /* 1 second */
@@ -94,6 +95,33 @@ struct l2cap_conninfo {
#define L2CAP_FCS_NONE 0x00
#define L2CAP_FCS_CRC16 0x01
+/* L2CAP Control Field bit masks */
+#define L2CAP_CONTROL_SAR 0xC000
+#define L2CAP_CONTROL_REQSEQ 0x3F00
+#define L2CAP_CONTROL_TXSEQ 0x007E
+#define L2CAP_CONTROL_RETRANS 0x0080
+#define L2CAP_CONTROL_FINAL 0x0080
+#define L2CAP_CONTROL_POLL 0x0010
+#define L2CAP_CONTROL_SUPERVISE 0x000C
+#define L2CAP_CONTROL_FRAME_TYPE 0x0001 /* I- or S-Frame */
+
+#define L2CAP_SEQ_NUM_INC(seq) ((seq) = (seq + 1) % 64)
+#define L2CAP_CONTROL_TXSEQ_SHIFT 1
+#define L2CAP_CONTROL_REQSEQ_SHIFT 8
+#define L2CAP_NUM_TO_ACK_INC(seq) ((seq) = (seq + 1) % L2CAP_DEFAULT_NUM_TO_ACK)
+
+/* L2CAP Supervisory Function */
+#define L2CAP_SUPER_RCV_READY 0x0000
+#define L2CAP_SUPER_REJECT 0x0004
+#define L2CAP_SUPER_RCV_NOT_READY 0x0008
+#define L2CAP_SUPER_SELECT_REJECT 0x000C
+
+/* L2CAP Segmentation and Reassembly */
+#define L2CAP_SAR_UNSEGMENTED 0x0000
+#define L2CAP_SAR_SDU_START 0x4000
+#define L2CAP_SAR_SDU_END 0x8000
+#define L2CAP_SAR_SDU_CONTINUE 0xC000
+
/* L2CAP structures */
struct l2cap_hdr {
__le16 len;
@@ -101,6 +129,13 @@ struct l2cap_hdr {
} __attribute__ ((packed));
#define L2CAP_HDR_SIZE 4
+struct l2cap_ertm_hdr {
+ __le16 len;
+ __le16 cid;
+ __le16 control;
+} __attribute__ ((packed));
+#define L2CAP_ERTM_HDR_SIZE 6
+
struct l2cap_cmd_hdr {
__u8 code;
__u8 ident;
@@ -284,6 +319,13 @@ struct l2cap_pinfo {
__u8 conf_req[64];
__u8 conf_len;
__u8 conf_state;
+ __u8 conn_state;
+
+ __u8 next_tx_seq;
+ __u8 expected_ack_seq;
+ __u8 req_seq;
+ __u8 expected_tx_seq;
+ __u8 num_to_ack;
__u8 ident;
@@ -295,6 +337,7 @@ struct l2cap_pinfo {
__le16 sport;
+ struct sk_buff_head tx_queue;
struct l2cap_conn *conn;
struct sock *next_c;
struct sock *prev_c;
@@ -311,6 +354,107 @@ struct l2cap_pinfo {
#define L2CAP_CONF_MAX_CONF_REQ 2
#define L2CAP_CONF_MAX_CONF_RSP 2
+#define L2CAP_CONN_TRANSMITTER 0x01
+
+static inline void l2cap_init_send_head(struct sock *sk)
+{
+ sk->sk_send_head = NULL;
+}
+
+static inline struct sk_buff *l2cap_send_head(struct sock *sk)
+{
+ return sk->sk_send_head;
+}
+
+static inline void __l2cap_add_tx_queue_tail(struct sock *sk, struct sk_buff *skb)
+{
+ __skb_queue_tail(&l2cap_pi(sk)->tx_queue, skb);
+}
+
+static inline void l2cap_tx_queue_head_init(struct sock *sk) {
+ __skb_queue_head_init(&l2cap_pi(sk)->tx_queue);
+}
+
+static inline void l2cap_add_tx_queue_tail(struct sock *sk, struct sk_buff *skb)
+{
+ __l2cap_add_tx_queue_tail(sk, skb);
+
+ /* Queue it, remembering where we must start sending. */
+ if (sk->sk_send_head == NULL) {
+ sk->sk_send_head = skb;
+ }
+}
+
+static inline struct sk_buff *l2cap_write_queue_next(struct sock *sk, struct sk_buff *skb)
+{
+ return skb_queue_next(&l2cap_pi(sk)->tx_queue, skb);
+}
+
+static inline bool l2cap_skb_is_last(const struct sock *sk,
+ const struct sk_buff *skb)
+{
+ return skb_queue_is_last(&l2cap_pi(sk)->tx_queue, skb);
+}
+
+static inline struct sk_buff *l2cap_skb_peek(struct sock *sk)
+{
+ return skb_peek(&l2cap_pi(sk)->tx_queue);
+}
+
+static inline struct sk_buff *l2cap_skb_dequeue(struct sock *sk)
+{
+ return skb_dequeue(&l2cap_pi(sk)->tx_queue);
+}
+
+static inline void l2cap_advance_send_head(struct sock *sk, struct sk_buff *skb)
+{
+ if (l2cap_skb_is_last(sk, skb))
+ sk->sk_send_head = NULL;
+ else
+ sk->sk_send_head = l2cap_write_queue_next(sk, skb);
+}
+
+static inline int l2cap_tx_window_full(struct sock *sk)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ int sub;
+
+ sub = (pi->next_tx_seq - pi->expected_ack_seq) % 64;
+
+ if (sub < 0)
+ sub += 64;
+
+ return (sub == pi->remote_tx_win);
+
+}
+
+static inline u8 l2cap_control_txseq(u16 control)
+{
+ return (control & L2CAP_CONTROL_TXSEQ) >>
+ L2CAP_CONTROL_TXSEQ_SHIFT;
+}
+
+static inline u8 l2cap_control_reqseq(u16 control)
+{
+ return (control & L2CAP_CONTROL_REQSEQ) >>
+ L2CAP_CONTROL_REQSEQ_SHIFT;
+}
+
+static inline u16 l2cap_txseq_to_reqseq(u16 control)
+{
+ return (control & L2CAP_CONTROL_TXSEQ) <<
+ (L2CAP_CONTROL_REQSEQ_SHIFT - L2CAP_CONTROL_TXSEQ_SHIFT);
+}
+
+static inline int l2cap_is_I_frame(u16 control)
+{
+ return !(control & L2CAP_CONTROL_FRAME_TYPE);
+}
+
+static inline int l2cap_is_S_frame(u16 control)
+{
+ return (control & L2CAP_CONTROL_FRAME_TYPE);
+}
void l2cap_load(void);
diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index af0fbf9..17e1d8b 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -69,6 +69,7 @@ static void l2cap_sock_kill(struct sock *sk);
static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
u8 code, u8 ident, u16 dlen, void *data);
+static struct sk_buff *l2cap_build_S_frame(struct l2cap_pinfo *pi, u16 control);
/* ---- L2CAP timers ---- */
static void l2cap_sock_timeout(unsigned long arg)
@@ -333,6 +334,18 @@ static inline int l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16
return hci_send_acl(conn->hcon, skb, 0);
}
+static inline int l2cap_send_S_frame(struct l2cap_pinfo *pi, u16 control)
+{
+ struct sk_buff *skb = l2cap_build_S_frame(pi, control);
+
+ BT_DBG("control 0x%2.2x", control);
+
+ if (!skb)
+ return -ENOMEM;
+
+ return hci_send_acl(pi->conn->hcon, skb, 0);
+}
+
static void l2cap_do_start(struct sock *sk)
{
struct l2cap_conn *conn = l2cap_pi(sk)->conn;
@@ -1149,39 +1162,84 @@ static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *l
return 0;
}
-static inline int l2cap_do_send(struct sock *sk, struct msghdr *msg, int len)
+static void l2cap_drop_acked_frames(struct sock *sk)
{
- struct l2cap_conn *conn = l2cap_pi(sk)->conn;
- struct sk_buff *skb, **frag;
- int err, hlen, count, sent = 0;
- struct l2cap_hdr *lh;
+ struct sk_buff *skb;
- BT_DBG("sk %p len %d", sk, len);
+ while ((skb = l2cap_skb_peek(sk))) {
- /* First fragment (with L2CAP header) */
- if (sk->sk_type == SOCK_DGRAM)
- hlen = L2CAP_HDR_SIZE + 2;
- else
- hlen = L2CAP_HDR_SIZE;
+ if (bt_cb(skb)->tx_seq == l2cap_pi(sk)->expected_ack_seq)
+ break;
- count = min_t(unsigned int, (conn->mtu - hlen), len);
+ skb = l2cap_skb_dequeue(sk);
+ kfree_skb(skb);
+ }
- skb = bt_skb_send_alloc(sk, hlen + count,
- msg->msg_flags & MSG_DONTWAIT, &err);
- if (!skb)
- return err;
+ return;
+}
- /* Create L2CAP header */
- lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
- lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
- lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
+static inline int l2cap_do_send(struct sock *sk, struct sk_buff *skb)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ int err;
- if (sk->sk_type == SOCK_DGRAM)
- put_unaligned(l2cap_pi(sk)->psm, (__le16 *) skb_put(skb, 2));
+ BT_DBG("sk %p, skb %p len %d", sk, skb, skb->len);
+
+ err = hci_send_acl(pi->conn->hcon, skb, 0);
+
+ if (unlikely(err) < 0)
+ kfree_skb(skb);
+
+ return err;
+}
+
+static int l2cap_ertm_send(struct sock *sk)
+{
+ struct sk_buff *skb, *tx_skb;
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ struct l2cap_ertm_hdr *lh;
+ u8 tx_seq;
+ u16 control;
+ int err;
+
+ while ((skb = l2cap_send_head(sk)) && (!l2cap_tx_window_full(sk))) {
+
+ tx_seq = pi->next_tx_seq;
+ tx_skb = skb_clone(skb, GFP_ATOMIC);
+
+ lh = (struct l2cap_ertm_hdr *) skb_push(tx_skb, 0);
+ control = (pi->req_seq << L2CAP_CONTROL_REQSEQ_SHIFT)
+ | (tx_seq << L2CAP_CONTROL_TXSEQ_SHIFT);
+ lh->control |= cpu_to_le16(control);
+
+ err = l2cap_do_send(sk, tx_skb);
+ if (err < 0) {
+ struct l2cap_disconn_req req;
+ req.dcid = cpu_to_le16(pi->dcid);
+ req.scid = cpu_to_le16(pi->scid);
+ l2cap_send_cmd(pi->conn, l2cap_get_ident(pi->conn),
+ L2CAP_DISCONN_REQ, sizeof(req), &req);
+ return err;
+ }
+
+ L2CAP_SEQ_NUM_INC(pi->next_tx_seq);
+ bt_cb(skb)->tx_seq = tx_seq;
+
+ l2cap_advance_send_head(sk, skb);
+ }
+
+ return 0;
+}
+
+
+static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb)
+{
+ struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+ struct sk_buff **frag;
+ int err, sent = 0;
if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {
- err = -EFAULT;
- goto fail;
+ return -EFAULT;
}
sent += count;
@@ -1194,33 +1252,72 @@ static inline int l2cap_do_send(struct sock *sk, struct msghdr *msg, int len)
*frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
if (!*frag)
- goto fail;
-
- if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) {
- err = -EFAULT;
- goto fail;
- }
+ return -EFAULT;
+ if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
+ return -EFAULT;
sent += count;
len -= count;
frag = &(*frag)->next;
}
- err = hci_send_acl(conn->hcon, skb, 0);
- if (err < 0)
- goto fail;
return sent;
+}
+
+static int l2cap_create_sdu(struct sock *sk, struct msghdr *msg, size_t len, u16 *control, struct sk_buff **out_skb)
+{
+ struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+ struct sk_buff *skb;
+ int err, count, hlen;
+ struct l2cap_hdr *lh;
+
+ hlen = (control) ? L2CAP_ERTM_HDR_SIZE : L2CAP_HDR_SIZE;
+
+ BT_DBG("sk %p len %d", sk, (int)len);
+
+ /* First fragment (with L2CAP header) */
+ if (sk->sk_type == SOCK_DGRAM)
+ hlen += 2;
+
+ count = min_t(unsigned int, (conn->mtu - hlen), len);
+ skb = bt_skb_send_alloc(sk, count + hlen,
+ msg->msg_flags & MSG_DONTWAIT, &err);
+ if (!skb)
+ return -ENOMEM;
+
+ /* Create L2CAP header */
+ lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
+ lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
+ lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
+
+ if (control)
+ put_unaligned(*control, (__le16 *) skb_put(skb, 2));
+
+ if (sk->sk_type == SOCK_DGRAM)
+ put_unaligned(l2cap_pi(sk)->psm, (__le16 *) skb_put(skb, 2));
+
+ err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
+ if (unlikely(err < 0))
+ kfree_skb(skb);
+ else
+ *out_skb = skb;
-fail:
- kfree_skb(skb);
return err;
}
+static inline int l2cap_create_basic_sdu(struct sock *sk, struct msghdr *msg, size_t len, struct sk_buff **out_skb)
+{
+ return l2cap_create_sdu(sk, msg, len, NULL, out_skb);
+}
+
static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
{
struct sock *sk = sock->sk;
- int err = 0;
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ struct sk_buff *skb;
+ u16 control;
+ int err;
BT_DBG("sock %p, sk %p", sock, sk);
@@ -1232,16 +1329,56 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
return -EOPNOTSUPP;
/* Check outgoing MTU */
- if (sk->sk_type != SOCK_RAW && len > l2cap_pi(sk)->omtu)
+ if ((sk->sk_type != SOCK_RAW || pi->mode == L2CAP_MODE_BASIC)
+ && len > pi->omtu)
return -EINVAL;
lock_sock(sk);
- if (sk->sk_state == BT_CONNECTED)
- err = l2cap_do_send(sk, msg, len);
- else
+ if (sk->sk_state != BT_CONNECTED) {
err = -ENOTCONN;
+ goto done;
+ }
+
+ switch (pi->mode) {
+ case L2CAP_MODE_BASIC:
+ err = l2cap_create_basic_sdu(sk, msg, len, &skb);
+ if (err < 0)
+ goto done;
+ err = l2cap_do_send(sk, skb);
+ if (!err)
+ err = len;
+ break;
+
+ case L2CAP_MODE_ERTM:
+
+ /* Entire SDU fits into one PDU */
+ if (len <= pi->omtu) {
+ control = L2CAP_SAR_UNSEGMENTED;
+ err = l2cap_create_sdu(sk, msg, len, &control, &skb);
+ }
+ else {
+ /* Segmentation will be added later */
+ err = -EINVAL;
+ goto done;
+ }
+ if (err < 0)
+ goto done;
+
+ l2cap_add_tx_queue_tail(sk, skb);
+
+ err = l2cap_ertm_send(sk);
+ if (!err)
+ err = len;
+ break;
+
+ default:
+ BT_DBG("bad state %1.1x", pi->mode);
+ err = -EINVAL;
+ }
+
+done:
release_sock(sk);
return err;
}
@@ -1678,6 +1815,30 @@ fail:
return NULL;
}
+static struct sk_buff *l2cap_build_S_frame(struct l2cap_pinfo *pi, u16 control)
+{
+ struct sk_buff *skb;
+ struct l2cap_ertm_hdr *lh;
+ struct l2cap_conn *conn = pi->conn;
+ int count;
+
+ BT_DBG("pi %p, control 0x%2.2x", pi, control);
+
+ count = min_t(unsigned int, conn->mtu, L2CAP_ERTM_HDR_SIZE);
+ control |= L2CAP_CONTROL_FRAME_TYPE;
+
+ skb = bt_skb_alloc(count, GFP_ATOMIC);
+ if (!skb)
+ return NULL;
+
+ lh = (struct l2cap_ertm_hdr *) skb_put(skb, L2CAP_ERTM_HDR_SIZE);
+ lh->len = cpu_to_le16(2);
+ lh->cid = cpu_to_le16(pi->dcid);
+ lh->control = cpu_to_le16(control);
+
+ return skb;
+}
+
static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
{
struct l2cap_conf_opt *opt = *ptr;
@@ -2305,6 +2466,10 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
sk->sk_state = BT_CONNECTED;
+ l2cap_pi(sk)->next_tx_seq = 0;
+ l2cap_pi(sk)->expected_ack_seq = 0;
+ l2cap_pi(sk)->conn_state |= L2CAP_CONN_TRANSMITTER;
+ l2cap_tx_queue_head_init(sk);
l2cap_chan_ready(sk);
goto unlock;
}
@@ -2389,6 +2554,8 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
sk->sk_state = BT_CONNECTED;
+ l2cap_pi(sk)->expected_tx_seq = 0;
+ l2cap_pi(sk)->num_to_ack = 0;
l2cap_chan_ready(sk);
}
@@ -2419,6 +2586,8 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd
sk->sk_shutdown = SHUTDOWN_MASK;
+ if (l2cap_pi(sk)->conn_state & L2CAP_CONN_TRANSMITTER)
+ skb_queue_purge(&l2cap_pi(sk)->tx_queue);
l2cap_chan_del(sk, ECONNRESET);
bh_unlock_sock(sk);
@@ -2441,6 +2610,8 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd
if (!sk)
return 0;
+ if (l2cap_pi(sk)->conn_state & L2CAP_CONN_TRANSMITTER)
+ skb_queue_purge(&l2cap_pi(sk)->tx_queue);
l2cap_chan_del(sk, 0);
bh_unlock_sock(sk);
@@ -2616,7 +2787,89 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *sk
kfree_skb(skb);
}
-static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
+static inline int l2cap_data_channel_I_frame(struct sock *sk, u16 rx_control, struct sk_buff *skb)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ u8 tx_seq = l2cap_control_txseq(rx_control);
+ u16 tx_control = 0;
+ int err;
+
+ BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
+
+ if (tx_seq != pi->expected_tx_seq)
+ return -EINVAL;
+
+ L2CAP_SEQ_NUM_INC(pi->expected_tx_seq);
+ err = sock_queue_rcv_skb(sk, skb);
+ if (err)
+ return err;
+
+ L2CAP_NUM_TO_ACK_INC(pi->num_to_ack);
+ if (pi->num_to_ack == L2CAP_DEFAULT_NUM_TO_ACK - 1) {
+ tx_control |= L2CAP_CONTROL_FRAME_TYPE;
+ tx_control |= L2CAP_SUPER_RCV_READY;
+ tx_control |= pi->expected_tx_seq << L2CAP_CONTROL_REQSEQ_SHIFT;
+
+ if (l2cap_send_S_frame(pi, tx_control))
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static inline int l2cap_data_channel_S_frame(struct sock *sk, u16 rx_control, struct sk_buff *skb)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ u16 tx_control;
+
+ BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
+
+ if (rx_control & L2CAP_CONTROL_POLL) {
+ tx_control = L2CAP_SUPER_RCV_READY | L2CAP_CONTROL_FINAL;
+ return l2cap_send_S_frame(pi, tx_control);
+ }
+
+ switch (rx_control & L2CAP_CONTROL_SUPERVISE) {
+ case L2CAP_SUPER_RCV_READY:
+ pi->expected_ack_seq = l2cap_control_reqseq(rx_control);
+ l2cap_drop_acked_frames(sk);
+ l2cap_ertm_send(sk);
+ break;
+
+ case L2CAP_SUPER_RCV_NOT_READY:
+ case L2CAP_SUPER_REJECT:
+ case L2CAP_SUPER_SELECT_REJECT:
+ break;
+ }
+
+ return 0;
+}
+
+static inline int l2cap_ertm_data_channel(struct sock *sk, struct l2cap_hdr *lh, struct sk_buff *skb)
+{
+ u16 control, len = skb->len;
+
+ BT_DBG("sk %p skb %p", sk, skb);
+
+ control = get_unaligned((__le16 *) skb->data);
+ skb_pull(skb, 2);
+ len -=2;
+ BT_DBG("control 0x%4.4x", control);
+
+ if (l2cap_pi(sk)->imtu < skb->len)
+ goto drop;
+
+ if (l2cap_is_I_frame(control))
+ return l2cap_data_channel_I_frame(sk, control, skb);
+ else
+ return l2cap_data_channel_S_frame(sk, control, skb);
+
+drop:
+ kfree_skb(skb);
+ return 0;
+}
+
+static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct l2cap_hdr *lh, struct sk_buff *skb)
{
struct sock *sk;
@@ -2631,16 +2884,29 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
if (sk->sk_state != BT_CONNECTED)
goto drop;
- if (l2cap_pi(sk)->imtu < skb->len)
- goto drop;
+ switch (l2cap_pi(sk)->mode) {
+ case L2CAP_MODE_BASIC:
+ /* If socket recv buffers overflows we drop data here
+ * which is *bad* because L2CAP has to be reliable.
+ * But we don't have any other choice. L2CAP doesn't
+ * provide flow control mechanism. */
- /* If socket recv buffers overflows we drop data here
- * which is *bad* because L2CAP has to be reliable.
- * But we don't have any other choice. L2CAP doesn't
- * provide flow control mechanism. */
+ if (l2cap_pi(sk)->imtu < skb->len)
+ goto drop;
- if (!sock_queue_rcv_skb(sk, skb))
- goto done;
+ if (!sock_queue_rcv_skb(sk, skb))
+ goto done;
+ break;
+
+ case L2CAP_MODE_ERTM:
+ if (!l2cap_ertm_data_channel(sk, lh, skb));
+ goto done;
+ break;
+
+ default:
+ BT_DBG("sk %p: bad mode 0x%2.2x", sk, l2cap_pi(sk)->mode);
+ break;
+ }
drop:
kfree_skb(skb);
@@ -2704,7 +2970,7 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
break;
default:
- l2cap_data_channel(conn, cid, skb);
+ l2cap_data_channel(conn, cid, lh, skb);
break;
}
}
@@ -2885,7 +3151,6 @@ static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 fl
goto drop;
BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
-
if (flags & ACL_START) {
struct l2cap_hdr *hdr;
int len;
--
1.6.3.3