NFC: RFKILL support

[karo-tx-linux.git] / net / nfc / core.c

diff --git a/net/nfc/core.c b/net/nfc/core.c
index c571ca9a960cfe29b54d5bcac9d8fe524fb33cdd..40d2527693da8170b4edd9d0d50837255387f1eb 100644 (file)
--- 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;