Merge tag 'ide-5.11-2021-02-28' of git://git.kernel.dk/linux-block

[linux-2.6-microblaze.git] / drivers / clocksource / sh_cmt.c

diff --git a/drivers/clocksource/sh_cmt.c b/drivers/clocksource/sh_cmt.c
index e258230..c98f885 100644 (file)
--- a/drivers/clocksource/sh_cmt.c
+++ b/drivers/clocksource/sh_cmt.c
@@ -235,6 +235,8 @@ static const struct sh_cmt_info sh_cmt_info[] = {
 #define CMCNT 1 /* channel register */
 #define CMCOR 2 /* channel register */
 
+#define CMCLKE 0x1000  /* CLK Enable Register (R-Car Gen2) */
+
 static inline u32 sh_cmt_read_cmstr(struct sh_cmt_channel *ch)
 {
        if (ch->iostart)
@@ -853,6 +855,7 @@ static int sh_cmt_setup_channel(struct sh_cmt_channel *ch, unsigned int index,
                                unsigned int hwidx, bool clockevent,
                                bool clocksource, struct sh_cmt_device *cmt)
 {
+       u32 value;
        int ret;
 
        /* Skip unused channels. */
@@ -882,6 +885,11 @@ static int sh_cmt_setup_channel(struct sh_cmt_channel *ch, unsigned int index,
                ch->iostart = cmt->mapbase + ch->hwidx * 0x100;
                ch->ioctrl = ch->iostart + 0x10;
                ch->timer_bit = 0;
+
+               /* Enable the clock supply to the channel */
+               value = ioread32(cmt->mapbase + CMCLKE);
+               value |= BIT(hwidx);
+               iowrite32(value, cmt->mapbase + CMCLKE);
                break;
        }
 
@@ -1014,12 +1022,10 @@ static int sh_cmt_setup(struct sh_cmt_device *cmt, struct platform_device *pdev)
        else
                cmt->rate = clk_get_rate(cmt->clk) / 8;
 
-       clk_disable(cmt->clk);
-
        /* Map the memory resource(s). */
        ret = sh_cmt_map_memory(cmt);
        if (ret < 0)
-               goto err_clk_unprepare;
+               goto err_clk_disable;
 
        /* Allocate and setup the channels. */
        cmt->num_channels = hweight8(cmt->hw_channels);
@@ -1047,6 +1053,8 @@ static int sh_cmt_setup(struct sh_cmt_device *cmt, struct platform_device *pdev)
                mask &= ~(1 << hwidx);
        }
 
+       clk_disable(cmt->clk);
+
        platform_set_drvdata(pdev, cmt);
 
        return 0;
@@ -1054,6 +1062,8 @@ static int sh_cmt_setup(struct sh_cmt_device *cmt, struct platform_device *pdev)
 err_unmap:
        kfree(cmt->channels);
        iounmap(cmt->mapbase);
+err_clk_disable:
+       clk_disable(cmt->clk);
 err_clk_unprepare:
        clk_unprepare(cmt->clk);
 err_clk_put: