struct sock *sk, *parent_sk;
int i;
-
mutex_lock(&local->socket_lock);
for (i = 0; i < LLCP_MAX_SAP; i++) {
/* 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);
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;
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;
}
}
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);
}
num_wks = ARRAY_SIZE(wks);
- for (sap = 0 ; sap < num_wks; sap++) {
+ for (sap = 0; sap < num_wks; sap++) {
if (wks[sap] == NULL)
continue;
}
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);
return LLCP_SAP_MAX;
}
- set_bit(BIT(ssap), &local->local_wks);
+ set_bit(ssap, &local->local_wks);
mutex_unlock(&local->sdp_lock);
return ssap;
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;
return LLCP_SAP_MAX;
}
- set_bit(BIT(local_ssap), &local->local_sap);
+ set_bit(local_ssap, &local->local_sap);
mutex_unlock(&local->sdp_lock);
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;
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 */
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);
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)) {
}
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)
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;
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);
}
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;
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;
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;
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);
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);
}
+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;
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");
}
}
- /* 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;
nfc_llcp_sock_put(llcp_sock);
}
-
if (sk->sk_state == LLCP_CONNECTED) {
nfc_put_device(local->dev);
sk->sk_state = LLCP_CLOSED;
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);
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);
}
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;
case LLCP_PDU_I:
case LLCP_PDU_RR:
+ case LLCP_PDU_RNR:
pr_debug("I frame\n");
nfc_llcp_recv_hdlc(local, skb);
break;
pr_debug("Received an LLCP PDU\n");
if (err < 0) {
- pr_err("err %d", err);
+ pr_err("err %d\n", err);
return;
}
if (local == NULL)
return;
+ nfc_llcp_clear_sdp(local);
+
/* Close and purge all existing sockets */
nfc_llcp_socket_release(local);
}
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));
}
}
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;
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;
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;
projects / cascardo / linux.git / blobdiff