1 files changed, 73 insertions, 9 deletions

diff --git a/drivers/nfc/pn533.c b/drivers/nfc/pn533.c
index 6a506e267d20..db4078765ac9 100644
--- a/drivers/nfc/pn533.c
+++ b/drivers/nfc/pn533.c
@@ -76,6 +76,7 @@ MODULE_DEVICE_TABLE(usb, pn533_table);
 
 #define PN533_CMD_TG_INIT_AS_TARGET 0x8c
 #define PN533_CMD_TG_GET_DATA 0x86
+#define PN533_CMD_TG_SET_DATA 0x8e
 
 #define PN533_CMD_RESPONSE(cmd) (cmd + 1)
 
@@ -1630,7 +1631,8 @@ static int pn533_dep_link_down(struct nfc_dev *nfc_dev)
         return 0;
 }
 
-static int pn533_data_exchange_tx_frame(struct pn533 *dev, struct sk_buff *skb)
+static int pn533_build_tx_frame(struct pn533 *dev, struct sk_buff *skb,
+                                bool target)
 {
         int payload_len = skb->len;
         struct pn533_frame *out_frame;
@@ -1647,14 +1649,20 @@ static int pn533_data_exchange_tx_frame(struct pn533 *dev, struct sk_buff *skb)
                 return -ENOSYS;
         }
 
-        skb_push(skb, PN533_CMD_DATAEXCH_HEAD_LEN);
-        out_frame = (struct pn533_frame *) skb->data;
+        if (target == true) {
+                skb_push(skb, PN533_CMD_DATAEXCH_HEAD_LEN);
+                out_frame = (struct pn533_frame *) skb->data;
 
-        pn533_tx_frame_init(out_frame, PN533_CMD_IN_DATA_EXCHANGE);
+                pn533_tx_frame_init(out_frame, PN533_CMD_IN_DATA_EXCHANGE);
+                tg = 1;
+                memcpy(PN533_FRAME_CMD_PARAMS_PTR(out_frame), &tg, sizeof(u8));
+                out_frame->datalen += sizeof(u8);
+        } else {
+                skb_push(skb, PN533_CMD_DATAEXCH_HEAD_LEN - 1);
+                out_frame = (struct pn533_frame *) skb->data;
+                pn533_tx_frame_init(out_frame, PN533_CMD_TG_SET_DATA);
+        }
 
-        tg = 1;
-        memcpy(PN533_FRAME_CMD_PARAMS_PTR(out_frame), &tg, sizeof(u8));
-        out_frame->datalen += sizeof(u8);
 
         /* The data is already in the out_frame, just update the datalen */
         out_frame->datalen += payload_len;
@@ -1785,7 +1793,7 @@ static int pn533_transceive(struct nfc_dev *nfc_dev,
                 goto error;
         }
 
-        rc = pn533_data_exchange_tx_frame(dev, skb);
+        rc = pn533_build_tx_frame(dev, skb, true);
         if (rc)
                 goto error;
 
@@ -1833,6 +1841,61 @@ error:
         return rc;
 }
 
+static int pn533_tm_send_complete(struct pn533 *dev, void *arg,
+                                  u8 *params, int params_len)
+{
+        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+
+        if (params_len < 0) {
+                nfc_dev_err(&dev->interface->dev,
+                            "Error %d when sending data",
+                            params_len);
+
+                return params_len;
+        }
+
+        if (params_len > 0 && params[0] != 0) {
+                nfc_tm_deactivated(dev->nfc_dev);
+
+                return 0;
+        }
+
+        queue_work(dev->wq, &dev->tg_work);
+
+        return 0;
+}
+
+static int pn533_tm_send(struct nfc_dev *nfc_dev, struct sk_buff *skb)
+{
+        struct pn533 *dev = nfc_get_drvdata(nfc_dev);
+        struct pn533_frame *out_frame;
+        int rc;
+
+        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+
+        rc = pn533_build_tx_frame(dev, skb, false);
+        if (rc)
+                goto error;
+
+        out_frame = (struct pn533_frame *) skb->data;
+
+        rc = pn533_send_cmd_frame_async(dev, out_frame, dev->in_frame,
+                                        dev->in_maxlen, pn533_tm_send_complete,
+                                        NULL, GFP_KERNEL);
+        if (rc) {
+                nfc_dev_err(&dev->interface->dev,
+                            "Error %d when trying to send data", rc);
+                goto error;
+        }
+
+        return 0;
+
+error:
+        kfree_skb(skb);
+
+        return rc;
+}
+
 static void pn533_wq_mi_recv(struct work_struct *work)
 {
         struct pn533 *dev = container_of(work, struct pn533, mi_work);
@@ -1853,7 +1916,7 @@ static void pn533_wq_mi_recv(struct work_struct *work)
 
         skb_reserve(skb_cmd, PN533_CMD_DATAEXCH_HEAD_LEN);
 
-        rc = pn533_data_exchange_tx_frame(dev, skb_cmd);
+        rc = pn533_build_tx_frame(dev, skb_cmd, true);
         if (rc)
                 goto error_frame;
 
@@ -1928,6 +1991,7 @@ struct nfc_ops pn533_nfc_ops = {
         .activate_target = pn533_activate_target,
         .deactivate_target = pn533_deactivate_target,
         .im_transceive = pn533_transceive,
+        .tm_send = pn533_tm_send,
 };
 
 static int pn533_probe(struct usb_interface *interface,