diff options
Diffstat (limited to 'net/nfc/llcp/llcp.c')
-rw-r--r-- | net/nfc/llcp/llcp.c | 127 |
1 files changed, 90 insertions, 37 deletions
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c index 7de0368aff0c..9e483c8e52f8 100644 --- a/net/nfc/llcp/llcp.c +++ b/net/nfc/llcp/llcp.c | |||
@@ -31,6 +31,8 @@ static u8 llcp_magic[3] = {0x46, 0x66, 0x6d}; | |||
31 | 31 | ||
32 | static struct list_head llcp_devices; | 32 | static struct list_head llcp_devices; |
33 | 33 | ||
34 | static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb); | ||
35 | |||
34 | void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk) | 36 | void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk) |
35 | { | 37 | { |
36 | write_lock(&l->lock); | 38 | write_lock(&l->lock); |
@@ -45,6 +47,12 @@ void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk) | |||
45 | write_unlock(&l->lock); | 47 | write_unlock(&l->lock); |
46 | } | 48 | } |
47 | 49 | ||
50 | void nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock *sock) | ||
51 | { | ||
52 | sock->remote_rw = LLCP_DEFAULT_RW; | ||
53 | sock->remote_miu = LLCP_MAX_MIU + 1; | ||
54 | } | ||
55 | |||
48 | static void nfc_llcp_socket_purge(struct nfc_llcp_sock *sock) | 56 | static void nfc_llcp_socket_purge(struct nfc_llcp_sock *sock) |
49 | { | 57 | { |
50 | struct nfc_llcp_local *local = sock->local; | 58 | struct nfc_llcp_local *local = sock->local; |
@@ -68,7 +76,7 @@ static void nfc_llcp_socket_purge(struct nfc_llcp_sock *sock) | |||
68 | } | 76 | } |
69 | } | 77 | } |
70 | 78 | ||
71 | static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen, | 79 | static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool device, |
72 | int err) | 80 | int err) |
73 | { | 81 | { |
74 | struct sock *sk; | 82 | struct sock *sk; |
@@ -108,21 +116,6 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen, | |||
108 | 116 | ||
109 | bh_unlock_sock(accept_sk); | 117 | bh_unlock_sock(accept_sk); |
110 | } | 118 | } |
111 | |||
112 | if (listen == true) { | ||
113 | bh_unlock_sock(sk); | ||
114 | continue; | ||
115 | } | ||
116 | } | ||
117 | |||
118 | /* | ||
119 | * If we have a connection less socket bound, we keep it alive | ||
120 | * if the device is still present. | ||
121 | */ | ||
122 | if (sk->sk_state == LLCP_BOUND && sk->sk_type == SOCK_DGRAM && | ||
123 | listen == true) { | ||
124 | bh_unlock_sock(sk); | ||
125 | continue; | ||
126 | } | 119 | } |
127 | 120 | ||
128 | if (err) | 121 | if (err) |
@@ -137,11 +130,8 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen, | |||
137 | 130 | ||
138 | write_unlock(&local->sockets.lock); | 131 | write_unlock(&local->sockets.lock); |
139 | 132 | ||
140 | /* | 133 | /* If we still have a device, we keep the RAW sockets alive */ |
141 | * If we want to keep the listening sockets alive, | 134 | if (device == true) |
142 | * we don't touch the RAW ones. | ||
143 | */ | ||
144 | if (listen == true) | ||
145 | return; | 135 | return; |
146 | 136 | ||
147 | write_lock(&local->raw_sockets.lock); | 137 | write_lock(&local->raw_sockets.lock); |
@@ -173,9 +163,9 @@ struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local) | |||
173 | return local; | 163 | return local; |
174 | } | 164 | } |
175 | 165 | ||
176 | static void local_cleanup(struct nfc_llcp_local *local, bool listen) | 166 | static void local_cleanup(struct nfc_llcp_local *local) |
177 | { | 167 | { |
178 | nfc_llcp_socket_release(local, listen, ENXIO); | 168 | nfc_llcp_socket_release(local, false, ENXIO); |
179 | del_timer_sync(&local->link_timer); | 169 | del_timer_sync(&local->link_timer); |
180 | skb_queue_purge(&local->tx_queue); | 170 | skb_queue_purge(&local->tx_queue); |
181 | cancel_work_sync(&local->tx_work); | 171 | cancel_work_sync(&local->tx_work); |
@@ -194,7 +184,7 @@ static void local_release(struct kref *ref) | |||
194 | local = container_of(ref, struct nfc_llcp_local, ref); | 184 | local = container_of(ref, struct nfc_llcp_local, ref); |
195 | 185 | ||
196 | list_del(&local->list); | 186 | list_del(&local->list); |
197 | local_cleanup(local, false); | 187 | local_cleanup(local); |
198 | kfree(local); | 188 | kfree(local); |
199 | } | 189 | } |
200 | 190 | ||
@@ -1116,6 +1106,12 @@ static void nfc_llcp_recv_disc(struct nfc_llcp_local *local, | |||
1116 | dsap = nfc_llcp_dsap(skb); | 1106 | dsap = nfc_llcp_dsap(skb); |
1117 | ssap = nfc_llcp_ssap(skb); | 1107 | ssap = nfc_llcp_ssap(skb); |
1118 | 1108 | ||
1109 | if ((dsap == 0) && (ssap == 0)) { | ||
1110 | pr_debug("Connection termination"); | ||
1111 | nfc_dep_link_down(local->dev); | ||
1112 | return; | ||
1113 | } | ||
1114 | |||
1119 | llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); | 1115 | llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); |
1120 | if (llcp_sock == NULL) { | 1116 | if (llcp_sock == NULL) { |
1121 | nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); | 1117 | nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); |
@@ -1349,19 +1345,54 @@ exit: | |||
1349 | nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len); | 1345 | nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len); |
1350 | } | 1346 | } |
1351 | 1347 | ||
1352 | static void nfc_llcp_rx_work(struct work_struct *work) | 1348 | static void nfc_llcp_recv_agf(struct nfc_llcp_local *local, struct sk_buff *skb) |
1353 | { | 1349 | { |
1354 | struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, | 1350 | u8 ptype; |
1355 | rx_work); | 1351 | u16 pdu_len; |
1356 | u8 dsap, ssap, ptype; | 1352 | struct sk_buff *new_skb; |
1357 | struct sk_buff *skb; | ||
1358 | 1353 | ||
1359 | skb = local->rx_pending; | 1354 | if (skb->len <= LLCP_HEADER_SIZE) { |
1360 | if (skb == NULL) { | 1355 | pr_err("Malformed AGF PDU\n"); |
1361 | pr_debug("No pending SKB\n"); | ||
1362 | return; | 1356 | return; |
1363 | } | 1357 | } |
1364 | 1358 | ||
1359 | skb_pull(skb, LLCP_HEADER_SIZE); | ||
1360 | |||
1361 | while (skb->len > LLCP_AGF_PDU_HEADER_SIZE) { | ||
1362 | pdu_len = skb->data[0] << 8 | skb->data[1]; | ||
1363 | |||
1364 | skb_pull(skb, LLCP_AGF_PDU_HEADER_SIZE); | ||
1365 | |||
1366 | if (pdu_len < LLCP_HEADER_SIZE || pdu_len > skb->len) { | ||
1367 | pr_err("Malformed AGF PDU\n"); | ||
1368 | return; | ||
1369 | } | ||
1370 | |||
1371 | ptype = nfc_llcp_ptype(skb); | ||
1372 | |||
1373 | if (ptype == LLCP_PDU_SYMM || ptype == LLCP_PDU_AGF) | ||
1374 | goto next; | ||
1375 | |||
1376 | new_skb = nfc_alloc_recv_skb(pdu_len, GFP_KERNEL); | ||
1377 | if (new_skb == NULL) { | ||
1378 | pr_err("Could not allocate PDU\n"); | ||
1379 | return; | ||
1380 | } | ||
1381 | |||
1382 | memcpy(skb_put(new_skb, pdu_len), skb->data, pdu_len); | ||
1383 | |||
1384 | nfc_llcp_rx_skb(local, new_skb); | ||
1385 | |||
1386 | kfree_skb(new_skb); | ||
1387 | next: | ||
1388 | skb_pull(skb, pdu_len); | ||
1389 | } | ||
1390 | } | ||
1391 | |||
1392 | static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb) | ||
1393 | { | ||
1394 | u8 dsap, ssap, ptype; | ||
1395 | |||
1365 | ptype = nfc_llcp_ptype(skb); | 1396 | ptype = nfc_llcp_ptype(skb); |
1366 | dsap = nfc_llcp_dsap(skb); | 1397 | dsap = nfc_llcp_dsap(skb); |
1367 | ssap = nfc_llcp_ssap(skb); | 1398 | ssap = nfc_llcp_ssap(skb); |
@@ -1372,10 +1403,6 @@ static void nfc_llcp_rx_work(struct work_struct *work) | |||
1372 | print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET, | 1403 | print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET, |
1373 | 16, 1, skb->data, skb->len, true); | 1404 | 16, 1, skb->data, skb->len, true); |
1374 | 1405 | ||
1375 | __net_timestamp(skb); | ||
1376 | |||
1377 | nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX); | ||
1378 | |||
1379 | switch (ptype) { | 1406 | switch (ptype) { |
1380 | case LLCP_PDU_SYMM: | 1407 | case LLCP_PDU_SYMM: |
1381 | pr_debug("SYMM\n"); | 1408 | pr_debug("SYMM\n"); |
@@ -1418,7 +1445,30 @@ static void nfc_llcp_rx_work(struct work_struct *work) | |||
1418 | nfc_llcp_recv_hdlc(local, skb); | 1445 | nfc_llcp_recv_hdlc(local, skb); |
1419 | break; | 1446 | break; |
1420 | 1447 | ||
1448 | case LLCP_PDU_AGF: | ||
1449 | pr_debug("AGF frame\n"); | ||
1450 | nfc_llcp_recv_agf(local, skb); | ||
1451 | break; | ||
1421 | } | 1452 | } |
1453 | } | ||
1454 | |||
1455 | static void nfc_llcp_rx_work(struct work_struct *work) | ||
1456 | { | ||
1457 | struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, | ||
1458 | rx_work); | ||
1459 | struct sk_buff *skb; | ||
1460 | |||
1461 | skb = local->rx_pending; | ||
1462 | if (skb == NULL) { | ||
1463 | pr_debug("No pending SKB\n"); | ||
1464 | return; | ||
1465 | } | ||
1466 | |||
1467 | __net_timestamp(skb); | ||
1468 | |||
1469 | nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX); | ||
1470 | |||
1471 | nfc_llcp_rx_skb(local, skb); | ||
1422 | 1472 | ||
1423 | schedule_work(&local->tx_work); | 1473 | schedule_work(&local->tx_work); |
1424 | kfree_skb(local->rx_pending); | 1474 | kfree_skb(local->rx_pending); |
@@ -1466,6 +1516,9 @@ void nfc_llcp_mac_is_down(struct nfc_dev *dev) | |||
1466 | if (local == NULL) | 1516 | if (local == NULL) |
1467 | return; | 1517 | return; |
1468 | 1518 | ||
1519 | local->remote_miu = LLCP_DEFAULT_MIU; | ||
1520 | local->remote_lto = LLCP_DEFAULT_LTO; | ||
1521 | |||
1469 | /* Close and purge all existing sockets */ | 1522 | /* Close and purge all existing sockets */ |
1470 | nfc_llcp_socket_release(local, true, 0); | 1523 | nfc_llcp_socket_release(local, true, 0); |
1471 | } | 1524 | } |
@@ -1553,7 +1606,7 @@ void nfc_llcp_unregister_device(struct nfc_dev *dev) | |||
1553 | return; | 1606 | return; |
1554 | } | 1607 | } |
1555 | 1608 | ||
1556 | local_cleanup(local, false); | 1609 | local_cleanup(local); |
1557 | 1610 | ||
1558 | nfc_llcp_local_put(local); | 1611 | nfc_llcp_local_put(local); |
1559 | } | 1612 | } |