From: Eric Lapuyade <eric.lapuyade@xxxxxxxxx>
Most NFC HCI chipsets actually use a simplified HDLC link layer to
carry HCI payloads.
This implementation registers itself as an HCI device on behalf of the
NFC driver.
Signed-off-by: Eric Lapuyade <eric.lapuyade@xxxxxxxxx>
Signed-off-by: Samuel Ortiz <sameo@xxxxxxxxxxxxxxx>
---
include/net/nfc/shdlc.h | 104 ++++++
net/nfc/hci/Kconfig | 8 +
net/nfc/hci/Makefile | 1 +
net/nfc/hci/shdlc.c | 945 +++++++++++++++++++++++++++++++++++++++++++++++
4 files changed, 1058 insertions(+), 0 deletions(-)
create mode 100644 include/net/nfc/shdlc.h
create mode 100644 net/nfc/hci/shdlc.c
diff --git a/include/net/nfc/shdlc.h b/include/net/nfc/shdlc.h
new file mode 100644
index 0000000..1071987
--- /dev/null
+++ b/include/net/nfc/shdlc.h
@@ -0,0 +1,104 @@
+/*
+ * Copyright (C) 2012 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef __NFC_SHDLC_H
+#define __NFC_SHDLC_H
+
+struct nfc_shdlc;
+
+struct nfc_shdlc_ops {
+ int (*open) (struct nfc_shdlc *shdlc);
+ void (*close) (struct nfc_shdlc *shdlc);
+ int (*hci_ready) (struct nfc_shdlc *shdlc);
+ int (*xmit) (struct nfc_shdlc *shdlc, struct sk_buff *skb);
+ int (*start_poll) (struct nfc_shdlc *shdlc, u32 protocols);
+ int (*target_from_gate) (struct nfc_shdlc *shdlc, u8 gate,
+ struct nfc_target *target);
+ int (*complete_target_discovered) (struct nfc_shdlc *shdlc, u8 gate,
+ struct nfc_target *target);
+ int (*data_exchange) (struct nfc_shdlc *shdlc,
+ struct nfc_target *target,
+ struct sk_buff *skb, struct sk_buff **res_skb);
+};
+
+enum shdlc_state {
+ SHDLC_DISCONNECTED = 0,
+ SHDLC_CONNECTING = 1,
+ SHDLC_NEGOCIATING = 2,
+ SHDLC_CONNECTED = 3
+};
+
+struct nfc_shdlc {
+ struct mutex state_mutex;
+ enum shdlc_state state;
+ int hard_fault;
+
+ struct nfc_hci_dev *hdev;
+
+ wait_queue_head_t *connect_wq;
+ int connect_tries;
+ int connect_result;
+ struct timer_list connect_timer;/* aka T3 in spec 10.6.1 */
+
+ u8 w; /* window size */
+ bool srej_support;
+
+ struct timer_list t1_timer; /* send ack timeout */
+ bool t1_active;
+
+ struct timer_list t2_timer; /* guard/retransmit timeout */
+ bool t2_active;
+
+ int ns; /* next seq num for send */
+ int nr; /* next expected seq num for receive */
+ int dnr; /* oldest sent unacked seq num */
+
+ struct sk_buff_head rcv_q;
+
+ struct sk_buff_head send_q;
+ bool rnr; /* other side is not ready to receive */
+
+ struct sk_buff_head ack_pending_q;
+
+ struct workqueue_struct *sm_wq;
+ struct work_struct sm_work;
+
+ struct nfc_shdlc_ops *ops;
+
+ int client_headroom;
+ int client_tailroom;
+
+ void *clientdata;
+};
+
+void nfc_shdlc_recv_frame(struct nfc_shdlc *shdlc, struct sk_buff *skb);
+
+struct nfc_shdlc *nfc_shdlc_allocate(struct nfc_shdlc_ops *ops,
+ struct nfc_hci_init_data *init_data,
+ u32 protocols,
+ int tx_headroom, int tx_tailroom,
+ int max_link_payload, const char *devname);
+
+void nfc_shdlc_free(struct nfc_shdlc *shdlc);
+
+void nfc_shdlc_set_clientdata(struct nfc_shdlc *shdlc, void *clientdata);
+void *nfc_shdlc_get_clientdata(struct nfc_shdlc *shdlc);
+struct nfc_hci_dev *nfc_shdlc_get_hci_dev(struct nfc_shdlc *shdlc);
+
+#endif /* __NFC_SHDLC_H */
diff --git a/net/nfc/hci/Kconfig b/net/nfc/hci/Kconfig
index c32d2d4..17213a6 100644
--- a/net/nfc/hci/Kconfig
+++ b/net/nfc/hci/Kconfig
@@ -6,3 +6,11 @@ config NFC_HCI
Say Y here if you want to build support for a kernel NFC HCI
implementation. This is mostly needed for devices that only process
HCI frames, like for example the NXP pn544.
+
+config NFC_SHDLC
+ depends on NFC_HCI
+ bool "SHDLC link layer for HCI based NFC drivers"
+ default n
+ ---help---
+ Say yes if you use an NFC HCI driver that requires SHDLC link layer.
+ If unsure, say N here.
diff --git a/net/nfc/hci/Makefile b/net/nfc/hci/Makefile
index af17c7b..f9c44b2 100644
--- a/net/nfc/hci/Makefile
+++ b/net/nfc/hci/Makefile
@@ -5,3 +5,4 @@
obj-$(CONFIG_NFC_HCI) += hci.o
hci-y := core.o hcp.o command.o
+hci-$(CONFIG_NFC_SHDLC) += shdlc.o
diff --git a/net/nfc/hci/shdlc.c b/net/nfc/hci/shdlc.c
new file mode 100644
index 0000000..923bdf7
--- /dev/null
+++ b/net/nfc/hci/shdlc.c
@@ -0,0 +1,945 @@
+/*
+ * Copyright (C) 2012 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#define pr_fmt(fmt) "shdlc: %s: " fmt, __func__
+
+#include <linux/sched.h>
+#include <linux/export.h>
+#include <linux/wait.h>
+#include <linux/crc-ccitt.h>
+#include <linux/slab.h>
+#include <linux/skbuff.h>
+
+#include <net/nfc/hci.h>
+#include <net/nfc/shdlc.h>
+
+#define SHDLC_LLC_HEAD_ROOM 2
+#define SHDLC_LLC_TAIL_ROOM 2
+
+#define SHDLC_MAX_WINDOW 4
+#define SHDLC_SREJ_SUPPORT false
+
+#define SHDLC_CONTROL_HEAD_MASK 0xe0
+#define SHDLC_CONTROL_HEAD_I 0x80
+#define SHDLC_CONTROL_HEAD_I2 0xa0
+#define SHDLC_CONTROL_HEAD_S 0xc0
+#define SHDLC_CONTROL_HEAD_U 0xe0
+
+#define SHDLC_CONTROL_NS_MASK 0x38
+#define SHDLC_CONTROL_NR_MASK 0x07
+#define SHDLC_CONTROL_TYPE_MASK 0x18
+
+#define SHDLC_CONTROL_M_MASK 0x1f
+
+enum sframe_type {
+ S_FRAME_RR = 0x00,
+ S_FRAME_REJ = 0x01,
+ S_FRAME_RNR = 0x02,
+ S_FRAME_SREJ = 0x03
+};
+
+enum uframe_modifier {
+ U_FRAME_UA = 0x06,
+ U_FRAME_RSET = 0x19
+};
+
+#define SHDLC_CONNECT_VALUE_MS 5
+#define SHDLC_T1_VALUE_MS(w) ((5 * w) / 4)
+#define SHDLC_T2_VALUE_MS 300
+
+#define SHDLC_DUMP_SKB(info, skb) \
+do { \
+ pr_debug("%s:\n", info); \
+ print_hex_dump(KERN_DEBUG, "shdlc: ", DUMP_PREFIX_OFFSET, \
+ 16, 1, skb->data, skb->len, 0); \
+} while (0)
+
+/* checks x < y <= z modulo 8 */
+static bool nfc_shdlc_x_lt_y_lteq_z(int x, int y, int z)
+{
+ if (x < z)
+ return ((x < y) && (y <= z)) ? true : false;
+ else
+ return ((y > x) || (y <= z)) ? true : false;
+}
+
+/* checks x <= y < z modulo 8 */
+static bool nfc_shdlc_x_lteq_y_lt_z(int x, int y, int z)
+{
+ if (x <= z)
+ return ((x <= y) && (y < z)) ? true : false;
+ else /* x > z -> z+8 > x */
+ return ((y >= x) || (y < z)) ? true : false;
+}
+
+static struct sk_buff *nfc_shdlc_alloc_skb(struct nfc_shdlc *shdlc,
+ int payload_len)
+{
+ struct sk_buff *skb;
+
+ skb = alloc_skb(shdlc->client_headroom + SHDLC_LLC_HEAD_ROOM +
+ shdlc->client_tailroom + SHDLC_LLC_TAIL_ROOM +
+ payload_len, GFP_KERNEL);
+ if (skb)
+ skb_reserve(skb, shdlc->client_headroom + SHDLC_LLC_HEAD_ROOM);
+
+ return skb;
+}
+
+static void nfc_shdlc_add_len_crc(struct sk_buff *skb)
+{
+ u16 crc;
+ int len;
+
+ len = skb->len + 2;
+ *skb_push(skb, 1) = len;
+
+ crc = crc_ccitt(0xffff, skb->data, skb->len);
+ crc = ~crc;
+ *skb_put(skb, 1) = crc & 0xff;
+ *skb_put(skb, 1) = crc >> 8;
+}
+
+/* immediately sends an S frame. */
+static int nfc_shdlc_send_s_frame(struct nfc_shdlc *shdlc,
+ enum sframe_type sframe_type, int nr)
+{
+ int r;
+ struct sk_buff *skb;
+
+ pr_debug("sframe_type=%d nr=%d\n", sframe_type, nr);
+
+ skb = nfc_shdlc_alloc_skb(shdlc, 0);
+ if (skb == NULL)
+ return -ENOMEM;
+
+ *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_S | (sframe_type << 3) | nr;
+
+ nfc_shdlc_add_len_crc(skb);
+
+ r = shdlc->ops->xmit(shdlc, skb);
+
+ kfree_skb(skb);
+
+ return r;
+}
+
+/* immediately sends an U frame. skb may contain optional payload */
+static int nfc_shdlc_send_u_frame(struct nfc_shdlc *shdlc,
+ struct sk_buff *skb,
+ enum uframe_modifier uframe_modifier)
+{
+ int r;
+
+ pr_debug("uframe_modifier=%d\n", uframe_modifier);
+
+ *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_U | uframe_modifier;
+
+ nfc_shdlc_add_len_crc(skb);
+
+ r = shdlc->ops->xmit(shdlc, skb);
+
+ kfree_skb(skb);
+
+ return r;
+}
+
+/*
+ * Free ack_pending frames until y_nr - 1, and reset t2 according to
+ * the remaining oldest ack_pending frame sent time
+ */
+static void nfc_shdlc_reset_t2(struct nfc_shdlc *shdlc, int y_nr)
+{
+ struct sk_buff *skb;
+ int dnr = shdlc->dnr; /* MUST initially be < y_nr */
+
+ pr_debug("release ack pending up to frame %d excluded\n", y_nr);
+
+ while (dnr != y_nr) {
+ pr_debug("release ack pending frame %d\n", dnr);
+
+ skb = skb_dequeue(&shdlc->ack_pending_q);
+ kfree_skb(skb);
+
+ dnr = (dnr + 1) % 8;
+ }
+
+ if (skb_queue_empty(&shdlc->ack_pending_q)) {
+ if (shdlc->t2_active) {
+ del_timer_sync(&shdlc->t2_timer);
+ shdlc->t2_active = false;
+
+ pr_debug
+ ("All sent frames acked. Stopped T2(retransmit)\n");
+ }
+ } else {
+ skb = skb_peek(&shdlc->ack_pending_q);
+
+ mod_timer(&shdlc->t2_timer, *(unsigned long *)skb->cb +
+ msecs_to_jiffies(SHDLC_T2_VALUE_MS));
+ shdlc->t2_active = true;
+
+ pr_debug
+ ("Start T2(retransmit) for remaining unacked sent frames\n");
+ }
+}
+
+/*
+ * Receive validated frames from lower layer. skb contains HCI payload only.
+ * Handle according to algorithm at spec:10.8.2
+ */
+static void nfc_shdlc_rcv_i_frame(struct nfc_shdlc *shdlc,
+ struct sk_buff *skb, int ns, int nr)
+{
+ int x_ns = ns;
+ int y_nr = nr;
+
+ pr_debug("recvd I-frame %d, remote waiting frame %d\n", ns, nr);
+
+ if (shdlc->state != SHDLC_CONNECTED)
+ goto exit;
+
+ if (x_ns != shdlc->nr) {
+ nfc_shdlc_send_s_frame(shdlc, S_FRAME_REJ, shdlc->nr);
+ goto exit;
+ }
+
+ if (shdlc->t1_active == false) {
+ shdlc->t1_active = true;
+ mod_timer(&shdlc->t1_timer,
+ msecs_to_jiffies(SHDLC_T1_VALUE_MS(shdlc->w)));
+ pr_debug("(re)Start T1(send ack)\n");
+ }
+
+ if (skb->len) {
+ nfc_hci_recv_frame(shdlc->hdev, skb);
+ skb = NULL;
+ }
+
+ shdlc->nr = (shdlc->nr + 1) % 8;
+
+ if (nfc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
+ nfc_shdlc_reset_t2(shdlc, y_nr);
+
+ shdlc->dnr = y_nr;
+ }
+
+exit:
+ if (skb)
+ kfree_skb(skb);
+}
+
+static void nfc_shdlc_rcv_ack(struct nfc_shdlc *shdlc, int y_nr)
+{
+ pr_debug("remote acked up to frame %d excluded\n", y_nr);
+
+ if (nfc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
+ nfc_shdlc_reset_t2(shdlc, y_nr);
+ shdlc->dnr = y_nr;
+ }
+}
+
+static void nfc_shdlc_requeue_ack_pending(struct nfc_shdlc *shdlc)
+{
+ struct sk_buff *skb;
+
+ pr_debug("ns reset to %d\n", shdlc->dnr);
+
+ while ((skb = skb_dequeue_tail(&shdlc->ack_pending_q))) {
+ skb_pull(skb, 2); /* remove len+control */
+ skb_trim(skb, skb->len - 2); /* remove crc */
+ skb_queue_head(&shdlc->send_q, skb);
+ }
+ shdlc->ns = shdlc->dnr;
+}
+
+static void nfc_shdlc_rcv_rej(struct nfc_shdlc *shdlc, int y_nr)
+{
+ struct sk_buff *skb;
+
+ pr_debug("remote asks retransmition from frame %d\n", y_nr);
+
+ if (nfc_shdlc_x_lteq_y_lt_z(shdlc->dnr, y_nr, shdlc->ns)) {
+ if (shdlc->t2_active) {
+ del_timer_sync(&shdlc->t2_timer);
+ shdlc->t2_active = false;
+ pr_debug("Stopped T2(retransmit)\n");
+ }
+
+ if (shdlc->dnr != y_nr) {
+ while ((shdlc->dnr = ((shdlc->dnr + 1) % 8)) != y_nr) {
+ skb = skb_dequeue(&shdlc->ack_pending_q);
+ kfree_skb(skb);
+ }
+ }
+
+ nfc_shdlc_requeue_ack_pending(shdlc);
+ }
+}
+
+/* See spec RR:10.8.3 REJ:10.8.4 */
+static void nfc_shdlc_rcv_s_frame(struct nfc_shdlc *shdlc,
+ enum sframe_type s_frame_type, int nr)
+{
+ struct sk_buff *skb;
+
+ if (shdlc->state != SHDLC_CONNECTED)
+ return;
+
+ switch (s_frame_type) {
+ case S_FRAME_RR:
+ nfc_shdlc_rcv_ack(shdlc, nr);
+ if (shdlc->rnr == true) { /* see SHDLC 10.7.7 */
+ shdlc->rnr = false;
+ if (shdlc->send_q.qlen == 0) {
+ skb = nfc_shdlc_alloc_skb(shdlc, 0);
+ if (skb)
+ skb_queue_tail(&shdlc->send_q, skb);
+ }
+ }
+ break;
+ case S_FRAME_REJ:
+ nfc_shdlc_rcv_rej(shdlc, nr);
+ break;
+ case S_FRAME_RNR:
+ nfc_shdlc_rcv_ack(shdlc, nr);
+ shdlc->rnr = true;
+ break;
+ default:
+ break;
+ }
+}
+
+static void nfc_shdlc_connect_complete(struct nfc_shdlc *shdlc, int r)
+{
+ pr_debug("result=%d\n", r);
+
+ del_timer_sync(&shdlc->connect_timer);
+
+ if (r == 0) {
+ shdlc->ns = 0;
+ shdlc->nr = 0;
+ shdlc->dnr = 0;
+
+ shdlc->state = SHDLC_CONNECTED;
+ } else {
+ shdlc->state = SHDLC_DISCONNECTED;
+
+ /*
+ * TODO: Could it be possible that there are pending
+ * executing commands that are waiting for connect to complete
+ * before they can be carried? As connect is a blocking
+ * operation, it would require that the userspace process can
+ * send commands on the same device from a second thread before
+ * the device is up. I don't think that is possible, is it?
+ */
+ }
+
+ shdlc->connect_result = r;
+
+ wake_up(shdlc->connect_wq);
+}
+
+static int nfc_shdlc_connect_initiate(struct nfc_shdlc *shdlc)
+{
+ struct sk_buff *skb;
+
+ pr_debug("\n");
+
+ skb = nfc_shdlc_alloc_skb(shdlc, 2);
+ if (skb == NULL)
+ return -ENOMEM;
+
+ *skb_put(skb, 1) = SHDLC_MAX_WINDOW;
+ *skb_put(skb, 1) = SHDLC_SREJ_SUPPORT ? 1 : 0;
+
+ return nfc_shdlc_send_u_frame(shdlc, skb, U_FRAME_RSET);
+}
+
+static int nfc_shdlc_connect_send_ua(struct nfc_shdlc *shdlc)
+{
+ struct sk_buff *skb;
+
+ pr_debug("\n");
+
+ skb = nfc_shdlc_alloc_skb(shdlc, 0);
+ if (skb == NULL)
+ return -ENOMEM;
+
+ return nfc_shdlc_send_u_frame(shdlc, skb, U_FRAME_UA);
+}
+
+static void nfc_shdlc_rcv_u_frame(struct nfc_shdlc *shdlc,
+ struct sk_buff *skb,
+ enum uframe_modifier u_frame_modifier)
+{
+ u8 w = SHDLC_MAX_WINDOW;
+ bool srej_support = SHDLC_SREJ_SUPPORT;
+ int r;
+
+ pr_debug("u_frame_modifier=%d\n", u_frame_modifier);
+
+ switch (u_frame_modifier) {
+ case U_FRAME_RSET:
+ if (shdlc->state == SHDLC_NEGOCIATING) {
+ /* we sent RSET, but chip wants to negociate */
+ if (skb->len > 0)
+ w = skb->data[0];
+
+ if (skb->len > 1)
+ srej_support = skb->data[1] & 0x01 ? true :
+ false;
+
+ if ((w <= SHDLC_MAX_WINDOW) &&
+ (SHDLC_SREJ_SUPPORT || (srej_support == false))) {
+ shdlc->w = w;
+ shdlc->srej_support = srej_support;
+ r = nfc_shdlc_connect_send_ua(shdlc);
+ nfc_shdlc_connect_complete(shdlc, r);
+ }
+ } else if (shdlc->state > SHDLC_NEGOCIATING) {
+ /*
+ * TODO: Chip wants to reset link
+ * send ua, empty skb lists, reset counters
+ * propagate info to HCI layer
+ */
+ }
+ break;
+ case U_FRAME_UA:
+ if ((shdlc->state == SHDLC_CONNECTING &&
+ shdlc->connect_tries > 0) ||
+ (shdlc->state == SHDLC_NEGOCIATING))
+ nfc_shdlc_connect_complete(shdlc, 0);
+ break;
+ default:
+ break;
+ }
+
+ kfree_skb(skb);
+}
+
+static void nfc_shdlc_handle_rcv_queue(struct nfc_shdlc *shdlc)
+{
+ struct sk_buff *skb;
+ u8 control;
+ int nr;
+ int ns;
+ enum sframe_type s_frame_type;
+ enum uframe_modifier u_frame_modifier;
+
+ if (shdlc->rcv_q.qlen)
+ pr_debug("rcvQlen=%d\n", shdlc->rcv_q.qlen);
+
+ while ((skb = skb_dequeue(&shdlc->rcv_q)) != NULL) {
+ control = skb->data[0];
+ skb_pull(skb, 1);
+ switch (control & SHDLC_CONTROL_HEAD_MASK) {
+ case SHDLC_CONTROL_HEAD_I:
+ case SHDLC_CONTROL_HEAD_I2:
+ ns = (control & SHDLC_CONTROL_NS_MASK) >> 3;
+ nr = control & SHDLC_CONTROL_NR_MASK;
+ nfc_shdlc_rcv_i_frame(shdlc, skb, ns, nr);
+ break;
+ case SHDLC_CONTROL_HEAD_S:
+ s_frame_type = (control & SHDLC_CONTROL_TYPE_MASK) >> 3;
+ nr = control & SHDLC_CONTROL_NR_MASK;
+ nfc_shdlc_rcv_s_frame(shdlc, s_frame_type, nr);
+ kfree_skb(skb);
+ break;
+ case SHDLC_CONTROL_HEAD_U:
+ u_frame_modifier = control & SHDLC_CONTROL_M_MASK;
+ nfc_shdlc_rcv_u_frame(shdlc, skb, u_frame_modifier);
+ break;
+ default:
+ pr_err("UNKNOWN Control=%d\n", control);
+ kfree_skb(skb);
+ break;
+ }
+ }
+}
+
+static int nfc_shdlc_w_used(int ns, int dnr)
+{
+ int unack_count;
+
+ if (dnr <= ns)
+ unack_count = ns - dnr;
+ else
+ unack_count = 8 - dnr + ns;
+
+ return unack_count;
+}
+
+/* Send frames according to algorithm at spec:10.8.1 */
+static void nfc_shdlc_handle_send_queue(struct nfc_shdlc *shdlc)
+{
+ struct sk_buff *skb;
+ int r;
+ unsigned long time_sent;
+
+ if (shdlc->send_q.qlen)
+ pr_debug
+ ("sendQlen=%d ns=%d dnr=%d rnr=%s w_room=%d unackQlen=%d\n",
+ shdlc->send_q.qlen, shdlc->ns, shdlc->dnr,
+ shdlc->rnr == false ? "false" : "true",
+ shdlc->w - nfc_shdlc_w_used(shdlc->ns, shdlc->dnr),
+ shdlc->ack_pending_q.qlen);
+
+ while (shdlc->send_q.qlen && shdlc->ack_pending_q.qlen < shdlc->w &&
+ (shdlc->rnr == false)) {
+
+ if (shdlc->t1_active) {
+ del_timer_sync(&shdlc->t1_timer);
+ shdlc->t1_active = false;
+ pr_debug("Stopped T1(send ack)\n");
+ }
+
+ skb = skb_dequeue(&shdlc->send_q);
+
+ *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_I | (shdlc->ns << 3) |
+ shdlc->nr;
+
+ pr_debug("Sending I-Frame %d, waiting to rcv %d\n", shdlc->ns,
+ shdlc->nr);
+ /* SHDLC_DUMP_SKB("shdlc frame written", skb); */
+
+ nfc_shdlc_add_len_crc(skb);
+
+ r = shdlc->ops->xmit(shdlc, skb);
+ if (r < 0) {
+ /*
+ * TODO: Cannot send, shdlc machine is dead, we
+ * must propagate the information up to HCI.
+ */
+ shdlc->hard_fault = r;
+ break;
+ }
+
+ shdlc->ns = (shdlc->ns + 1) % 8;
+
+ time_sent = jiffies;
+ *(unsigned long *)skb->cb = time_sent;
+
+ skb_queue_tail(&shdlc->ack_pending_q, skb);
+
+ if (shdlc->t2_active == false) {
+ shdlc->t2_active = true;
+ mod_timer(&shdlc->t2_timer, time_sent +
+ msecs_to_jiffies(SHDLC_T2_VALUE_MS));
+ pr_debug("Started T2 (retransmit)\n");
+ }
+ }
+}
+
+static void nfc_shdlc_connect_timeout(unsigned long data)
+{
+ struct nfc_shdlc *shdlc = (struct nfc_shdlc *)data;
+
+ pr_debug("\n");
+
+ queue_work(shdlc->sm_wq, &shdlc->sm_work);
+}
+
+static void nfc_shdlc_t1_timeout(unsigned long data)
+{
+ struct nfc_shdlc *shdlc = (struct nfc_shdlc *)data;
+
+ pr_debug("SoftIRQ: need to send ack\n");
+
+ queue_work(shdlc->sm_wq, &shdlc->sm_work);
+}
+
+static void nfc_shdlc_t2_timeout(unsigned long data)
+{
+ struct nfc_shdlc *shdlc = (struct nfc_shdlc *)data;
+
+ pr_debug("SoftIRQ: need to retransmit\n");
+
+ queue_work(shdlc->sm_wq, &shdlc->sm_work);
+}
+
+static void nfc_shdlc_sm_work(struct work_struct *work)
+{
+ struct nfc_shdlc *shdlc = container_of(work, struct nfc_shdlc, sm_work);
+ int r;
+
+ pr_debug("\n");
+
+ mutex_lock(&shdlc->state_mutex);
+
+ switch (shdlc->state) {
+ case SHDLC_DISCONNECTED:
+ skb_queue_purge(&shdlc->rcv_q);
+ skb_queue_purge(&shdlc->send_q);
+ skb_queue_purge(&shdlc->ack_pending_q);
+ break;
+ case SHDLC_CONNECTING:
+ if (shdlc->connect_tries++ < 5)
+ r = nfc_shdlc_connect_initiate(shdlc);
+ else
+ r = -ETIME;
+ if (r < 0)
+ nfc_shdlc_connect_complete(shdlc, r);
+ else {
+ mod_timer(&shdlc->connect_timer, jiffies +
+ msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS));
+
+ shdlc->state = SHDLC_NEGOCIATING;
+ }
+ break;
+ case SHDLC_NEGOCIATING:
+ if (timer_pending(&shdlc->connect_timer) == 0) {
+ shdlc->state = SHDLC_CONNECTING;
+ queue_work(shdlc->sm_wq, &shdlc->sm_work);
+ }
+
+ nfc_shdlc_handle_rcv_queue(shdlc);
+ break;
+ case SHDLC_CONNECTED:
+ nfc_shdlc_handle_rcv_queue(shdlc);
+ nfc_shdlc_handle_send_queue(shdlc);
+
+ if (shdlc->t1_active && timer_pending(&shdlc->t1_timer) == 0) {
+ pr_debug
+ ("Handle T1(send ack) elapsed (T1 now inactive)\n");
+
+ shdlc->t1_active = false;
+ r = nfc_shdlc_send_s_frame(shdlc, S_FRAME_RR,
+ shdlc->nr);
+ if (r < 0)
+ shdlc->hard_fault = r;
+ }
+
+ if (shdlc->t2_active && timer_pending(&shdlc->t2_timer) == 0) {
+ pr_debug
+ ("Handle T2(retransmit) elapsed (T2 inactive)\n");
+
+ shdlc->t2_active = false;
+
+ nfc_shdlc_requeue_ack_pending(shdlc);
+ nfc_shdlc_handle_send_queue(shdlc);
+ }
+
+ if (shdlc->hard_fault) {
+ /*
+ * TODO: Handle hard_fault that occured during
+ * this invocation of the shdlc worker
+ */
+ }
+ break;
+ default:
+ break;
+ }
+ mutex_unlock(&shdlc->state_mutex);
+}
+
+/*
+ * Called from syscall context to establish shdlc link. Sleeps until
+ * link is ready or failure.
+ */
+static int nfc_shdlc_connect(struct nfc_shdlc *shdlc)
+{
+ DECLARE_WAIT_QUEUE_HEAD_ONSTACK(connect_wq);
+
+ pr_debug("\n");
+
+ mutex_lock(&shdlc->state_mutex);
+
+ shdlc->state = SHDLC_CONNECTING;
+ shdlc->connect_wq = &connect_wq;
+ shdlc->connect_tries = 0;
+ shdlc->connect_result = 1;
+
+ mutex_unlock(&shdlc->state_mutex);
+
+ queue_work(shdlc->sm_wq, &shdlc->sm_work);
+
+ wait_event(connect_wq, shdlc->connect_result != 1);
+
+ return shdlc->connect_result;
+}
+
+static void nfc_shdlc_disconnect(struct nfc_shdlc *shdlc)
+{
+ pr_debug("\n");
+
+ mutex_lock(&shdlc->state_mutex);
+
+ shdlc->state = SHDLC_DISCONNECTED;
+
+ mutex_unlock(&shdlc->state_mutex);
+
+ queue_work(shdlc->sm_wq, &shdlc->sm_work);
+}
+
+/*
+ * Receive an incoming shdlc frame. Frame has already been crc-validated.
+ * skb contains only LLC header and payload.
+ * If skb == NULL, it is a notification that the link below is dead.
+ */
+void nfc_shdlc_recv_frame(struct nfc_shdlc *shdlc, struct sk_buff *skb)
+{
+ if (skb == NULL) {
+ pr_err("NULL Frame -> link is dead\n");
+ shdlc->hard_fault = -EREMOTEIO;
+ } else {
+ SHDLC_DUMP_SKB("incoming frame", skb);
+ skb_queue_tail(&shdlc->rcv_q, skb);
+ }
+
+ queue_work(shdlc->sm_wq, &shdlc->sm_work);
+}
+EXPORT_SYMBOL(nfc_shdlc_recv_frame);
+
+static int nfc_shdlc_open(struct nfc_hci_dev *hdev)
+{
+ struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev);
+ int r;
+
+ pr_debug("\n");
+
+ if (shdlc->ops->open) {
+ r = shdlc->ops->open(shdlc);
+ if (r < 0)
+ return r;
+ }
+
+ r = nfc_shdlc_connect(shdlc);
+ if (r < 0 && shdlc->ops->close)
+ shdlc->ops->close(shdlc);
+
+ return r;
+}
+
+static void nfc_shdlc_close(struct nfc_hci_dev *hdev)
+{
+ struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev);
+
+ pr_debug("\n");
+
+ nfc_shdlc_disconnect(shdlc);
+
+ if (shdlc->ops->close)
+ shdlc->ops->close(shdlc);
+}
+
+static int nfc_shdlc_hci_ready(struct nfc_hci_dev *hdev)
+{
+ struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev);
+ int r = 0;
+
+ pr_debug("\n");
+
+ if (shdlc->ops->hci_ready)
+ r = shdlc->ops->hci_ready(shdlc);
+
+ return r;
+}
+
+static int nfc_shdlc_xmit(struct nfc_hci_dev *hdev, struct sk_buff *skb)
+{
+ struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev);
+
+ SHDLC_DUMP_SKB("queuing HCP packet to shdlc", skb);
+
+ skb_queue_tail(&shdlc->send_q, skb);
+
+ queue_work(shdlc->sm_wq, &shdlc->sm_work);
+
+ return 0;
+}
+
+static int nfc_shdlc_start_poll(struct nfc_hci_dev *hdev, u32 protocols)
+{
+ struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev);
+
+ pr_debug("\n");
+
+ if (shdlc->ops->start_poll)
+ return shdlc->ops->start_poll(shdlc, protocols);
+
+ return 0;
+}
+
+static int nfc_shdlc_target_from_gate(struct nfc_hci_dev *hdev, u8 gate,
+ struct nfc_target *target)
+{
+ struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev);
+
+ if (shdlc->ops->target_from_gate)
+ return shdlc->ops->target_from_gate(shdlc, gate, target);
+
+ return -EPERM;
+}
+
+static int nfc_shdlc_complete_target_discovered(struct nfc_hci_dev *hdev,
+ u8 gate,
+ struct nfc_target *target)
+{
+ struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev);
+
+ pr_debug("\n");
+
+ if (shdlc->ops->complete_target_discovered)
+ return shdlc->ops->complete_target_discovered(shdlc, gate,
+ target);
+
+ return 0;
+}
+
+static int nfc_shdlc_data_exchange(struct nfc_hci_dev *hdev,
+ struct nfc_target *target,
+ struct sk_buff *skb,
+ struct sk_buff **res_skb)
+{
+ struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev);
+
+ if (shdlc->ops->data_exchange)
+ return shdlc->ops->data_exchange(shdlc, target, skb, res_skb);
+
+ return -EPERM;
+}
+
+static struct nfc_hci_ops shdlc_ops = {
+ .open = nfc_shdlc_open,
+ .close = nfc_shdlc_close,
+ .hci_ready = nfc_shdlc_hci_ready,
+ .xmit = nfc_shdlc_xmit,
+ .start_poll = nfc_shdlc_start_poll,
+ .target_from_gate = nfc_shdlc_target_from_gate,
+ .complete_target_discovered = nfc_shdlc_complete_target_discovered,
+ .data_exchange = nfc_shdlc_data_exchange,
+};
+
+struct nfc_shdlc *nfc_shdlc_allocate(struct nfc_shdlc_ops *ops,
+ struct nfc_hci_init_data *init_data,
+ u32 protocols,
+ int tx_headroom, int tx_tailroom,
+ int max_link_payload, const char *devname)
+{
+ struct nfc_shdlc *shdlc;
+ int r;
+ char name[32];
+
+ if (ops->xmit == NULL)
+ return NULL;
+
+ shdlc = kzalloc(sizeof(struct nfc_shdlc), GFP_KERNEL);
+ if (shdlc == NULL)
+ return NULL;
+
+ mutex_init(&shdlc->state_mutex);
+ shdlc->ops = ops;
+ shdlc->state = SHDLC_DISCONNECTED;
+
+ init_timer(&shdlc->connect_timer);
+ shdlc->connect_timer.data = (unsigned long)shdlc;
+ shdlc->connect_timer.function = nfc_shdlc_connect_timeout;
+
+ init_timer(&shdlc->t1_timer);
+ shdlc->t1_timer.data = (unsigned long)shdlc;
+ shdlc->t1_timer.function = nfc_shdlc_t1_timeout;
+
+ init_timer(&shdlc->t2_timer);
+ shdlc->t2_timer.data = (unsigned long)shdlc;
+ shdlc->t2_timer.function = nfc_shdlc_t2_timeout;
+
+ shdlc->w = SHDLC_MAX_WINDOW;
+ shdlc->srej_support = SHDLC_SREJ_SUPPORT;
+
+ skb_queue_head_init(&shdlc->rcv_q);
+ skb_queue_head_init(&shdlc->send_q);
+ skb_queue_head_init(&shdlc->ack_pending_q);
+
+ INIT_WORK(&shdlc->sm_work, nfc_shdlc_sm_work);
+ snprintf(name, sizeof(name), "%s_shdlc_sm_wq", devname);
+ shdlc->sm_wq = alloc_workqueue(name, WQ_NON_REENTRANT | WQ_UNBOUND |
+ WQ_MEM_RECLAIM, 1);
+ if (shdlc->sm_wq == NULL)
+ goto err_allocwq;
+
+ shdlc->client_headroom = tx_headroom;
+ shdlc->client_tailroom = tx_tailroom;
+
+ shdlc->hdev = nfc_hci_allocate_device(&shdlc_ops, init_data, protocols,
+ tx_headroom + SHDLC_LLC_HEAD_ROOM,
+ tx_tailroom + SHDLC_LLC_TAIL_ROOM,
+ max_link_payload);
+ if (shdlc->hdev == NULL)
+ goto err_allocdev;
+
+ nfc_hci_set_clientdata(shdlc->hdev, shdlc);
+
+ r = nfc_hci_register_device(shdlc->hdev);
+ if (r < 0)
+ goto err_regdev;
+
+ return shdlc;
+
+err_regdev:
+ nfc_hci_free_device(shdlc->hdev);
+
+err_allocdev:
+ destroy_workqueue(shdlc->sm_wq);
+
+err_allocwq:
+ kfree(shdlc);
+
+ return NULL;
+}
+EXPORT_SYMBOL(nfc_shdlc_allocate);
+
+void nfc_shdlc_free(struct nfc_shdlc *shdlc)
+{
+ pr_debug("\n");
+
+ /* TODO: Check that this cannot be called while still in use */
+
+ nfc_hci_unregister_device(shdlc->hdev);
+ nfc_hci_free_device(shdlc->hdev);
+
+ destroy_workqueue(shdlc->sm_wq);
+
+ skb_queue_purge(&shdlc->rcv_q);
+ skb_queue_purge(&shdlc->send_q);
+ skb_queue_purge(&shdlc->ack_pending_q);
+
+ kfree(shdlc);
+}
+EXPORT_SYMBOL(nfc_shdlc_free);
+
+void nfc_shdlc_set_clientdata(struct nfc_shdlc *shdlc, void *clientdata)
+{
+ pr_debug("\n");
+
+ shdlc->clientdata = clientdata;
+}
+EXPORT_SYMBOL(nfc_shdlc_set_clientdata);
+
+void *nfc_shdlc_get_clientdata(struct nfc_shdlc *shdlc)
+{
+ return shdlc->clientdata;
+}
+EXPORT_SYMBOL(nfc_shdlc_get_clientdata);
+
+struct nfc_hci_dev *nfc_shdlc_get_hci_dev(struct nfc_shdlc *shdlc)
+{
+ return shdlc->hdev;
+}
+EXPORT_SYMBOL(nfc_shdlc_get_hci_dev);
--
1.7.9.1
--
To unsubscribe from this list: send the line "unsubscribe linux-wireless" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at http://vger.kernel.org/majordomo-info.html
