aboutsummaryrefslogtreecommitdiffstats
path: root/net/bluetooth
diff options
context:
space:
mode:
Diffstat (limited to 'net/bluetooth')
-rw-r--r--net/bluetooth/l2cap.c384
1 files changed, 335 insertions, 49 deletions
diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index c1b562085cb4..45b8697a7398 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -333,6 +333,30 @@ static inline int l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16
333 return hci_send_acl(conn->hcon, skb, 0); 333 return hci_send_acl(conn->hcon, skb, 0);
334} 334}
335 335
336static inline int l2cap_send_sframe(struct l2cap_pinfo *pi, u16 control)
337{
338 struct sk_buff *skb;
339 struct l2cap_hdr *lh;
340 struct l2cap_conn *conn = pi->conn;
341 int count;
342
343 BT_DBG("pi %p, control 0x%2.2x", pi, control);
344
345 count = min_t(unsigned int, conn->mtu, L2CAP_HDR_SIZE + 2);
346 control |= L2CAP_CTRL_FRAME_TYPE;
347
348 skb = bt_skb_alloc(count, GFP_ATOMIC);
349 if (!skb)
350 return -ENOMEM;
351
352 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
353 lh->len = cpu_to_le16(2);
354 lh->cid = cpu_to_le16(pi->dcid);
355 put_unaligned_le16(control, skb_put(skb, 2));
356
357 return hci_send_acl(pi->conn->hcon, skb, 0);
358}
359
336static void l2cap_do_start(struct sock *sk) 360static void l2cap_do_start(struct sock *sk)
337{ 361{
338 struct l2cap_conn *conn = l2cap_pi(sk)->conn; 362 struct l2cap_conn *conn = l2cap_pi(sk)->conn;
@@ -1154,39 +1178,80 @@ static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *l
1154 return 0; 1178 return 0;
1155} 1179}
1156 1180
1157static inline int l2cap_do_send(struct sock *sk, struct msghdr *msg, int len) 1181static void l2cap_drop_acked_frames(struct sock *sk)
1158{ 1182{
1159 struct l2cap_conn *conn = l2cap_pi(sk)->conn; 1183 struct sk_buff *skb;
1160 struct sk_buff *skb, **frag;
1161 int err, hlen, count, sent = 0;
1162 struct l2cap_hdr *lh;
1163 1184
1164 BT_DBG("sk %p len %d", sk, len); 1185 while ((skb = skb_peek(TX_QUEUE(sk)))) {
1186 if (bt_cb(skb)->tx_seq == l2cap_pi(sk)->expected_ack_seq)
1187 break;
1165 1188
1166 /* First fragment (with L2CAP header) */ 1189 skb = skb_dequeue(TX_QUEUE(sk));
1167 if (sk->sk_type == SOCK_DGRAM) 1190 kfree_skb(skb);
1168 hlen = L2CAP_HDR_SIZE + 2;
1169 else
1170 hlen = L2CAP_HDR_SIZE;
1171 1191
1172 count = min_t(unsigned int, (conn->mtu - hlen), len); 1192 l2cap_pi(sk)->unacked_frames--;
1193 }
1173 1194
1174 skb = bt_skb_send_alloc(sk, hlen + count, 1195 return;
1175 msg->msg_flags & MSG_DONTWAIT, &err); 1196}
1176 if (!skb)
1177 return err;
1178 1197
1179 /* Create L2CAP header */ 1198static inline int l2cap_do_send(struct sock *sk, struct sk_buff *skb)
1180 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 1199{
1181 lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid); 1200 struct l2cap_pinfo *pi = l2cap_pi(sk);
1182 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); 1201 int err;
1202
1203 BT_DBG("sk %p, skb %p len %d", sk, skb, skb->len);
1204
1205 err = hci_send_acl(pi->conn->hcon, skb, 0);
1206 if (err < 0)
1207 kfree_skb(skb);
1208
1209 return err;
1210}
1211
1212static int l2cap_ertm_send(struct sock *sk)
1213{
1214 struct sk_buff *skb, *tx_skb;
1215 struct l2cap_pinfo *pi = l2cap_pi(sk);
1216 u16 control;
1217 int err;
1218
1219 while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk))) {
1220 tx_skb = skb_clone(skb, GFP_ATOMIC);
1183 1221
1184 if (sk->sk_type == SOCK_DGRAM) 1222 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1185 put_unaligned(l2cap_pi(sk)->psm, (__le16 *) skb_put(skb, 2)); 1223 control |= (pi->req_seq << L2CAP_CTRL_REQSEQ_SHIFT)
1224 | (pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
1225 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1226
1227 err = l2cap_do_send(sk, tx_skb);
1228 if (err < 0) {
1229 l2cap_send_disconn_req(pi->conn, sk);
1230 return err;
1231 }
1232
1233 bt_cb(skb)->tx_seq = pi->next_tx_seq;
1234 pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
1235
1236 pi->unacked_frames++;
1237
1238 if (skb_queue_is_last(TX_QUEUE(sk), skb))
1239 sk->sk_send_head = NULL;
1240 else
1241 sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb);
1242 }
1243
1244 return 0;
1245}
1246
1247static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb)
1248{
1249 struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1250 struct sk_buff **frag;
1251 int err, sent = 0;
1186 1252
1187 if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) { 1253 if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {
1188 err = -EFAULT; 1254 return -EFAULT;
1189 goto fail;
1190 } 1255 }
1191 1256
1192 sent += count; 1257 sent += count;
@@ -1199,33 +1264,112 @@ static inline int l2cap_do_send(struct sock *sk, struct msghdr *msg, int len)
1199 1264
1200 *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err); 1265 *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
1201 if (!*frag) 1266 if (!*frag)
1202 goto fail; 1267 return -EFAULT;
1203 1268 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
1204 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) { 1269 return -EFAULT;
1205 err = -EFAULT;
1206 goto fail;
1207 }
1208 1270
1209 sent += count; 1271 sent += count;
1210 len -= count; 1272 len -= count;
1211 1273
1212 frag = &(*frag)->next; 1274 frag = &(*frag)->next;
1213 } 1275 }
1214 err = hci_send_acl(conn->hcon, skb, 0);
1215 if (err < 0)
1216 goto fail;
1217 1276
1218 return sent; 1277 return sent;
1278}
1219 1279
1220fail: 1280static struct sk_buff *l2cap_create_connless_pdu(struct sock *sk, struct msghdr *msg, size_t len)
1221 kfree_skb(skb); 1281{
1222 return err; 1282 struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1283 struct sk_buff *skb;
1284 int err, count, hlen = L2CAP_HDR_SIZE + 2;
1285 struct l2cap_hdr *lh;
1286
1287 BT_DBG("sk %p len %d", sk, (int)len);
1288
1289 count = min_t(unsigned int, (conn->mtu - hlen), len);
1290 skb = bt_skb_send_alloc(sk, count + hlen,
1291 msg->msg_flags & MSG_DONTWAIT, &err);
1292 if (!skb)
1293 return ERR_PTR(-ENOMEM);
1294
1295 /* Create L2CAP header */
1296 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1297 lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1298 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1299 put_unaligned_le16(l2cap_pi(sk)->psm, skb_put(skb, 2));
1300
1301 err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1302 if (unlikely(err < 0)) {
1303 kfree_skb(skb);
1304 return ERR_PTR(err);
1305 }
1306 return skb;
1307}
1308
1309static struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size_t len)
1310{
1311 struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1312 struct sk_buff *skb;
1313 int err, count, hlen = L2CAP_HDR_SIZE;
1314 struct l2cap_hdr *lh;
1315
1316 BT_DBG("sk %p len %d", sk, (int)len);
1317
1318 count = min_t(unsigned int, (conn->mtu - hlen), len);
1319 skb = bt_skb_send_alloc(sk, count + hlen,
1320 msg->msg_flags & MSG_DONTWAIT, &err);
1321 if (!skb)
1322 return ERR_PTR(-ENOMEM);
1323
1324 /* Create L2CAP header */
1325 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1326 lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1327 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1328
1329 err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1330 if (unlikely(err < 0)) {
1331 kfree_skb(skb);
1332 return ERR_PTR(err);
1333 }
1334 return skb;
1335}
1336
1337static struct sk_buff *l2cap_create_ertm_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 control)
1338{
1339 struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1340 struct sk_buff *skb;
1341 int err, count, hlen = L2CAP_HDR_SIZE + 2;
1342 struct l2cap_hdr *lh;
1343
1344 BT_DBG("sk %p len %d", sk, (int)len);
1345
1346 count = min_t(unsigned int, (conn->mtu - hlen), len);
1347 skb = bt_skb_send_alloc(sk, count + hlen,
1348 msg->msg_flags & MSG_DONTWAIT, &err);
1349 if (!skb)
1350 return ERR_PTR(-ENOMEM);
1351
1352 /* Create L2CAP header */
1353 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1354 lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1355 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1356 put_unaligned_le16(control, skb_put(skb, 2));
1357
1358 err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1359 if (unlikely(err < 0)) {
1360 kfree_skb(skb);
1361 return ERR_PTR(err);
1362 }
1363 return skb;
1223} 1364}
1224 1365
1225static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len) 1366static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
1226{ 1367{
1227 struct sock *sk = sock->sk; 1368 struct sock *sk = sock->sk;
1228 int err = 0; 1369 struct l2cap_pinfo *pi = l2cap_pi(sk);
1370 struct sk_buff *skb;
1371 u16 control;
1372 int err;
1229 1373
1230 BT_DBG("sock %p, sk %p", sock, sk); 1374 BT_DBG("sock %p, sk %p", sock, sk);
1231 1375
@@ -1237,16 +1381,67 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
1237 return -EOPNOTSUPP; 1381 return -EOPNOTSUPP;
1238 1382
1239 /* Check outgoing MTU */ 1383 /* Check outgoing MTU */
1240 if (sk->sk_type != SOCK_RAW && len > l2cap_pi(sk)->omtu) 1384 if (sk->sk_type == SOCK_SEQPACKET && pi->mode == L2CAP_MODE_BASIC
1385 && len > pi->omtu)
1241 return -EINVAL; 1386 return -EINVAL;
1242 1387
1243 lock_sock(sk); 1388 lock_sock(sk);
1244 1389
1245 if (sk->sk_state == BT_CONNECTED) 1390 if (sk->sk_state != BT_CONNECTED) {
1246 err = l2cap_do_send(sk, msg, len);
1247 else
1248 err = -ENOTCONN; 1391 err = -ENOTCONN;
1392 goto done;
1393 }
1394
1395 /* Connectionless channel */
1396 if (sk->sk_type == SOCK_DGRAM) {
1397 skb = l2cap_create_connless_pdu(sk, msg, len);
1398 err = l2cap_do_send(sk, skb);
1399 goto done;
1400 }
1249 1401
1402 switch (pi->mode) {
1403 case L2CAP_MODE_BASIC:
1404 /* Create a basic PDU */
1405 skb = l2cap_create_basic_pdu(sk, msg, len);
1406 if (IS_ERR(skb)) {
1407 err = PTR_ERR(skb);
1408 goto done;
1409 }
1410
1411 err = l2cap_do_send(sk, skb);
1412 if (!err)
1413 err = len;
1414 break;
1415
1416 case L2CAP_MODE_ERTM:
1417 /* Entire SDU fits into one PDU */
1418 if (len <= pi->omtu) {
1419 control = L2CAP_SDU_UNSEGMENTED;
1420 skb = l2cap_create_ertm_pdu(sk, msg, len, control);
1421 if (IS_ERR(skb)) {
1422 err = PTR_ERR(skb);
1423 goto done;
1424 }
1425 } else {
1426 /* FIXME: Segmentation will be added later */
1427 err = -EINVAL;
1428 goto done;
1429 }
1430 __skb_queue_tail(TX_QUEUE(sk), skb);
1431 if (sk->sk_send_head == NULL)
1432 sk->sk_send_head = skb;
1433
1434 err = l2cap_ertm_send(sk);
1435 if (!err)
1436 err = len;
1437 break;
1438
1439 default:
1440 BT_DBG("bad state %1.1x", pi->mode);
1441 err = -EINVAL;
1442 }
1443
1444done:
1250 release_sock(sk); 1445 release_sock(sk);
1251 return err; 1446 return err;
1252} 1447}
@@ -2301,6 +2496,10 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
2301 2496
2302 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) { 2497 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
2303 sk->sk_state = BT_CONNECTED; 2498 sk->sk_state = BT_CONNECTED;
2499 l2cap_pi(sk)->next_tx_seq = 0;
2500 l2cap_pi(sk)->expected_ack_seq = 0;
2501 l2cap_pi(sk)->unacked_frames = 0;
2502 __skb_queue_head_init(TX_QUEUE(sk));
2304 l2cap_chan_ready(sk); 2503 l2cap_chan_ready(sk);
2305 goto unlock; 2504 goto unlock;
2306 } 2505 }
@@ -2375,6 +2574,9 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
2375 2574
2376 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) { 2575 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
2377 sk->sk_state = BT_CONNECTED; 2576 sk->sk_state = BT_CONNECTED;
2577 l2cap_pi(sk)->expected_tx_seq = 0;
2578 l2cap_pi(sk)->num_to_ack = 0;
2579 __skb_queue_head_init(TX_QUEUE(sk));
2378 l2cap_chan_ready(sk); 2580 l2cap_chan_ready(sk);
2379 } 2581 }
2380 2582
@@ -2405,6 +2607,8 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd
2405 2607
2406 sk->sk_shutdown = SHUTDOWN_MASK; 2608 sk->sk_shutdown = SHUTDOWN_MASK;
2407 2609
2610 skb_queue_purge(TX_QUEUE(sk));
2611
2408 l2cap_chan_del(sk, ECONNRESET); 2612 l2cap_chan_del(sk, ECONNRESET);
2409 bh_unlock_sock(sk); 2613 bh_unlock_sock(sk);
2410 2614
@@ -2427,6 +2631,8 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd
2427 if (!sk) 2631 if (!sk)
2428 return 0; 2632 return 0;
2429 2633
2634 skb_queue_purge(TX_QUEUE(sk));
2635
2430 l2cap_chan_del(sk, 0); 2636 l2cap_chan_del(sk, 0);
2431 bh_unlock_sock(sk); 2637 bh_unlock_sock(sk);
2432 2638
@@ -2602,9 +2808,60 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *sk
2602 kfree_skb(skb); 2808 kfree_skb(skb);
2603} 2809}
2604 2810
2811static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
2812{
2813 struct l2cap_pinfo *pi = l2cap_pi(sk);
2814 u8 tx_seq = __get_txseq(rx_control);
2815 u16 tx_control = 0;
2816 int err = 0;
2817
2818 BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
2819
2820 if (tx_seq != pi->expected_tx_seq)
2821 return -EINVAL;
2822
2823 pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
2824 err = sock_queue_rcv_skb(sk, skb);
2825 if (err)
2826 return err;
2827
2828 pi->num_to_ack = (pi->num_to_ack + 1) % L2CAP_DEFAULT_NUM_TO_ACK;
2829 if (pi->num_to_ack == L2CAP_DEFAULT_NUM_TO_ACK - 1) {
2830 tx_control |= L2CAP_CTRL_FRAME_TYPE;
2831 tx_control |= L2CAP_SUPER_RCV_READY;
2832 tx_control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
2833 err = l2cap_send_sframe(pi, tx_control);
2834 }
2835 return err;
2836}
2837
2838static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
2839{
2840 struct l2cap_pinfo *pi = l2cap_pi(sk);
2841
2842 BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
2843
2844 switch (rx_control & L2CAP_CTRL_SUPERVISE) {
2845 case L2CAP_SUPER_RCV_READY:
2846 pi->expected_ack_seq = __get_reqseq(rx_control);
2847 l2cap_drop_acked_frames(sk);
2848 l2cap_ertm_send(sk);
2849 break;
2850
2851 case L2CAP_SUPER_RCV_NOT_READY:
2852 case L2CAP_SUPER_REJECT:
2853 case L2CAP_SUPER_SELECT_REJECT:
2854 break;
2855 }
2856
2857 return 0;
2858}
2859
2605static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb) 2860static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
2606{ 2861{
2607 struct sock *sk; 2862 struct sock *sk;
2863 u16 control;
2864 int err;
2608 2865
2609 sk = l2cap_get_chan_by_scid(&conn->chan_list, cid); 2866 sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
2610 if (!sk) { 2867 if (!sk) {
@@ -2617,16 +2874,40 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
2617 if (sk->sk_state != BT_CONNECTED) 2874 if (sk->sk_state != BT_CONNECTED)
2618 goto drop; 2875 goto drop;
2619 2876
2620 if (l2cap_pi(sk)->imtu < skb->len) 2877 switch (l2cap_pi(sk)->mode) {
2621 goto drop; 2878 case L2CAP_MODE_BASIC:
2879 /* If socket recv buffers overflows we drop data here
2880 * which is *bad* because L2CAP has to be reliable.
2881 * But we don't have any other choice. L2CAP doesn't
2882 * provide flow control mechanism. */
2622 2883
2623 /* If socket recv buffers overflows we drop data here 2884 if (l2cap_pi(sk)->imtu < skb->len)
2624 * which is *bad* because L2CAP has to be reliable. 2885 goto drop;
2625 * But we don't have any other choice. L2CAP doesn't
2626 * provide flow control mechanism. */
2627 2886
2628 if (!sock_queue_rcv_skb(sk, skb)) 2887 if (!sock_queue_rcv_skb(sk, skb))
2629 goto done; 2888 goto done;
2889 break;
2890
2891 case L2CAP_MODE_ERTM:
2892 control = get_unaligned_le16(skb->data);
2893 skb_pull(skb, 2);
2894
2895 if (l2cap_pi(sk)->imtu < skb->len)
2896 goto drop;
2897
2898 if (__is_iframe(control))
2899 err = l2cap_data_channel_iframe(sk, control, skb);
2900 else
2901 err = l2cap_data_channel_sframe(sk, control, skb);
2902
2903 if (!err)
2904 goto done;
2905 break;
2906
2907 default:
2908 BT_DBG("sk %p: bad mode 0x%2.2x", sk, l2cap_pi(sk)->mode);
2909 break;
2910 }
2630 2911
2631drop: 2912drop:
2632 kfree_skb(skb); 2913 kfree_skb(skb);
@@ -2676,6 +2957,11 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
2676 cid = __le16_to_cpu(lh->cid); 2957 cid = __le16_to_cpu(lh->cid);
2677 len = __le16_to_cpu(lh->len); 2958 len = __le16_to_cpu(lh->len);
2678 2959
2960 if (len != skb->len) {
2961 kfree_skb(skb);
2962 return;
2963 }
2964
2679 BT_DBG("len %d, cid 0x%4.4x", len, cid); 2965 BT_DBG("len %d, cid 0x%4.4x", len, cid);
2680 2966
2681 switch (cid) { 2967 switch (cid) {