diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c index f6c2257c11aa82a0f1e9714b10b27e02803719d8..8ebd322b11ebfe5cd18382aa7116972efb551c42 100644 --- a/net/nfc/llcp/commands.c +++ b/net/nfc/llcp/commands.c @@ -428,3 +428,27 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock, return 0; } + +int nfc_llcp_send_rr(struct nfc_llcp_sock *sock) +{ + struct sk_buff *skb; + struct nfc_llcp_local *local; + + pr_debug("Send rr nr %d\n", sock->recv_n); + + local = sock->local; + if (local == NULL) + return -ENODEV; + + skb = llcp_allocate_pdu(sock, LLCP_PDU_RR, LLCP_SEQUENCE_SIZE); + if (skb == NULL) + return -ENOMEM; + + skb_put(skb, LLCP_SEQUENCE_SIZE); + + skb->data[2] = sock->recv_n % 16; + + skb_queue_head(&local->tx_queue, skb); + + return 0; +} diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c index 2506810cd780ed8f8e1e626a04743784a664e2da..a0cb133c610a3d4768a0c9036083a341411d5891 100644 --- a/net/nfc/llcp/llcp.c +++ b/net/nfc/llcp/llcp.c @@ -627,8 +627,9 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local, } -void nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock) +int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock) { + int nr_frames = 0; struct nfc_llcp_local *local = sock->local; pr_debug("Remote ready %d tx queue len %d remote rw %d", @@ -651,7 +652,10 @@ void nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock) skb_queue_tail(&local->tx_queue, pdu); skb_queue_tail(&sock->tx_pending_queue, pending_pdu); + nr_frames++; } + + return nr_frames; } static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local, @@ -716,7 +720,8 @@ static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local, else if (ptype == LLCP_PDU_RNR) llcp_sock->remote_ready = false; - nfc_llcp_queue_i_frames(llcp_sock); + if (nfc_llcp_queue_i_frames(llcp_sock) == 0) + nfc_llcp_send_rr(llcp_sock); release_sock(sk); nfc_llcp_sock_put(llcp_sock); diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h index 36d8572c8beb5d67b2e92d6042162c2bead92b17..054c64ffdaa79ffb80316dfb0e7ebabdcd5ec0bc 100644 --- a/net/nfc/llcp/llcp.h +++ b/net/nfc/llcp/llcp.h @@ -165,7 +165,7 @@ u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local, struct nfc_llcp_sock *sock); u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local); void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap); -void nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock); +int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock); /* Sock API */ struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp); @@ -190,6 +190,7 @@ int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason); int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock); int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock, struct msghdr *msg, size_t len); +int nfc_llcp_send_rr(struct nfc_llcp_sock *sock); /* Socket API */ int __init nfc_llcp_sock_init(void);