NFC: llcp: Only keep raw sockets alive when the LLCP local leaves

[karo-tx-linux.git] / net / nfc / llcp / llcp.c

diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index bb67b98b979728e88624ea1bfb8349be190a183e..99ec39d6e9377aa15dbdca396ddae7c1274bd960 100644 (file)
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -31,6 +31,8 @@ static u8 llcp_magic[3] = {0x46, 0x66, 0x6d};
 
 static struct list_head llcp_devices;
 
+static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb);
+
 void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk)
 {
        write_lock(&l->lock);
@@ -45,6 +47,12 @@ void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk)
        write_unlock(&l->lock);
 }
 
+void nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock *sock)
+{
+       sock->remote_rw = LLCP_DEFAULT_RW;
+       sock->remote_miu = LLCP_MAX_MIU + 1;
+}
+
 static void nfc_llcp_socket_purge(struct nfc_llcp_sock *sock)
 {
        struct nfc_llcp_local *local = sock->local;
@@ -68,7 +76,7 @@ static void nfc_llcp_socket_purge(struct nfc_llcp_sock *sock)
        }
 }
 
-static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen,
+static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool device,
                                    int err)
 {
        struct sock *sk;
@@ -107,26 +115,9 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen,
                                accept_sk->sk_state_change(sk);
 
                                bh_unlock_sock(accept_sk);
-
-                               sock_orphan(accept_sk);
-                       }
-
-                       if (listen == true) {
-                               bh_unlock_sock(sk);
-                               continue;
                        }
                }
 
-               /*
-                * If we have a connection less socket bound, we keep it alive
-                * if the device is still present.
-                */
-               if (sk->sk_state == LLCP_BOUND && sk->sk_type == SOCK_DGRAM &&
-                   listen == true) {
-                       bh_unlock_sock(sk);
-                       continue;
-               }
-
                if (err)
                        sk->sk_err = err;
                sk->sk_state = LLCP_CLOSED;
@@ -134,18 +125,13 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen,
 
                bh_unlock_sock(sk);
 
-               sock_orphan(sk);
-
                sk_del_node_init(sk);
        }
 
        write_unlock(&local->sockets.lock);
 
-       /*
-        * If we want to keep the listening sockets alive,
-        * we don't touch the RAW ones.
-        */
-       if (listen == true)
+       /* If we still have a device, we keep the RAW sockets alive */
+       if (device == true)
                return;
 
        write_lock(&local->raw_sockets.lock);
@@ -164,8 +150,6 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen,
 
                bh_unlock_sock(sk);
 
-               sock_orphan(sk);
-
                sk_del_node_init(sk);
        }
 
@@ -869,7 +853,6 @@ static void nfc_llcp_recv_ui(struct nfc_llcp_local *local,
                skb_get(skb);
        } else {
                pr_err("Receive queue is full\n");
-               kfree_skb(skb);
        }
 
        nfc_llcp_sock_put(llcp_sock);
@@ -1072,7 +1055,6 @@ static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
                        skb_get(skb);
                } else {
                        pr_err("Receive queue is full\n");
-                       kfree_skb(skb);
                }
        }
 
@@ -1357,19 +1339,54 @@ exit:
                nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len);
 }
 
-static void nfc_llcp_rx_work(struct work_struct *work)
+static void nfc_llcp_recv_agf(struct nfc_llcp_local *local, struct sk_buff *skb)
 {
-       struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
-                                                   rx_work);
-       u8 dsap, ssap, ptype;
-       struct sk_buff *skb;
+       u8 ptype;
+       u16 pdu_len;
+       struct sk_buff *new_skb;
 
-       skb = local->rx_pending;
-       if (skb == NULL) {
-               pr_debug("No pending SKB\n");
+       if (skb->len <= LLCP_HEADER_SIZE) {
+               pr_err("Malformed AGF PDU\n");
                return;
        }
 
+       skb_pull(skb, LLCP_HEADER_SIZE);
+
+       while (skb->len > LLCP_AGF_PDU_HEADER_SIZE) {
+               pdu_len = skb->data[0] << 8 | skb->data[1];
+
+               skb_pull(skb, LLCP_AGF_PDU_HEADER_SIZE);
+
+               if (pdu_len < LLCP_HEADER_SIZE || pdu_len > skb->len) {
+                       pr_err("Malformed AGF PDU\n");
+                       return;
+               }
+
+               ptype = nfc_llcp_ptype(skb);
+
+               if (ptype == LLCP_PDU_SYMM || ptype == LLCP_PDU_AGF)
+                       goto next;
+
+               new_skb = nfc_alloc_recv_skb(pdu_len, GFP_KERNEL);
+               if (new_skb == NULL) {
+                       pr_err("Could not allocate PDU\n");
+                       return;
+               }
+
+               memcpy(skb_put(new_skb, pdu_len), skb->data, pdu_len);
+
+               nfc_llcp_rx_skb(local, new_skb);
+
+               kfree_skb(new_skb);
+next:
+               skb_pull(skb, pdu_len);
+       }
+}
+
+static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+       u8 dsap, ssap, ptype;
+
        ptype = nfc_llcp_ptype(skb);
        dsap = nfc_llcp_dsap(skb);
        ssap = nfc_llcp_ssap(skb);
@@ -1380,10 +1397,6 @@ static void nfc_llcp_rx_work(struct work_struct *work)
                print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET,
                               16, 1, skb->data, skb->len, true);
 
-       __net_timestamp(skb);
-
-       nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX);
-
        switch (ptype) {
        case LLCP_PDU_SYMM:
                pr_debug("SYMM\n");
@@ -1426,7 +1439,30 @@ static void nfc_llcp_rx_work(struct work_struct *work)
                nfc_llcp_recv_hdlc(local, skb);
                break;
 
+       case LLCP_PDU_AGF:
+               pr_debug("AGF frame\n");
+               nfc_llcp_recv_agf(local, skb);
+               break;
        }
+}
+
+static void nfc_llcp_rx_work(struct work_struct *work)
+{
+       struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+                                                   rx_work);
+       struct sk_buff *skb;
+
+       skb = local->rx_pending;
+       if (skb == NULL) {
+               pr_debug("No pending SKB\n");
+               return;
+       }
+
+       __net_timestamp(skb);
+
+       nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX);
+
+       nfc_llcp_rx_skb(local, skb);
 
        schedule_work(&local->tx_work);
        kfree_skb(local->rx_pending);
@@ -1474,6 +1510,9 @@ void nfc_llcp_mac_is_down(struct nfc_dev *dev)
        if (local == NULL)
                return;
 
+       local->remote_miu = LLCP_DEFAULT_MIU;
+       local->remote_lto = LLCP_DEFAULT_LTO;
+
        /* Close and purge all existing sockets */
        nfc_llcp_socket_release(local, true, 0);
 }