2 files changed, 23 insertions, 7 deletions

diff --git a/arch/arm/common/sa1111.c b/arch/arm/common/sa1111.c
index b0f93628dcd7..1366e82e6707 100644
--- a/arch/arm/common/sa1111.c
+++ b/arch/arm/common/sa1111.c
@@ -106,6 +106,7 @@ struct sa1111 {
         int             irq_base;       /* base for cascaded on-chip IRQs */
         spinlock_t      lock;
         void __iomem    *base;
+        struct sa1111_platform_data *pdata;
 #ifdef CONFIG_PM
         void            *saved_state;
 #endif
@@ -756,6 +757,7 @@ __sa1111_probe(struct device *me, struct resource *mem, int irq)
         sachip->dev = me;
         dev_set_drvdata(sachip->dev, sachip);
 
+        sachip->pdata = pd;
         sachip->phys = mem->start;
         sachip->irq = irq;
 
@@ -1282,16 +1284,23 @@ EXPORT_SYMBOL(sa1111_set_sleep_io);
  *      sa1111_enable_device - enable an on-chip SA1111 function block
  *      @sadev: SA1111 function block device to enable
  */
-void sa1111_enable_device(struct sa1111_dev *sadev)
+int sa1111_enable_device(struct sa1111_dev *sadev)
 {
         struct sa1111 *sachip = sa1111_chip_driver(sadev);
         unsigned long flags;
         unsigned int val;
+        int ret = 0;
 
-        spin_lock_irqsave(&sachip->lock, flags);
-        val = sa1111_readl(sachip->base + SA1111_SKPCR);
-        sa1111_writel(val | sadev->skpcr_mask, sachip->base + SA1111_SKPCR);
-        spin_unlock_irqrestore(&sachip->lock, flags);
+        if (sachip->pdata && sachip->pdata->enable)
+                ret = sachip->pdata->enable(sachip->pdata->data, sadev->devid);
+
+        if (ret == 0) {
+                spin_lock_irqsave(&sachip->lock, flags);
+                val = sa1111_readl(sachip->base + SA1111_SKPCR);
+                sa1111_writel(val | sadev->skpcr_mask, sachip->base + SA1111_SKPCR);
+                spin_unlock_irqrestore(&sachip->lock, flags);
+        }
+        return ret;
 }
 EXPORT_SYMBOL(sa1111_enable_device);
 
@@ -1309,6 +1318,9 @@ void sa1111_disable_device(struct sa1111_dev *sadev)
         val = sa1111_readl(sachip->base + SA1111_SKPCR);
         sa1111_writel(val & ~sadev->skpcr_mask, sachip->base + SA1111_SKPCR);
         spin_unlock_irqrestore(&sachip->lock, flags);
+
+        if (sachip->pdata && sachip->pdata->disable)
+                sachip->pdata->disable(sachip->pdata->data, sadev->devid);
 }
 EXPORT_SYMBOL(sa1111_disable_device);
 
diff --git a/arch/arm/include/asm/hardware/sa1111.h b/arch/arm/include/asm/hardware/sa1111.h
index 29e320f6f85f..d54d781021c8 100644
--- a/arch/arm/include/asm/hardware/sa1111.h
+++ b/arch/arm/include/asm/hardware/sa1111.h
@@ -556,9 +556,10 @@ struct sa1111_driver {
 #define SA1111_DRIVER_NAME(_sadev) ((_sadev)->dev.driver->name)
 
 /*
- * These frob the SKPCR register.
+ * These frob the SKPCR register, and call platform specific
+ * enable/disable functions.
  */
-void sa1111_enable_device(struct sa1111_dev *);
+int sa1111_enable_device(struct sa1111_dev *);
 void sa1111_disable_device(struct sa1111_dev *);
 
 unsigned int sa1111_pll_clock(struct sa1111_dev *);
@@ -581,6 +582,9 @@ void sa1111_set_sleep_io(struct sa1111_dev *sadev, unsigned int bits, unsigned i
 
 struct sa1111_platform_data {
         int     irq_base;       /* base for cascaded on-chip IRQs */
+        void    *data;
+        int     (*enable)(void *, unsigned);
+        void    (*disable)(void *, unsigned);
 };
 
 #endif  /* _ASM_ARCH_SA1111 */