summaryrefslogtreecommitdiff
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.c627
1 files changed, 410 insertions, 217 deletions
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 42994fa..82f0f75 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -31,47 +31,41 @@ static u8 llcp_magic[3] = {0x46, 0x66, 0x6d};
static struct list_head llcp_devices;
-static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
+void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk)
{
- struct nfc_llcp_sock *parent, *s, *n;
- struct sock *sk, *parent_sk;
- int i;
-
- mutex_lock(&local->socket_lock);
-
- for (i = 0; i < LLCP_MAX_SAP; i++) {
- parent = local->sockets[i];
- if (parent == NULL)
- continue;
-
- /* Release all child sockets */
- list_for_each_entry_safe(s, n, &parent->list, list) {
- list_del_init(&s->list);
- sk = &s->sk;
-
- lock_sock(sk);
-
- if (sk->sk_state == LLCP_CONNECTED)
- nfc_put_device(s->dev);
+ write_lock(&l->lock);
+ sk_add_node(sk, &l->head);
+ write_unlock(&l->lock);
+}
- sk->sk_state = LLCP_CLOSED;
+void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk)
+{
+ write_lock(&l->lock);
+ sk_del_node_init(sk);
+ write_unlock(&l->lock);
+}
- release_sock(sk);
+static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
+{
+ struct sock *sk;
+ struct hlist_node *node, *tmp;
+ struct nfc_llcp_sock *llcp_sock;
- sock_orphan(sk);
+ write_lock(&local->sockets.lock);
- s->local = NULL;
- }
+ sk_for_each_safe(sk, node, tmp, &local->sockets.head) {
+ llcp_sock = nfc_llcp_sock(sk);
- parent_sk = &parent->sk;
+ lock_sock(sk);
- lock_sock(parent_sk);
+ if (sk->sk_state == LLCP_CONNECTED)
+ nfc_put_device(llcp_sock->dev);
- if (parent_sk->sk_state == LLCP_LISTEN) {
+ if (sk->sk_state == LLCP_LISTEN) {
struct nfc_llcp_sock *lsk, *n;
struct sock *accept_sk;
- list_for_each_entry_safe(lsk, n, &parent->accept_queue,
+ list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
accept_queue) {
accept_sk = &lsk->sk;
lock_sock(accept_sk);
@@ -83,35 +77,94 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
release_sock(accept_sk);
sock_orphan(accept_sk);
+ }
- lsk->local = NULL;
+ if (listen == true) {
+ release_sock(sk);
+ continue;
}
}
- if (parent_sk->sk_state == LLCP_CONNECTED)
- nfc_put_device(parent->dev);
-
- parent_sk->sk_state = LLCP_CLOSED;
+ sk->sk_state = LLCP_CLOSED;
- release_sock(parent_sk);
+ release_sock(sk);
- sock_orphan(parent_sk);
+ sock_orphan(sk);
- parent->local = NULL;
+ sk_del_node_init(sk);
}
- mutex_unlock(&local->socket_lock);
+ write_unlock(&local->sockets.lock);
}
-static void nfc_llcp_clear_sdp(struct nfc_llcp_local *local)
+struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local)
{
- mutex_lock(&local->sdp_lock);
+ kref_get(&local->ref);
- local->local_wks = 0;
- local->local_sdp = 0;
- local->local_sap = 0;
+ return local;
+}
- mutex_unlock(&local->sdp_lock);
+static void local_release(struct kref *ref)
+{
+ struct nfc_llcp_local *local;
+
+ local = container_of(ref, struct nfc_llcp_local, ref);
+
+ list_del(&local->list);
+ nfc_llcp_socket_release(local, false);
+ del_timer_sync(&local->link_timer);
+ skb_queue_purge(&local->tx_queue);
+ destroy_workqueue(local->tx_wq);
+ destroy_workqueue(local->rx_wq);
+ destroy_workqueue(local->timeout_wq);
+ kfree_skb(local->rx_pending);
+ kfree(local);
+}
+
+int nfc_llcp_local_put(struct nfc_llcp_local *local)
+{
+ if (local == NULL)
+ return 0;
+
+ return kref_put(&local->ref, local_release);
+}
+
+static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
+ u8 ssap, u8 dsap)
+{
+ struct sock *sk;
+ struct hlist_node *node;
+ struct nfc_llcp_sock *llcp_sock;
+
+ pr_debug("ssap dsap %d %d\n", ssap, dsap);
+
+ if (ssap == 0 && dsap == 0)
+ return NULL;
+
+ read_lock(&local->sockets.lock);
+
+ llcp_sock = NULL;
+
+ sk_for_each(sk, node, &local->sockets.head) {
+ llcp_sock = nfc_llcp_sock(sk);
+
+ if (llcp_sock->ssap == ssap && llcp_sock->dsap == dsap)
+ break;
+ }
+
+ read_unlock(&local->sockets.lock);
+
+ if (llcp_sock == NULL)
+ return NULL;
+
+ sock_hold(&llcp_sock->sk);
+
+ return llcp_sock;
+}
+
+static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
+{
+ sock_put(&sock->sk);
}
static void nfc_llcp_timeout_work(struct work_struct *work)
@@ -174,6 +227,51 @@ static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len)
return -EINVAL;
}
+static
+struct nfc_llcp_sock *nfc_llcp_sock_from_sn(struct nfc_llcp_local *local,
+ u8 *sn, size_t sn_len)
+{
+ struct sock *sk;
+ struct hlist_node *node;
+ struct nfc_llcp_sock *llcp_sock, *tmp_sock;
+
+ pr_debug("sn %zd %p\n", sn_len, sn);
+
+ if (sn == NULL || sn_len == 0)
+ return NULL;
+
+ read_lock(&local->sockets.lock);
+
+ llcp_sock = NULL;
+
+ sk_for_each(sk, node, &local->sockets.head) {
+ tmp_sock = nfc_llcp_sock(sk);
+
+ pr_debug("llcp sock %p\n", tmp_sock);
+
+ if (tmp_sock->sk.sk_state != LLCP_LISTEN)
+ continue;
+
+ if (tmp_sock->service_name == NULL ||
+ tmp_sock->service_name_len == 0)
+ continue;
+
+ if (tmp_sock->service_name_len != sn_len)
+ continue;
+
+ if (memcmp(sn, tmp_sock->service_name, sn_len) == 0) {
+ llcp_sock = tmp_sock;
+ break;
+ }
+ }
+
+ read_unlock(&local->sockets.lock);
+
+ pr_debug("Found llcp sock %p\n", llcp_sock);
+
+ return llcp_sock;
+}
+
u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
struct nfc_llcp_sock *sock)
{
@@ -200,41 +298,26 @@ u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
}
/*
- * This is not a well known service,
- * we should try to find a local SDP free spot
+ * Check if there already is a non WKS socket bound
+ * to this service name.
*/
- ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP);
- if (ssap == LLCP_SDP_NUM_SAP) {
+ if (nfc_llcp_sock_from_sn(local, sock->service_name,
+ sock->service_name_len) != NULL) {
mutex_unlock(&local->sdp_lock);
return LLCP_SAP_MAX;
}
- pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap);
-
- set_bit(ssap, &local->local_sdp);
mutex_unlock(&local->sdp_lock);
- return LLCP_WKS_NUM_SAP + ssap;
+ return LLCP_SDP_UNBOUND;
- } else if (sock->ssap != 0) {
- if (sock->ssap < LLCP_WKS_NUM_SAP) {
- 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 (!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);
+ } else if (sock->ssap != 0 && sock->ssap < LLCP_WKS_NUM_SAP) {
+ if (!test_bit(sock->ssap, &local->local_wks)) {
+ set_bit(sock->ssap, &local->local_wks);
+ mutex_unlock(&local->sdp_lock);
- return sock->ssap;
- }
+ return sock->ssap;
}
}
@@ -271,8 +354,34 @@ void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap)
local_ssap = ssap;
sdp = &local->local_wks;
} else if (ssap < LLCP_LOCAL_NUM_SAP) {
+ atomic_t *client_cnt;
+
local_ssap = ssap - LLCP_WKS_NUM_SAP;
sdp = &local->local_sdp;
+ client_cnt = &local->local_sdp_cnt[local_ssap];
+
+ pr_debug("%d clients\n", atomic_read(client_cnt));
+
+ mutex_lock(&local->sdp_lock);
+
+ if (atomic_dec_and_test(client_cnt)) {
+ struct nfc_llcp_sock *l_sock;
+
+ pr_debug("No more clients for SAP %d\n", ssap);
+
+ clear_bit(local_ssap, sdp);
+
+ /* Find the listening sock and set it back to UNBOUND */
+ l_sock = nfc_llcp_sock_get(local, ssap, LLCP_SAP_SDP);
+ if (l_sock) {
+ l_sock->ssap = LLCP_SDP_UNBOUND;
+ nfc_llcp_sock_put(l_sock);
+ }
+ }
+
+ mutex_unlock(&local->sdp_lock);
+
+ return;
} else if (ssap < LLCP_MAX_SAP) {
local_ssap = ssap - LLCP_LOCAL_NUM_SAP;
sdp = &local->local_sap;
@@ -287,19 +396,26 @@ void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap)
mutex_unlock(&local->sdp_lock);
}
-u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len)
+static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local)
{
- struct nfc_llcp_local *local;
+ u8 ssap;
- local = nfc_llcp_find_local(dev);
- if (local == NULL) {
- *general_bytes_len = 0;
- return NULL;
+ mutex_lock(&local->sdp_lock);
+
+ ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP);
+ if (ssap == LLCP_SDP_NUM_SAP) {
+ mutex_unlock(&local->sdp_lock);
+
+ return LLCP_SAP_MAX;
}
- *general_bytes_len = local->gb_len;
+ pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap);
- return local->gb;
+ set_bit(ssap, &local->local_sdp);
+
+ mutex_unlock(&local->sdp_lock);
+
+ return LLCP_WKS_NUM_SAP + ssap;
}
static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
@@ -363,6 +479,23 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
return 0;
}
+u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len)
+{
+ struct nfc_llcp_local *local;
+
+ local = nfc_llcp_find_local(dev);
+ if (local == NULL) {
+ *general_bytes_len = 0;
+ return NULL;
+ }
+
+ nfc_llcp_build_gb(local);
+
+ *general_bytes_len = local->gb_len;
+
+ return local->gb;
+}
+
int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
{
struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
@@ -384,31 +517,9 @@ int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
return -EINVAL;
}
- return nfc_llcp_parse_tlv(local,
- &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);
- struct sk_buff *skb;
-
- skb = skb_dequeue(&local->tx_queue);
- if (skb != NULL) {
- pr_debug("Sending pending skb\n");
- print_hex_dump(KERN_DEBUG, "LLCP Tx: ", DUMP_PREFIX_OFFSET,
- 16, 1, skb->data, skb->len, true);
-
- nfc_data_exchange(local->dev, local->target_idx,
- skb, nfc_llcp_recv, local);
- } else {
- nfc_llcp_send_symm(local->dev);
- }
-
- mod_timer(&local->link_timer,
- jiffies + msecs_to_jiffies(local->remote_lto));
+ return nfc_llcp_parse_gb_tlv(local,
+ &local->remote_gb[3],
+ local->remote_gb_len - 3);
}
static u8 nfc_llcp_dsap(struct sk_buff *pdu)
@@ -443,51 +554,84 @@ static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
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)
+static void nfc_llcp_tx_work(struct work_struct *work)
{
- struct nfc_llcp_sock *sock, *llcp_sock, *n;
+ struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+ tx_work);
+ struct sk_buff *skb;
+ struct sock *sk;
+ struct nfc_llcp_sock *llcp_sock;
- pr_debug("ssap dsap %d %d\n", ssap, dsap);
+ skb = skb_dequeue(&local->tx_queue);
+ if (skb != NULL) {
+ sk = skb->sk;
+ llcp_sock = nfc_llcp_sock(sk);
+ if (llcp_sock != NULL) {
+ int ret;
+
+ pr_debug("Sending pending skb\n");
+ print_hex_dump(KERN_DEBUG, "LLCP Tx: ",
+ DUMP_PREFIX_OFFSET, 16, 1,
+ skb->data, skb->len, true);
+
+ ret = nfc_data_exchange(local->dev, local->target_idx,
+ skb, nfc_llcp_recv, local);
+
+ if (!ret && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
+ skb = skb_get(skb);
+ skb_queue_tail(&llcp_sock->tx_pending_queue,
+ skb);
+ }
+ } else {
+ nfc_llcp_send_symm(local->dev);
+ }
+ } else {
+ nfc_llcp_send_symm(local->dev);
+ }
- if (ssap == 0 && dsap == 0)
- return NULL;
+ mod_timer(&local->link_timer,
+ jiffies + msecs_to_jiffies(2 * local->remote_lto));
+}
- mutex_lock(&local->socket_lock);
- sock = local->sockets[ssap];
- if (sock == NULL) {
- mutex_unlock(&local->socket_lock);
- return NULL;
- }
+static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local,
+ u8 ssap)
+{
+ struct sock *sk;
+ struct nfc_llcp_sock *llcp_sock;
+ struct hlist_node *node;
- pr_debug("root dsap %d (%d)\n", sock->dsap, dsap);
+ read_lock(&local->connecting_sockets.lock);
- if (sock->dsap == dsap) {
- sock_hold(&sock->sk);
- mutex_unlock(&local->socket_lock);
- return sock;
- }
+ sk_for_each(sk, node, &local->connecting_sockets.head) {
+ llcp_sock = nfc_llcp_sock(sk);
- 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);
- if (llcp_sock->dsap == dsap) {
+ if (llcp_sock->ssap == ssap) {
sock_hold(&llcp_sock->sk);
- mutex_unlock(&local->socket_lock);
- return llcp_sock;
+ goto out;
}
}
- pr_err("Could not find socket for %d %d\n", ssap, dsap);
+ llcp_sock = NULL;
- mutex_unlock(&local->socket_lock);
+out:
+ read_unlock(&local->connecting_sockets.lock);
- return NULL;
+ return llcp_sock;
}
-static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
+static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local,
+ u8 *sn, size_t sn_len)
{
- sock_put(&sock->sk);
+ struct nfc_llcp_sock *llcp_sock;
+
+ llcp_sock = nfc_llcp_sock_from_sn(local, sn, sn_len);
+
+ if (llcp_sock == NULL)
+ return NULL;
+
+ sock_hold(&llcp_sock->sk);
+
+ return llcp_sock;
}
static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
@@ -518,35 +662,19 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
{
struct sock *new_sk, *parent;
struct nfc_llcp_sock *sock, *new_sock;
- u8 dsap, ssap, bound_sap, reason;
+ u8 dsap, ssap, reason;
dsap = nfc_llcp_dsap(skb);
ssap = nfc_llcp_ssap(skb);
pr_debug("%d %d\n", dsap, ssap);
- nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],
- skb->len - LLCP_HEADER_SIZE);
-
if (dsap != LLCP_SAP_SDP) {
- bound_sap = dsap;
-
- mutex_lock(&local->socket_lock);
- sock = local->sockets[dsap];
- if (sock == NULL) {
- mutex_unlock(&local->socket_lock);
+ sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
+ if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) {
reason = LLCP_DM_NOBOUND;
goto fail;
}
-
- sock_hold(&sock->sk);
- mutex_unlock(&local->socket_lock);
-
- lock_sock(&sock->sk);
-
- if (sock->dsap == LLCP_SAP_SDP &&
- sock->sk.sk_state == LLCP_LISTEN)
- goto enqueue;
} else {
u8 *sn;
size_t sn_len;
@@ -559,40 +687,15 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
pr_debug("Service name length %zu\n", sn_len);
- mutex_lock(&local->socket_lock);
- for (bound_sap = 0; bound_sap < LLCP_LOCAL_SAP_OFFSET;
- bound_sap++) {
- sock = local->sockets[bound_sap];
- if (sock == NULL)
- continue;
-
- if (sock->service_name == NULL ||
- 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)) {
- pr_debug("Found service name at SAP %d\n",
- bound_sap);
- sock_hold(&sock->sk);
- mutex_unlock(&local->socket_lock);
-
- lock_sock(&sock->sk);
-
- goto enqueue;
- }
+ sock = nfc_llcp_sock_get_sn(local, sn, sn_len);
+ if (sock == NULL) {
+ reason = LLCP_DM_NOBOUND;
+ goto fail;
}
- mutex_unlock(&local->socket_lock);
}
- reason = LLCP_DM_NOBOUND;
- goto fail;
+ lock_sock(&sock->sk);
-enqueue:
parent = &sock->sk;
if (sk_acceptq_is_full(parent)) {
@@ -602,6 +705,21 @@ enqueue:
goto fail;
}
+ if (sock->ssap == LLCP_SDP_UNBOUND) {
+ u8 ssap = nfc_llcp_reserve_sdp_ssap(local);
+
+ pr_debug("First client, reserving %d\n", ssap);
+
+ if (ssap == LLCP_SAP_MAX) {
+ reason = LLCP_DM_REJ;
+ release_sock(&sock->sk);
+ sock_put(&sock->sk);
+ goto fail;
+ }
+
+ sock->ssap = ssap;
+ }
+
new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC);
if (new_sk == NULL) {
reason = LLCP_DM_REJ;
@@ -612,15 +730,31 @@ enqueue:
new_sock = nfc_llcp_sock(new_sk);
new_sock->dev = local->dev;
- new_sock->local = local;
+ new_sock->local = nfc_llcp_local_get(local);
+ new_sock->miu = local->remote_miu;
new_sock->nfc_protocol = sock->nfc_protocol;
- new_sock->ssap = bound_sap;
new_sock->dsap = ssap;
+ new_sock->target_idx = local->target_idx;
new_sock->parent = parent;
+ new_sock->ssap = sock->ssap;
+ if (sock->ssap < LLCP_LOCAL_NUM_SAP && sock->ssap >= LLCP_WKS_NUM_SAP) {
+ atomic_t *client_count;
+
+ pr_debug("reserved_ssap %d for %p\n", sock->ssap, new_sock);
+
+ client_count =
+ &local->local_sdp_cnt[sock->ssap - LLCP_WKS_NUM_SAP];
+
+ atomic_inc(client_count);
+ new_sock->reserved_ssap = sock->ssap;
+ }
+
+ nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE],
+ skb->len - LLCP_HEADER_SIZE);
pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk);
- list_add_tail(&new_sock->list, &sock->list);
+ nfc_llcp_sock_link(&local->sockets, new_sk);
nfc_llcp_accept_enqueue(&sock->sk, new_sk);
@@ -654,12 +788,12 @@ int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
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);
+ sock->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;
+ skb_queue_len(&sock->tx_pending_queue) < sock->rw) {
+ struct sk_buff *pdu;
pdu = skb_dequeue(&sock->tx_queue);
if (pdu == NULL)
@@ -668,10 +802,7 @@ int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
/* 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++;
}
@@ -728,11 +859,21 @@ static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
llcp_sock->send_ack_n = nr;
- skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp)
- if (nfc_llcp_ns(s) <= nr) {
- skb_unlink(s, &llcp_sock->tx_pending_queue);
- kfree_skb(s);
- }
+ /* Remove and free all skbs until ns == nr */
+ skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) {
+ skb_unlink(s, &llcp_sock->tx_pending_queue);
+ kfree_skb(s);
+
+ if (nfc_llcp_ns(s) == nr)
+ break;
+ }
+
+ /* Re-queue the remaining skbs for transmission */
+ skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue,
+ s, tmp) {
+ skb_unlink(s, &llcp_sock->tx_pending_queue);
+ skb_queue_head(&local->tx_queue, s);
+ }
}
if (ptype == LLCP_PDU_RR)
@@ -740,7 +881,7 @@ static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
else if (ptype == LLCP_PDU_RNR)
llcp_sock->remote_ready = false;
- if (nfc_llcp_queue_i_frames(llcp_sock) == 0)
+ if (nfc_llcp_queue_i_frames(llcp_sock) == 0 && ptype == LLCP_PDU_I)
nfc_llcp_send_rr(llcp_sock);
release_sock(sk);
@@ -791,11 +932,7 @@ static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
dsap = nfc_llcp_dsap(skb);
ssap = nfc_llcp_ssap(skb);
- llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
-
- if (llcp_sock == NULL)
- llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
-
+ llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
if (llcp_sock == NULL) {
pr_err("Invalid CC\n");
nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
@@ -803,11 +940,15 @@ static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
return;
}
- llcp_sock->dsap = ssap;
sk = &llcp_sock->sk;
- nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],
- skb->len - LLCP_HEADER_SIZE);
+ /* Unlink from connecting and link to the client array */
+ nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
+ nfc_llcp_sock_link(&local->sockets, sk);
+ llcp_sock->dsap = ssap;
+
+ nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE],
+ skb->len - LLCP_HEADER_SIZE);
sk->sk_state = LLCP_CONNECTED;
sk->sk_state_change(sk);
@@ -815,6 +956,45 @@ static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
nfc_llcp_sock_put(llcp_sock);
}
+static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+ struct nfc_llcp_sock *llcp_sock;
+ struct sock *sk;
+ u8 dsap, ssap, reason;
+
+ dsap = nfc_llcp_dsap(skb);
+ ssap = nfc_llcp_ssap(skb);
+ reason = skb->data[2];
+
+ pr_debug("%d %d reason %d\n", ssap, dsap, reason);
+
+ switch (reason) {
+ case LLCP_DM_NOBOUND:
+ case LLCP_DM_REJ:
+ llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
+ break;
+
+ default:
+ llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
+ break;
+ }
+
+ if (llcp_sock == NULL) {
+ pr_err("Invalid DM\n");
+ return;
+ }
+
+ sk = &llcp_sock->sk;
+
+ sk->sk_err = ENXIO;
+ sk->sk_state = LLCP_CLOSED;
+ sk->sk_state_change(sk);
+
+ nfc_llcp_sock_put(llcp_sock);
+
+ return;
+}
+
static void nfc_llcp_rx_work(struct work_struct *work)
{
struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
@@ -858,6 +1038,11 @@ static void nfc_llcp_rx_work(struct work_struct *work)
nfc_llcp_recv_cc(local, skb);
break;
+ case LLCP_PDU_DM:
+ pr_debug("DM\n");
+ nfc_llcp_recv_dm(local, skb);
+ break;
+
case LLCP_PDU_I:
case LLCP_PDU_RR:
case LLCP_PDU_RNR:
@@ -891,6 +1076,21 @@ void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
return;
}
+int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb)
+{
+ struct nfc_llcp_local *local;
+
+ local = nfc_llcp_find_local(dev);
+ if (local == NULL)
+ return -ENODEV;
+
+ local->rx_pending = skb_get(skb);
+ del_timer(&local->link_timer);
+ queue_work(local->rx_wq, &local->rx_work);
+
+ return 0;
+}
+
void nfc_llcp_mac_is_down(struct nfc_dev *dev)
{
struct nfc_llcp_local *local;
@@ -899,10 +1099,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);
+ nfc_llcp_socket_release(local, true);
}
void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
@@ -943,8 +1141,8 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
local->dev = ndev;
INIT_LIST_HEAD(&local->list);
+ kref_init(&local->ref);
mutex_init(&local->sdp_lock);
- mutex_init(&local->socket_lock);
init_timer(&local->link_timer);
local->link_timer.data = (unsigned long) local;
local->link_timer.function = nfc_llcp_symm_timer;
@@ -984,11 +1182,13 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
goto err_rx_wq;
}
+ local->sockets.lock = __RW_LOCK_UNLOCKED(local->sockets.lock);
+ local->connecting_sockets.lock = __RW_LOCK_UNLOCKED(local->connecting_sockets.lock);
+
nfc_llcp_build_gb(local);
local->remote_miu = LLCP_DEFAULT_MIU;
local->remote_lto = LLCP_DEFAULT_LTO;
- local->remote_rw = LLCP_DEFAULT_RW;
list_add(&llcp_devices, &local->list);
@@ -1015,14 +1215,7 @@ void nfc_llcp_unregister_device(struct nfc_dev *dev)
return;
}
- list_del(&local->list);
- nfc_llcp_socket_release(local);
- del_timer_sync(&local->link_timer);
- skb_queue_purge(&local->tx_queue);
- destroy_workqueue(local->tx_wq);
- destroy_workqueue(local->rx_wq);
- kfree_skb(local->rx_pending);
- kfree(local);
+ nfc_llcp_local_put(local);
}
int __init nfc_llcp_init(void)