4 files changed, 132 insertions, 74 deletions

diff --git a/drivers/clocksource/em_sti.c b/drivers/clocksource/em_sti.c
index e6a553cb73e8..4329a29a5310 100644
--- a/drivers/clocksource/em_sti.c
+++ b/drivers/clocksource/em_sti.c
@@ -399,7 +399,18 @@ static struct platform_driver em_sti_device_driver = {
         }
 };
 
-module_platform_driver(em_sti_device_driver);
+static int __init em_sti_init(void)
+{
+        return platform_driver_register(&em_sti_device_driver);
+}
+
+static void __exit em_sti_exit(void)
+{
+        platform_driver_unregister(&em_sti_device_driver);
+}
+
+subsys_initcall(em_sti_init);
+module_exit(em_sti_exit);
 
 MODULE_AUTHOR("Magnus Damm");
 MODULE_DESCRIPTION("Renesas Emma Mobile STI Timer Driver");
diff --git a/drivers/clocksource/sh_cmt.c b/drivers/clocksource/sh_cmt.c
index 488c14cc8dbf..08d0c418c94a 100644
--- a/drivers/clocksource/sh_cmt.c
+++ b/drivers/clocksource/sh_cmt.c
@@ -54,62 +54,100 @@ struct sh_cmt_priv {
         struct clocksource cs;
         unsigned long total_cycles;
         bool cs_enabled;
+
+        /* callbacks for CMSTR and CMCSR access */
+        unsigned long (*read_control)(void __iomem *base, unsigned long offs);
+        void (*write_control)(void __iomem *base, unsigned long offs,
+                              unsigned long value);
+
+        /* callbacks for CMCNT and CMCOR access */
+        unsigned long (*read_count)(void __iomem *base, unsigned long offs);
+        void (*write_count)(void __iomem *base, unsigned long offs,
+                            unsigned long value);
 };
 
-static DEFINE_RAW_SPINLOCK(sh_cmt_lock);
+/* Examples of supported CMT timer register layouts and I/O access widths:
+ *
+ * "16-bit counter and 16-bit control" as found on sh7263:
+ * CMSTR 0xfffec000 16-bit
+ * CMCSR 0xfffec002 16-bit
+ * CMCNT 0xfffec004 16-bit
+ * CMCOR 0xfffec006 16-bit
+ *
+ * "32-bit counter and 16-bit control" as found on sh7372, sh73a0, r8a7740:
+ * CMSTR 0xffca0000 16-bit
+ * CMCSR 0xffca0060 16-bit
+ * CMCNT 0xffca0064 32-bit
+ * CMCOR 0xffca0068 32-bit
+ */
+
+static unsigned long sh_cmt_read16(void __iomem *base, unsigned long offs)
+{
+        return ioread16(base + (offs << 1));
+}
+
+static unsigned long sh_cmt_read32(void __iomem *base, unsigned long offs)
+{
+        return ioread32(base + (offs << 2));
+}
+
+static void sh_cmt_write16(void __iomem *base, unsigned long offs,
+                           unsigned long value)
+{
+        iowrite16(value, base + (offs << 1));
+}
+
+static void sh_cmt_write32(void __iomem *base, unsigned long offs,
+                           unsigned long value)
+{
+        iowrite32(value, base + (offs << 2));
+}
 
-#define CMSTR -1 /* shared register */
 #define CMCSR 0 /* channel register */
 #define CMCNT 1 /* channel register */
 #define CMCOR 2 /* channel register */
 
-static inline unsigned long sh_cmt_read(struct sh_cmt_priv *p, int reg_nr)
+static inline unsigned long sh_cmt_read_cmstr(struct sh_cmt_priv *p)
 {
         struct sh_timer_config *cfg = p->pdev->dev.platform_data;
-        void __iomem *base = p->mapbase;
-        unsigned long offs;
-
-        if (reg_nr == CMSTR) {
-                offs = 0;
-                base -= cfg->channel_offset;
-        } else
-                offs = reg_nr;
-
-        if (p->width == 16)
-                offs <<= 1;
-        else {
-                offs <<= 2;
-                if ((reg_nr == CMCNT) || (reg_nr == CMCOR))
-                        return ioread32(base + offs);
-        }
 
-        return ioread16(base + offs);
+        return p->read_control(p->mapbase - cfg->channel_offset, 0);
 }
 
