3 files changed, 81 insertions, 9 deletions

diff --git a/net/nfc/digital.h b/net/nfc/digital.h
index 71ad7eefddd4..3c39c72eb038 100644
--- a/net/nfc/digital.h
+++ b/net/nfc/digital.h
@@ -29,6 +29,7 @@
 #define DIGITAL_CMD_TG_SEND        1
 #define DIGITAL_CMD_TG_LISTEN      2
 #define DIGITAL_CMD_TG_LISTEN_MDAA 3
+#define DIGITAL_CMD_TG_LISTEN_MD   4
 
 #define DIGITAL_MAX_HEADER_LEN 7
 #define DIGITAL_CRC_LEN        2
@@ -121,6 +122,8 @@ int digital_tg_send_dep_res(struct nfc_digital_dev *ddev, struct sk_buff *skb);
 
 int digital_tg_listen_nfca(struct nfc_digital_dev *ddev, u8 rf_tech);
 int digital_tg_listen_nfcf(struct nfc_digital_dev *ddev, u8 rf_tech);
+void digital_tg_recv_md_req(struct nfc_digital_dev *ddev, void *arg,
+                            struct sk_buff *resp);
 
 typedef u16 (*crc_func_t)(u16, const u8 *, size_t);
 
diff --git a/net/nfc/digital_core.c b/net/nfc/digital_core.c
index 361bc37d2db1..009bcf317101 100644
--- a/net/nfc/digital_core.c
+++ b/net/nfc/digital_core.c
@@ -201,6 +201,11 @@ static void digital_wq_cmd(struct work_struct *work)
                                                digital_send_cmd_complete, cmd);
                 break;
 
+        case DIGITAL_CMD_TG_LISTEN_MD:
+                rc = ddev->ops->tg_listen_md(ddev, cmd->timeout,
+                                               digital_send_cmd_complete, cmd);
+                break;
+
         default:
                 pr_err("Unknown cmd type %d\n", cmd->type);
                 return;
@@ -293,6 +298,12 @@ static int digital_tg_listen_mdaa(struct nfc_digital_dev *ddev, u8 rf_tech)
                                 500, digital_tg_recv_atr_req, NULL);
 }
 
+static int digital_tg_listen_md(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+        return digital_send_cmd(ddev, DIGITAL_CMD_TG_LISTEN_MD, NULL, NULL, 500,
+                                digital_tg_recv_md_req, NULL);
+}
+
 int digital_target_found(struct nfc_digital_dev *ddev,
                          struct nfc_target *target, u8 protocol)
 {
@@ -510,6 +521,9 @@ static int digital_start_poll(struct nfc_dev *nfc_dev, __u32 im_protocols,
                 if (ddev->ops->tg_listen_mdaa) {
                         digital_add_poll_tech(ddev, 0,
                                               digital_tg_listen_mdaa);
+                } else if (ddev->ops->tg_listen_md) {
+                        digital_add_poll_tech(ddev, 0,
+                                              digital_tg_listen_md);
                 } else {
                         digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_106A,
                                               digital_tg_listen_nfca);
@@ -737,7 +751,7 @@ struct nfc_digital_dev *nfc_digital_allocate_device(struct nfc_digital_ops *ops,
 
         if (!ops->in_configure_hw || !ops->in_send_cmd || !ops->tg_listen ||
             !ops->tg_configure_hw || !ops->tg_send_cmd || !ops->abort_cmd ||
-            !ops->switch_rf)
+            !ops->switch_rf || (ops->tg_listen_md && !ops->tg_get_rf_tech))
                 return NULL;
 
         ddev = kzalloc(sizeof(struct nfc_digital_dev), GFP_KERNEL);
diff --git a/net/nfc/digital_technology.c b/net/nfc/digital_technology.c
index d276518cc8bf..fb58ed2dd41d 100644
--- a/net/nfc/digital_technology.c
+++ b/net/nfc/digital_technology.c
@@ -1218,33 +1218,48 @@ exit:
         dev_kfree_skb(resp);
 }
 
-int digital_tg_listen_nfca(struct nfc_digital_dev *ddev, u8 rf_tech)
+static int digital_tg_config_nfca(struct nfc_digital_dev *ddev)
 {
         int rc;
 
-        rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH, rf_tech);
+        rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH,
+                                     NFC_DIGITAL_RF_TECH_106A);
         if (rc)
                 return rc;
 
-        rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
-                                     NFC_DIGITAL_FRAMING_NFCA_NFC_DEP);
+        return digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+                                       NFC_DIGITAL_FRAMING_NFCA_NFC_DEP);
+}
+
+int digital_tg_listen_nfca(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+        int rc;
+
+        rc = digital_tg_config_nfca(ddev);
         if (rc)
                 return rc;
 
         return digital_tg_listen(ddev, 300, digital_tg_recv_sens_req, NULL);
 }
 
-int digital_tg_listen_nfcf(struct nfc_digital_dev *ddev, u8 rf_tech)
+static int digital_tg_config_nfcf(struct nfc_digital_dev *ddev, u8 rf_tech)
 {
         int rc;
-        u8 *nfcid2;
 
         rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH, rf_tech);
         if (rc)
                 return rc;
 
-        rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
-                                     NFC_DIGITAL_FRAMING_NFCF_NFC_DEP);
+        return digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+                                       NFC_DIGITAL_FRAMING_NFCF_NFC_DEP);
+}
+
+int digital_tg_listen_nfcf(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+        int rc;
+        u8 *nfcid2;
+
+        rc = digital_tg_config_nfcf(ddev, rf_tech);
         if (rc)
                 return rc;
 
@@ -1258,3 +1273,43 @@ int digital_tg_listen_nfcf(struct nfc_digital_dev *ddev, u8 rf_tech)
 
         return digital_tg_listen(ddev, 300, digital_tg_recv_sensf_req, nfcid2);
 }
+
+void digital_tg_recv_md_req(struct nfc_digital_dev *ddev, void *arg,
+                            struct sk_buff *resp)
+{
+        u8 rf_tech;
+        int rc;
+
+        if (IS_ERR(resp)) {
+                resp = NULL;
+                goto exit_free_skb;
+        }
+
+        rc = ddev->ops->tg_get_rf_tech(ddev, &rf_tech);
+        if (rc)
+                goto exit_free_skb;
+
+        switch (rf_tech) {
+        case NFC_DIGITAL_RF_TECH_106A:
+                rc = digital_tg_config_nfca(ddev);
+                if (rc)
+                        goto exit_free_skb;
+                digital_tg_recv_sens_req(ddev, arg, resp);
+                break;
+        case NFC_DIGITAL_RF_TECH_212F:
+        case NFC_DIGITAL_RF_TECH_424F:
+                rc = digital_tg_config_nfcf(ddev, rf_tech);
+                if (rc)
+                        goto exit_free_skb;
+                digital_tg_recv_sensf_req(ddev, arg, resp);
+                break;
+        default:
+                goto exit_free_skb;
+        }
+
+        return;
+
+exit_free_skb:
+        digital_poll_next_tech(ddev);
+        dev_kfree_skb(resp);
+}