summaryrefslogtreecommitdiffstats
path: root/net/nfc/llcp/llcp.c
diff options
context:
space:
mode:
Diffstat (limited to 'net/nfc/llcp/llcp.c')
-rw-r--r--net/nfc/llcp/llcp.c201
1 files changed, 122 insertions, 79 deletions
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 1d32680..17a578f 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -37,7 +37,6 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
struct sock *sk, *parent_sk;
int i;
-
mutex_lock(&local->socket_lock);
for (i = 0; i < LLCP_MAX_SAP; i++) {
@@ -47,7 +46,7 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
/* Release all child sockets */
list_for_each_entry_safe(s, n, &parent->list, list) {
- list_del(&s->list);
+ list_del_init(&s->list);
sk = &s->sk;
lock_sock(sk);
@@ -56,9 +55,12 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
nfc_put_device(s->dev);
sk->sk_state = LLCP_CLOSED;
- sock_set_flag(sk, SOCK_DEAD);
release_sock(sk);
+
+ sock_orphan(sk);
+
+ s->local = NULL;
}
parent_sk = &parent->sk;
@@ -70,18 +72,19 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
struct sock *accept_sk;
list_for_each_entry_safe(lsk, n, &parent->accept_queue,
- accept_queue) {
+ accept_queue) {
accept_sk = &lsk->sk;
lock_sock(accept_sk);
nfc_llcp_accept_unlink(accept_sk);
accept_sk->sk_state = LLCP_CLOSED;
- sock_set_flag(accept_sk, SOCK_DEAD);
release_sock(accept_sk);
sock_orphan(accept_sk);
+
+ lsk->local = NULL;
}
}
@@ -89,18 +92,32 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
nfc_put_device(parent->dev);
parent_sk->sk_state = LLCP_CLOSED;
- sock_set_flag(parent_sk, SOCK_DEAD);
release_sock(parent_sk);
+
+ sock_orphan(parent_sk);
+
+ parent->local = NULL;
}
mutex_unlock(&local->socket_lock);
}
+static void nfc_llcp_clear_sdp(struct nfc_llcp_local *local)
+{
+ mutex_lock(&local->sdp_lock);
+
+ local->local_wks = 0;
+ local->local_sdp = 0;
+ local->local_sap = 0;
+
+ mutex_unlock(&local->sdp_lock);
+}
+
static void nfc_llcp_timeout_work(struct work_struct *work)
{
struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
- timeout_work);
+ timeout_work);
nfc_dep_link_down(local->dev);
}
@@ -146,7 +163,7 @@ static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len)
num_wks = ARRAY_SIZE(wks);
- for (sap = 0 ; sap < num_wks; sap++) {
+ for (sap = 0; sap < num_wks; sap++) {
if (wks[sap] == NULL)
continue;
@@ -158,13 +175,13 @@ static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len)
}
u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
- struct nfc_llcp_sock *sock)
+ struct nfc_llcp_sock *sock)
{
mutex_lock(&local->sdp_lock);
if (sock->service_name != NULL && sock->service_name_len > 0) {
int ssap = nfc_llcp_wks_sap(sock->service_name,
- sock->service_name_len);
+ sock->service_name_len);
if (ssap > 0) {
pr_debug("WKS %d\n", ssap);
@@ -176,7 +193,7 @@ u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
return LLCP_SAP_MAX;
}
- set_bit(BIT(ssap), &local->local_wks);
+ set_bit(ssap, &local->local_wks);
mutex_unlock(&local->sdp_lock);
return ssap;
@@ -195,25 +212,25 @@ u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap);
- set_bit(BIT(ssap), &local->local_sdp);
+ set_bit(ssap, &local->local_sdp);
mutex_unlock(&local->sdp_lock);
return LLCP_WKS_NUM_SAP + ssap;
} else if (sock->ssap != 0) {
if (sock->ssap < LLCP_WKS_NUM_SAP) {
- if (!(local->local_wks & BIT(sock->ssap))) {
- set_bit(BIT(sock->ssap), &local->local_wks);
+ if (!test_bit(sock->ssap, &local->local_wks)) {
+ set_bit(sock->ssap, &local->local_wks);
mutex_unlock(&local->sdp_lock);
return sock->ssap;
}
} else if (sock->ssap < LLCP_SDP_NUM_SAP) {
- if (!(local->local_sdp &
- BIT(sock->ssap - LLCP_WKS_NUM_SAP))) {
- set_bit(BIT(sock->ssap - LLCP_WKS_NUM_SAP),
- &local->local_sdp);
+ if (!test_bit(sock->ssap - LLCP_WKS_NUM_SAP,
+ &local->local_sdp)) {
+ set_bit(sock->ssap - LLCP_WKS_NUM_SAP,
+ &local->local_sdp);
mutex_unlock(&local->sdp_lock);
return sock->ssap;
@@ -238,7 +255,7 @@ u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local)
return LLCP_SAP_MAX;
}
- set_bit(BIT(local_ssap), &local->local_sap);
+ set_bit(local_ssap, &local->local_sap);
mutex_unlock(&local->sdp_lock);
@@ -265,12 +282,12 @@ void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap)
mutex_lock(&local->sdp_lock);
- clear_bit(1 << local_ssap, sdp);
+ clear_bit(local_ssap, sdp);
mutex_unlock(&local->sdp_lock);
}
-u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, u8 *general_bytes_len)
+u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len)
{
struct nfc_llcp_local *local;
@@ -294,7 +311,7 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
version = LLCP_VERSION_11;
version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version,
- 1, &version_length);
+ 1, &version_length);
gb_len += version_length;
/* 1500 ms */
@@ -304,7 +321,7 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
pr_debug("Local wks 0x%lx\n", local->local_wks);
wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2,
- &wks_length);
+ &wks_length);
gb_len += wks_length;
gb_len += ARRAY_SIZE(llcp_magic);
@@ -349,8 +366,7 @@ int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
memcpy(local->remote_gb, gb, gb_len);
local->remote_gb_len = gb_len;
- if (local->remote_gb == NULL ||
- local->remote_gb_len == 0)
+ if (local->remote_gb == NULL || local->remote_gb_len == 0)
return -ENODEV;
if (memcmp(local->remote_gb, llcp_magic, 3)) {
@@ -359,26 +375,27 @@ int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
}
return nfc_llcp_parse_tlv(local,
- &local->remote_gb[3], local->remote_gb_len - 3);
+ &local->remote_gb[3],
+ local->remote_gb_len - 3);
}
static void nfc_llcp_tx_work(struct work_struct *work)
{
struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
- tx_work);
+ tx_work);
struct sk_buff *skb;
skb = skb_dequeue(&local->tx_queue);
if (skb != NULL) {
pr_debug("Sending pending skb\n");
nfc_data_exchange(local->dev, local->target_idx,
- skb, nfc_llcp_recv, local);
+ skb, nfc_llcp_recv, local);
} else {
nfc_llcp_send_symm(local->dev);
}
mod_timer(&local->link_timer,
- jiffies + msecs_to_jiffies(local->remote_lto));
+ jiffies + msecs_to_jiffies(local->remote_lto));
}
static u8 nfc_llcp_dsap(struct sk_buff *pdu)
@@ -408,13 +425,13 @@ static u8 nfc_llcp_nr(struct sk_buff *pdu)
static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
{
- pdu->data[2] = (sock->send_n << 4) | ((sock->recv_n - 1) % 16);
+ pdu->data[2] = (sock->send_n << 4) | (sock->recv_n % 16);
sock->send_n = (sock->send_n + 1) % 16;
sock->recv_ack_n = (sock->recv_n - 1) % 16;
}
static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
- u8 ssap, u8 dsap)
+ u8 ssap, u8 dsap)
{
struct nfc_llcp_sock *sock, *llcp_sock, *n;
@@ -438,7 +455,7 @@ static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
list_for_each_entry_safe(llcp_sock, n, &sock->list, list) {
pr_debug("llcp_sock %p sk %p dsap %d\n", llcp_sock,
- &llcp_sock->sk, llcp_sock->dsap);
+ &llcp_sock->sk, llcp_sock->dsap);
if (llcp_sock->dsap == dsap) {
sock_hold(&llcp_sock->sk);
mutex_unlock(&local->socket_lock);
@@ -482,7 +499,7 @@ static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
}
static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
- struct sk_buff *skb)
+ struct sk_buff *skb)
{
struct sock *new_sk, *parent;
struct nfc_llcp_sock *sock, *new_sock;
@@ -494,7 +511,7 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
pr_debug("%d %d\n", dsap, ssap);
nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],
- skb->len - LLCP_HEADER_SIZE);
+ skb->len - LLCP_HEADER_SIZE);
if (dsap != LLCP_SAP_SDP) {
bound_sap = dsap;
@@ -513,7 +530,7 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
lock_sock(&sock->sk);
if (sock->dsap == LLCP_SAP_SDP &&
- sock->sk.sk_state == LLCP_LISTEN)
+ sock->sk.sk_state == LLCP_LISTEN)
goto enqueue;
} else {
u8 *sn;
@@ -529,23 +546,23 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
mutex_lock(&local->socket_lock);
for (bound_sap = 0; bound_sap < LLCP_LOCAL_SAP_OFFSET;
- bound_sap++) {
+ bound_sap++) {
sock = local->sockets[bound_sap];
if (sock == NULL)
continue;
if (sock->service_name == NULL ||
- sock->service_name_len == 0)
+ sock->service_name_len == 0)
continue;
if (sock->service_name_len != sn_len)
continue;
if (sock->dsap == LLCP_SAP_SDP &&
- sock->sk.sk_state == LLCP_LISTEN &&
- !memcmp(sn, sock->service_name, sn_len)) {
+ sock->sk.sk_state == LLCP_LISTEN &&
+ !memcmp(sn, sock->service_name, sn_len)) {
pr_debug("Found service name at SAP %d\n",
- bound_sap);
+ bound_sap);
sock_hold(&sock->sk);
mutex_unlock(&local->socket_lock);
@@ -570,8 +587,7 @@ enqueue:
goto fail;
}
- new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type,
- GFP_ATOMIC);
+ new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC);
if (new_sk == NULL) {
reason = LLCP_DM_REJ;
release_sock(&sock->sk);
@@ -616,8 +632,39 @@ fail:
}
+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",
+ sock->remote_ready, skb_queue_len(&sock->tx_pending_queue),
+ local->remote_rw);
+
+ /* Try to queue some I frames for transmission */
+ while (sock->remote_ready &&
+ skb_queue_len(&sock->tx_pending_queue) < local->remote_rw) {
+ struct sk_buff *pdu, *pending_pdu;
+
+ pdu = skb_dequeue(&sock->tx_queue);
+ if (pdu == NULL)
+ break;
+
+ /* Update N(S)/N(R) */
+ nfc_llcp_set_nrns(sock, pdu);
+
+ pending_pdu = skb_clone(pdu, GFP_KERNEL);
+
+ 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,
- struct sk_buff *skb)
+ struct sk_buff *skb)
{
struct nfc_llcp_sock *llcp_sock;
struct sock *sk;
@@ -644,15 +691,15 @@ static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
nfc_llcp_sock_put(llcp_sock);
}
- if (ns == llcp_sock->recv_n)
- llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
- else
- pr_err("Received out of sequence I PDU\n");
-
/* Pass the payload upstream */
if (ptype == LLCP_PDU_I) {
pr_debug("I frame, queueing on %p\n", &llcp_sock->sk);
+ if (ns == llcp_sock->recv_n)
+ llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
+ else
+ pr_err("Received out of sequence I PDU\n");
+
skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE);
if (sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
pr_err("receive queue is full\n");
@@ -673,30 +720,20 @@ static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
}
}
- /* Queue some I frames for transmission */
- while (llcp_sock->remote_ready &&
- skb_queue_len(&llcp_sock->tx_pending_queue) <= local->remote_rw) {
- struct sk_buff *pdu, *pending_pdu;
-
- pdu = skb_dequeue(&llcp_sock->tx_queue);
- if (pdu == NULL)
- break;
-
- /* Update N(S)/N(R) */
- nfc_llcp_set_nrns(llcp_sock, pdu);
+ if (ptype == LLCP_PDU_RR)
+ llcp_sock->remote_ready = true;
+ else if (ptype == LLCP_PDU_RNR)
+ llcp_sock->remote_ready = false;
- pending_pdu = skb_clone(pdu, GFP_KERNEL);
-
- skb_queue_tail(&local->tx_queue, pdu);
- skb_queue_tail(&llcp_sock->tx_pending_queue, pending_pdu);
- }
+ if (nfc_llcp_queue_i_frames(llcp_sock) == 0)
+ nfc_llcp_send_rr(llcp_sock);
release_sock(sk);
nfc_llcp_sock_put(llcp_sock);
}
static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
- struct sk_buff *skb)
+ struct sk_buff *skb)
{
struct nfc_llcp_sock *llcp_sock;
struct sock *sk;
@@ -718,7 +755,6 @@ static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
nfc_llcp_sock_put(llcp_sock);
}
-
if (sk->sk_state == LLCP_CONNECTED) {
nfc_put_device(local->dev);
sk->sk_state = LLCP_CLOSED;
@@ -731,13 +767,11 @@ static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
nfc_llcp_sock_put(llcp_sock);
}
-static void nfc_llcp_recv_cc(struct nfc_llcp_local *local,
- struct sk_buff *skb)
+static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
{
struct nfc_llcp_sock *llcp_sock;
u8 dsap, ssap;
-
dsap = nfc_llcp_dsap(skb);
ssap = nfc_llcp_ssap(skb);
@@ -756,7 +790,7 @@ static void nfc_llcp_recv_cc(struct nfc_llcp_local *local,
llcp_sock->dsap = ssap;
nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],
- skb->len - LLCP_HEADER_SIZE);
+ skb->len - LLCP_HEADER_SIZE);
nfc_llcp_sock_put(llcp_sock);
}
@@ -764,7 +798,7 @@ static void nfc_llcp_recv_cc(struct nfc_llcp_local *local,
static void nfc_llcp_rx_work(struct work_struct *work)
{
struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
- rx_work);
+ rx_work);
u8 dsap, ssap, ptype;
struct sk_buff *skb;
@@ -802,6 +836,7 @@ static void nfc_llcp_rx_work(struct work_struct *work)
case LLCP_PDU_I:
case LLCP_PDU_RR:
+ case LLCP_PDU_RNR:
pr_debug("I frame\n");
nfc_llcp_recv_hdlc(local, skb);
break;
@@ -821,7 +856,7 @@ void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
pr_debug("Received an LLCP PDU\n");
if (err < 0) {
- pr_err("err %d", err);
+ pr_err("err %d\n", err);
return;
}
@@ -840,6 +875,8 @@ void nfc_llcp_mac_is_down(struct nfc_dev *dev)
if (local == NULL)
return;
+ nfc_llcp_clear_sdp(local);
+
/* Close and purge all existing sockets */
nfc_llcp_socket_release(local);
}
@@ -865,7 +902,7 @@ void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
queue_work(local->tx_wq, &local->tx_work);
} else {
mod_timer(&local->link_timer,
- jiffies + msecs_to_jiffies(local->remote_lto));
+ jiffies + msecs_to_jiffies(local->remote_lto));
}
}
@@ -891,8 +928,10 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
skb_queue_head_init(&local->tx_queue);
INIT_WORK(&local->tx_work, nfc_llcp_tx_work);
snprintf(name, sizeof(name), "%s_llcp_tx_wq", dev_name(dev));
- local->tx_wq = alloc_workqueue(name,
- WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
+ local->tx_wq =
+ alloc_workqueue(name,
+ WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
+ 1);
if (local->tx_wq == NULL) {
err = -ENOMEM;
goto err_local;
@@ -901,8 +940,10 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
local->rx_pending = NULL;
INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
snprintf(name, sizeof(name), "%s_llcp_rx_wq", dev_name(dev));
- local->rx_wq = alloc_workqueue(name,
- WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
+ local->rx_wq =
+ alloc_workqueue(name,
+ WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
+ 1);
if (local->rx_wq == NULL) {
err = -ENOMEM;
goto err_tx_wq;
@@ -910,8 +951,10 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
snprintf(name, sizeof(name), "%s_llcp_timeout_wq", dev_name(dev));
- local->timeout_wq = alloc_workqueue(name,
- WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
+ local->timeout_wq =
+ alloc_workqueue(name,
+ WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
+ 1);
if (local->timeout_wq == NULL) {
err = -ENOMEM;
goto err_rx_wq;
OpenPOWER on IntegriCloud