2 files changed, 40 insertions, 0 deletions

diff --git a/include/net/nfc/nfc.h b/include/net/nfc/nfc.h
index 87a6417fc934..5eb80bb3cbb2 100644
--- a/include/net/nfc/nfc.h
+++ b/include/net/nfc/nfc.h
@@ -122,6 +122,8 @@ struct nfc_dev {
 
         bool shutting_down;
 
+        struct rfkill *rfkill;
+
         struct nfc_ops *ops;
 };
 #define to_nfc_dev(_dev) container_of(_dev, struct nfc_dev, dev)
diff --git a/net/nfc/core.c b/net/nfc/core.c
index c571ca9a960c..40d2527693da 100644
--- a/net/nfc/core.c
+++ b/net/nfc/core.c
@@ -27,6 +27,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/slab.h>
+#include <linux/rfkill.h>
 #include <linux/nfc.h>
 
 #include <net/genetlink.h>
@@ -58,6 +59,11 @@ int nfc_dev_up(struct nfc_dev *dev)
 
         device_lock(&dev->dev);
 
+        if (dev->rfkill && rfkill_blocked(dev->rfkill)) {
+                rc = -ERFKILL;
+                goto error;
+        }
+
         if (!device_is_registered(&dev->dev)) {
                 rc = -ENODEV;
                 goto error;
@@ -117,6 +123,24 @@ error:
         return rc;
 }
 
+static int nfc_rfkill_set_block(void *data, bool blocked)
+{
+        struct nfc_dev *dev = data;
+
+        pr_debug("%s blocked %d", dev_name(&dev->dev), blocked);
+
+        if (!blocked)
+                return 0;
+
+        nfc_dev_down(dev);
+
+        return 0;
+}
+
+static const struct rfkill_ops nfc_rfkill_ops = {
+        .set_block = nfc_rfkill_set_block,
+};
+
 /**
  * nfc_start_poll - start polling for nfc targets
  *
@@ -840,6 +864,15 @@ int nfc_register_device(struct nfc_dev *dev)
                 pr_debug("The userspace won't be notified that the device %s was added\n",
                          dev_name(&dev->dev));
 
+        dev->rfkill = rfkill_alloc(dev_name(&dev->dev), &dev->dev,
+                                   RFKILL_TYPE_NFC, &nfc_rfkill_ops, dev);
+        if (dev->rfkill) {
+                if (rfkill_register(dev->rfkill) < 0) {
+                        rfkill_destroy(dev->rfkill);
+                        dev->rfkill = NULL;
+                }
+        }
+
         return 0;
 }
 EXPORT_SYMBOL(nfc_register_device);
@@ -857,6 +890,11 @@ void nfc_unregister_device(struct nfc_dev *dev)
 
         id = dev->idx;
 
+        if (dev->rfkill) {
+                rfkill_unregister(dev->rfkill);
+                rfkill_destroy(dev->rfkill);
+        }
+
         if (dev->ops->check_presence) {
                 device_lock(&dev->dev);
                 dev->shutting_down = true;