-static inline void sh_cmt_write(struct sh_cmt_priv *p, int reg_nr,
-                                unsigned long value)
+static inline unsigned long sh_cmt_read_cmcsr(struct sh_cmt_priv *p)
+{
+        return p->read_control(p->mapbase, CMCSR);
+}
+
+static inline unsigned long sh_cmt_read_cmcnt(struct sh_cmt_priv *p)
+{
+        return p->read_count(p->mapbase, CMCNT);
+}
+
+static inline void sh_cmt_write_cmstr(struct sh_cmt_priv *p,
+                                      unsigned long value)
 {
         struct sh_timer_config *cfg = p->pdev->dev.platform_data;
-        void __iomem *base = p->mapbase;
-        unsigned long offs;
-
-        if (reg_nr == CMSTR) {
-                offs = 0;
-                base -= cfg->channel_offset;
-        } else
-                offs = reg_nr;
-
-        if (p->width == 16)
-                offs <<= 1;
-        else {
-                offs <<= 2;
-                if ((reg_nr == CMCNT) || (reg_nr == CMCOR)) {
-                        iowrite32(value, base + offs);
-                        return;
-                }
-        }
 
-        iowrite16(value, base + offs);
+        p->write_control(p->mapbase - cfg->channel_offset, 0, value);
+}
+
+static inline void sh_cmt_write_cmcsr(struct sh_cmt_priv *p,
+                                      unsigned long value)
+{
+        p->write_control(p->mapbase, CMCSR, value);
+}
+
+static inline void sh_cmt_write_cmcnt(struct sh_cmt_priv *p,
+                                      unsigned long value)
+{
+        p->write_count(p->mapbase, CMCNT, value);
+}
+
+static inline void sh_cmt_write_cmcor(struct sh_cmt_priv *p,
+                                      unsigned long value)
+{
+        p->write_count(p->mapbase, CMCOR, value);
 }
 
 static unsigned long sh_cmt_get_counter(struct sh_cmt_priv *p,
@@ -118,15 +156,15 @@ static unsigned long sh_cmt_get_counter(struct sh_cmt_priv *p,
         unsigned long v1, v2, v3;
         int o1, o2;
 
-        o1 = sh_cmt_read(p, CMCSR) & p->overflow_bit;
+        o1 = sh_cmt_read_cmcsr(p) & p->overflow_bit;
 
         /* Make sure the timer value is stable. Stolen from acpi_pm.c */
         do {
                 o2 = o1;
-                v1 = sh_cmt_read(p, CMCNT);
-                v2 = sh_cmt_read(p, CMCNT);
-                v3 = sh_cmt_read(p, CMCNT);
-                o1 = sh_cmt_read(p, CMCSR) & p->overflow_bit;
+                v1 = sh_cmt_read_cmcnt(p);
+                v2 = sh_cmt_read_cmcnt(p);
+                v3 = sh_cmt_read_cmcnt(p);
+                o1 = sh_cmt_read_cmcsr(p) & p->overflow_bit;
         } while (unlikely((o1 != o2) || (v1 > v2 && v1 < v3)
                           || (v2 > v3 && v2 < v1) || (v3 > v1 && v3 < v2)));
 
@@ -134,6 +172,7 @@ static unsigned long sh_cmt_get_counter(struct sh_cmt_priv *p,
         return v2;
 }
 
+static DEFINE_RAW_SPINLOCK(sh_cmt_lock);
 
 static void sh_cmt_start_stop_ch(struct sh_cmt_priv *p, int start)
 {
@@ -142,14 +181,14 @@ static void sh_cmt_start_stop_ch(struct sh_cmt_priv *p, int start)
 
         /* start stop register shared by multiple timer channels */
         raw_spin_lock_irqsave(&sh_cmt_lock, flags);
-        value = sh_cmt_read(p, CMSTR);
+        value = sh_cmt_read_cmstr(p);
 
         if (start)
                 value |= 1 << cfg->timer_bit;
         else
                 value &= ~(1 << cfg->timer_bit);
 
-        sh_cmt_write(p, CMSTR, value);
+        sh_cmt_write_cmstr(p, value);
         raw_spin_unlock_irqrestore(&sh_cmt_lock, flags);
 }
 
@@ -173,14 +212,14 @@ static int sh_cmt_enable(struct sh_cmt_priv *p, unsigned long *rate)
         /* configure channel, periodic mode and maximum timeout */
         if (p->width == 16) {
                 *rate = clk_get_rate(p->clk) / 512;
-                sh_cmt_write(p, CMCSR, 0x43);
+                sh_cmt_write_cmcsr(p, 0x43);
         } else {
                 *rate = clk_get_rate(p->clk) / 8;
-                sh_cmt_write(p, CMCSR, 0x01a4);
+                sh_cmt_write_cmcsr(p, 0x01a4);
         }
 
-        sh_cmt_write(p, CMCOR, 0xffffffff);
-        sh_cmt_write(p, CMCNT, 0);
+        sh_cmt_write_cmcor(p, 0xffffffff);
+        sh_cmt_write_cmcnt(p, 0);
 
         /*
          * According to the sh73a0 user's manual, as CMCNT can be operated
@@ -194,12 +233,12 @@ static int sh_cmt_enable(struct sh_cmt_priv *p, unsigned long *rate)
          * take RCLKx2 at maximum.
          */
         for (k = 0; k < 100; k++) {
-                if (!sh_cmt_read(p, CMCNT))
+                if (!sh_cmt_read_cmcnt(p))
                         break;
                 udelay(1);
         }
 
-        if (sh_cmt_read(p, CMCNT)) {
+        if (sh_cmt_read_cmcnt(p)) {
                 dev_err(&p->pdev->dev, "cannot clear CMCNT\n");
                 ret = -ETIMEDOUT;
                 goto err1;
@@ -222,7 +261,7 @@ static void sh_cmt_disable(struct sh_cmt_priv *p)
         sh_cmt_start_stop_ch(p, 0);
 
         /* disable interrupts in CMT block */
-        sh_cmt_write(p, CMCSR, 0);
+        sh_cmt_write_cmcsr(p, 0);
 
         /* stop clock */
         clk_disable(p->clk);
@@ -270,7 +309,7 @@ static void sh_cmt_clock_event_program_verify(struct sh_cmt_priv *p,
                 if (new_match > p->max_match_value)
                         new_match = p->max_match_value;
 
-                sh_cmt_write(p, CMCOR, new_match);
+                sh_cmt_write_cmcor(p, new_match);
 
                 now = sh_cmt_get_counter(p, &has_wrapped);
                 if (has_wrapped && (new_match > p->match_value)) {
@@ -346,7 +385,7 @@ static irqreturn_t sh_cmt_interrupt(int irq, void *dev_id)
         struct sh_cmt_priv *p = dev_id;
 
         /* clear flags */
-        sh_cmt_write(p, CMCSR, sh_cmt_read(p, CMCSR) & p->clear_bits);
+        sh_cmt_write_cmcsr(p, sh_cmt_read_cmcsr(p) & p->clear_bits);
 
         /* update clock source counter to begin with if enabled
          * the wrap flag should be cleared by the timer specific
@@ -625,14 +664,6 @@ static int sh_cmt_register(struct sh_cmt_priv *p, char *name,
                            unsigned long clockevent_rating,
                            unsigned long clocksource_rating)
 {
-        if (p->width == (sizeof(p->max_match_value) * 8))
-                p->max_match_value = ~0;
-        else
-                p->max_match_value = (1 << p->width) - 1;
-
-        p->match_value = p->max_match_value;
-        raw_spin_lock_init(&p->lock);
-
         if (clockevent_rating)
                 sh_cmt_register_clockevent(p, name, clockevent_rating);
 
@@ -657,8 +688,6 @@ static int sh_cmt_setup(struct sh_cmt_priv *p, struct platform_device *pdev)
                 goto err0;
         }
 
-        platform_set_drvdata(pdev, p);
-
         res = platform_get_resource(p->pdev, IORESOURCE_MEM, 0);
         if (!res) {
                 dev_err(&p->pdev->dev, "failed to get I/O memory\n");
@@ -693,32 +722,51 @@ static int sh_cmt_setup(struct sh_cmt_priv *p, struct platform_device *pdev)
                 goto err1;
         }
 
+        p->read_control = sh_cmt_read16;
+        p->write_control = sh_cmt_write16;
+
         if (resource_size(res) == 6) {
                 p->width = 16;
+                p->read_count = sh_cmt_read16;
+                p->write_count = sh_cmt_write16;
                 p->overflow_bit = 0x80;
                 p->clear_bits = ~0x80;
         } else {
                 p->width = 32;
+                p->read_count = sh_cmt_read32;
+                p->write_count = sh_cmt_write32;
                 p->overflow_bit = 0x8000;
                 p->clear_bits = ~0xc000;
         }
 
+        if (p->width == (sizeof(p->max_match_value) * 8))
+                p->max_match_value = ~0;
+        else
+                p->max_match_value = (1 << p->width) - 1;
+
+        p->match_value = p->max_match_value;
+        raw_spin_lock_init(&p->lock);
+
         ret = sh_cmt_register(p, (char *)dev_name(&p->pdev->dev),
                               cfg->clockevent_rating,
                               cfg->clocksource_rating);
         if (ret) {
                 dev_err(&p->pdev->dev, "registration failed\n");
-                goto err1;
+                goto err2;
         }
         p->cs_enabled = false;
 
         ret = setup_irq(irq, &p->irqaction);
         if (ret) {
                 dev_err(&p->pdev->dev, "failed to request irq %d\n", irq);
-                goto err1;
+                goto err2;
         }
 
+        platform_set_drvdata(pdev, p);
+
         return 0;
+err2:
+        clk_put(p->clk);
 
 err1:
         iounmap(p->mapbase);
@@ -751,7 +799,6 @@ static int sh_cmt_probe(struct platform_device *pdev)
         ret = sh_cmt_setup(p, pdev);
         if (ret) {
                 kfree(p);
-                platform_set_drvdata(pdev, NULL);
                 pm_runtime_idle(&pdev->dev);
                 return ret;
         }
@@ -791,7 +838,7 @@ static void __exit sh_cmt_exit(void)
 }
 
 early_platform_init("earlytimer", &sh_cmt_device_driver);
-module_init(sh_cmt_init);
+subsys_initcall(sh_cmt_init);
 module_exit(sh_cmt_exit);
 
 MODULE_AUTHOR("Magnus Damm");
diff --git a/drivers/clocksource/sh_mtu2.c b/drivers/clocksource/sh_mtu2.c
index 83943e27cfac..4aac9ee0d0c0 100644
--- a/drivers/clocksource/sh_mtu2.c
+++ b/drivers/clocksource/sh_mtu2.c
@@ -386,7 +386,7 @@ static void __exit sh_mtu2_exit(void)
 }
 
 early_platform_init("earlytimer", &sh_mtu2_device_driver);
-module_init(sh_mtu2_init);
+subsys_initcall(sh_mtu2_init);
 module_exit(sh_mtu2_exit);
 
 MODULE_AUTHOR("Magnus Damm");
diff --git a/drivers/clocksource/sh_tmu.c b/drivers/clocksource/sh_tmu.c
index b4502edce2a1..78b8dae49628 100644
--- a/drivers/clocksource/sh_tmu.c
+++ b/drivers/clocksource/sh_tmu.c
@@ -549,7 +549,7 @@ static void __exit sh_tmu_exit(void)
 }
 
 early_platform_init("earlytimer", &sh_tmu_device_driver);
-module_init(sh_tmu_init);
+subsys_initcall(sh_tmu_init);
 module_exit(sh_tmu_exit);
 
 MODULE_AUTHOR("Magnus Damm");