2 files changed, 80 insertions, 19 deletions

diff --git a/drivers/amba/bus.c b/drivers/amba/bus.c
index f60b2b6a0931..d31590e7011b 100644
--- a/drivers/amba/bus.c
+++ b/drivers/amba/bus.c
@@ -122,6 +122,31 @@ static int __init amba_init(void)
 
 postcore_initcall(amba_init);
 
+static int amba_get_enable_pclk(struct amba_device *pcdev)
+{
+        struct clk *pclk = clk_get(&pcdev->dev, "apb_pclk");
+        int ret;
+
+        pcdev->pclk = pclk;
+
+        if (IS_ERR(pclk))
+                return PTR_ERR(pclk);
+
+        ret = clk_enable(pclk);
+        if (ret)
+                clk_put(pclk);
+
+        return ret;
+}
+
+static void amba_put_disable_pclk(struct amba_device *pcdev)
+{
+        struct clk *pclk = pcdev->pclk;
+
+        clk_disable(pclk);
+        clk_put(pclk);
+}
+
 /*
  * These are the device model conversion veneers; they convert the
  * device model structures to our more specific structures.
@@ -130,17 +155,33 @@ static int amba_probe(struct device *dev)
 {
         struct amba_device *pcdev = to_amba_device(dev);
         struct amba_driver *pcdrv = to_amba_driver(dev->driver);
-        struct amba_id *id;
+        struct amba_id *id = amba_lookup(pcdrv->id_table, pcdev);
+        int ret;
 
-        id = amba_lookup(pcdrv->id_table, pcdev);
+        do {
+                ret = amba_get_enable_pclk(pcdev);
+                if (ret)
+                        break;
+
+                ret = pcdrv->probe(pcdev, id);
+                if (ret == 0)
+                        break;
 
-        return pcdrv->probe(pcdev, id);
+                amba_put_disable_pclk(pcdev);
+        } while (0);
+
+        return ret;
 }
 
 static int amba_remove(struct device *dev)
 {
+        struct amba_device *pcdev = to_amba_device(dev);
         struct amba_driver *drv = to_amba_driver(dev->driver);
-        return drv->remove(to_amba_device(dev));
+        int ret = drv->remove(pcdev);
+
+        amba_put_disable_pclk(pcdev);
+
+        return ret;
 }
 
 static void amba_shutdown(struct device *dev)
@@ -203,7 +244,6 @@ static void amba_device_release(struct device *dev)
  */
 int amba_device_register(struct amba_device *dev, struct resource *parent)
 {
-        u32 pid, cid;
         u32 size;
         void __iomem *tmp;
         int i, ret;
@@ -241,25 +281,35 @@ int amba_device_register(struct amba_device *dev, struct resource *parent)
                 goto err_release;
         }
 
-        /*
-         * Read pid and cid based on size of resource
-         * they are located at end of region
-         */
-        for (pid = 0, i = 0; i < 4; i++)
-                pid |= (readl(tmp + size - 0x20 + 4 * i) & 255) << (i * 8);
-        for (cid = 0, i = 0; i < 4; i++)
-                cid |= (readl(tmp + size - 0x10 + 4 * i) & 255) << (i * 8);
+        ret = amba_get_enable_pclk(dev);
+        if (ret == 0) {
+                u32 pid, cid;
 
-        iounmap(tmp);
+                /*
+                 * Read pid and cid based on size of resource
+                 * they are located at end of region
+                 */
+                for (pid = 0, i = 0; i < 4; i++)
+                        pid |= (readl(tmp + size - 0x20 + 4 * i) & 255) <<
+                                (i * 8);
+                for (cid = 0, i = 0; i < 4; i++)
+                        cid |= (readl(tmp + size - 0x10 + 4 * i) & 255) <<
+                                (i * 8);
 
-        if (cid == 0xb105f00d)
-                dev->periphid = pid;
+                amba_put_disable_pclk(dev);
 
-        if (!dev->periphid) {
-                ret = -ENODEV;
-                goto err_release;
+                if (cid == 0xb105f00d)
+                        dev->periphid = pid;
+
+                if (!dev->periphid)
+                        ret = -ENODEV;
         }
 
+        iounmap(tmp);
+
+        if (ret)
+                goto err_release;
+
         ret = device_add(&dev->dev);
         if (ret)
                 goto err_release;
diff --git a/include/linux/amba/bus.h b/include/linux/amba/bus.h
index 8b1038607831..b0c174012436 100644
--- a/include/linux/amba/bus.h
+++ b/include/linux/amba/bus.h
@@ -14,14 +14,19 @@
 #ifndef ASMARM_AMBA_H
 #define ASMARM_AMBA_H
 
+#include <linux/clk.h>
 #include <linux/device.h>
+#include <linux/err.h>
 #include <linux/resource.h>
 
 #define AMBA_NR_IRQS    2
 
+struct clk;
+
 struct amba_device {
         struct device           dev;
         struct resource         res;
+        struct clk              *pclk;
         u64                     dma_mask;
         unsigned int            periphid;
         unsigned int            irq[AMBA_NR_IRQS];
@@ -59,6 +64,12 @@ struct amba_device *amba_find_device(const char *, struct device *, unsigned int
 int amba_request_regions(struct amba_device *, const char *);
 void amba_release_regions(struct amba_device *);
 
+#define amba_pclk_enable(d)     \
+        (IS_ERR((d)->pclk) ? 0 : clk_enable((d)->pclk))
+
+#define amba_pclk_disable(d)    \
+        do { if (!IS_ERR((d)->pclk)) clk_disable((d)->pclk); } while (0)
+
 #define amba_config(d)  (((d)->periphid >> 24) & 0xff)
 #define amba_rev(d)     (((d)->periphid >> 20) & 0x0f)
 #define amba_manf(d)    (((d)->periphid >> 12) & 0xff)