1 files changed, 93 insertions, 0 deletions

diff --git a/drivers/mmc/core/sdio_io.c b/drivers/mmc/core/sdio_io.c
index 4ad06e575634..eb6c20935cef 100644
--- a/drivers/mmc/core/sdio_io.c
+++ b/drivers/mmc/core/sdio_io.c
@@ -11,6 +11,7 @@
 
 #include <linux/mmc/host.h>
 #include <linux/mmc/card.h>
+#include <linux/mmc/sdio.h>
 #include <linux/mmc/sdio_func.h>
 
 #include "sdio_ops.h"
@@ -48,6 +49,98 @@ void sdio_release_host(struct sdio_func *func)
 EXPORT_SYMBOL_GPL(sdio_release_host);
 
 /**
+ *      sdio_enable_func - enables a SDIO function for usage
+ *      @func: SDIO function to enable
+ *
+ *      Powers up and activates a SDIO function so that register
+ *      access is possible.
+ */
+int sdio_enable_func(struct sdio_func *func)
+{
+        int ret;
+        unsigned char reg;
+        unsigned long timeout;
+
+        BUG_ON(!func);
+        BUG_ON(!func->card);
+
+        pr_debug("SDIO: Enabling device %s...\n", sdio_func_id(func));
+
+        ret = mmc_io_rw_direct(func->card, 0, 0, SDIO_CCCR_IOEx, 0, &reg);
+        if (ret)
+                goto err;
+
+        reg |= 1 << func->num;
+
+        ret = mmc_io_rw_direct(func->card, 1, 0, SDIO_CCCR_IOEx, reg, NULL);
+        if (ret)
+                goto err;
+
+        /*
+         * FIXME: This should timeout based on information in the CIS,
+         * but we don't have card to parse that yet.
+         */
+        timeout = jiffies + HZ;
+
+        while (1) {
+                ret = mmc_io_rw_direct(func->card, 0, 0, SDIO_CCCR_IORx, 0, &reg);
+                if (ret)
+                        goto err;
+                if (reg & (1 << func->num))
+                        break;
+                ret = -ETIME;
+                if (time_after(jiffies, timeout))
+                        goto err;
+        }
+
+        pr_debug("SDIO: Enabled device %s\n", sdio_func_id(func));
+
+        return 0;
+
+err:
+        pr_debug("SDIO: Failed to enable device %s\n", sdio_func_id(func));
+        return ret;
+}
+EXPORT_SYMBOL_GPL(sdio_enable_func);
+
+/**
+ *      sdio_disable_func - disable a SDIO function
+ *      @func: SDIO function to disable
+ *
+ *      Powers down and deactivates a SDIO function. Register access
+ *      to this function will fail until the function is reenabled.
+ */
+int sdio_disable_func(struct sdio_func *func)
+{
+        int ret;
+        unsigned char reg;
+
+        BUG_ON(!func);
+        BUG_ON(!func->card);
+
+        pr_debug("SDIO: Disabling device %s...\n", sdio_func_id(func));
+
+        ret = mmc_io_rw_direct(func->card, 0, 0, SDIO_CCCR_IOEx, 0, &reg);
+        if (ret)
+                goto err;
+
+        reg &= ~(1 << func->num);
+
+        ret = mmc_io_rw_direct(func->card, 1, 0, SDIO_CCCR_IOEx, reg, NULL);
+        if (ret)
+                goto err;
+
+        pr_debug("SDIO: Disabled device %s\n", sdio_func_id(func));
+
+        return 0;
+
+err:
+        pr_debug("SDIO: Failed to disable device %s\n", sdio_func_id(func));
+        return -EIO;
+}
+EXPORT_SYMBOL_GPL(sdio_disable_func);
+
+/**
  *      sdio_readb - read a single byte from a SDIO function
  *      @func: SDIO function to access
  *      @addr: address to read