diff --git a/drivers/clocksource/timer-microchip-pit64b.c b/drivers/clocksource/timer-microchip-pit64b.c index 59e11ca8ee73..ab623b25a47b 100644 --- a/drivers/clocksource/timer-microchip-pit64b.c +++ b/drivers/clocksource/timer-microchip-pit64b.c @@ -71,10 +71,24 @@ struct mchp_pit64b_clkevt { struct clock_event_device clkevt; }; -#define to_mchp_pit64b_timer(x) \ +#define clkevt_to_mchp_pit64b_timer(x) \ ((struct mchp_pit64b_timer *)container_of(x,\ struct mchp_pit64b_clkevt, clkevt)) +/** + * mchp_pit64b_clksrc - PIT64B clocksource data structure + * @timer: PIT64B timer + * @clksrc: clocksource + */ +struct mchp_pit64b_clksrc { + struct mchp_pit64b_timer timer; + struct clocksource clksrc; +}; + +#define clksrc_to_mchp_pit64b_timer(x) \ + ((struct mchp_pit64b_timer *)container_of(x,\ + struct mchp_pit64b_clksrc, clksrc)) + /* Base address for clocksource timer. */ static void __iomem *mchp_pit64b_cs_base; /* Default cycles for clockevent timer. */ @@ -116,6 +130,36 @@ static inline void mchp_pit64b_reset(struct mchp_pit64b_timer *timer, writel_relaxed(MCHP_PIT64B_CR_START, timer->base + MCHP_PIT64B_CR); } +static void mchp_pit64b_suspend(struct mchp_pit64b_timer *timer) +{ + writel_relaxed(MCHP_PIT64B_CR_SWRST, timer->base + MCHP_PIT64B_CR); + if (timer->mode & MCHP_PIT64B_MR_SGCLK) + clk_disable_unprepare(timer->gclk); + clk_disable_unprepare(timer->pclk); +} + +static void mchp_pit64b_resume(struct mchp_pit64b_timer *timer) +{ + clk_prepare_enable(timer->pclk); + if (timer->mode & MCHP_PIT64B_MR_SGCLK) + clk_prepare_enable(timer->gclk); +} + +static void mchp_pit64b_clksrc_suspend(struct clocksource *cs) +{ + struct mchp_pit64b_timer *timer = clksrc_to_mchp_pit64b_timer(cs); + + mchp_pit64b_suspend(timer); +} + +static void mchp_pit64b_clksrc_resume(struct clocksource *cs) +{ + struct mchp_pit64b_timer *timer = clksrc_to_mchp_pit64b_timer(cs); + + mchp_pit64b_resume(timer); + mchp_pit64b_reset(timer, ULLONG_MAX, MCHP_PIT64B_MR_CONT, 0); +} + static u64 mchp_pit64b_clksrc_read(struct clocksource *cs) { return mchp_pit64b_cnt_read(mchp_pit64b_cs_base); @@ -128,7 +172,7 @@ static u64 mchp_pit64b_sched_read_clk(void) static int mchp_pit64b_clkevt_shutdown(struct clock_event_device *cedev) { - struct mchp_pit64b_timer *timer = to_mchp_pit64b_timer(cedev); + struct mchp_pit64b_timer *timer = clkevt_to_mchp_pit64b_timer(cedev); writel_relaxed(MCHP_PIT64B_CR_SWRST, timer->base + MCHP_PIT64B_CR); @@ -137,7 +181,7 @@ static int mchp_pit64b_clkevt_shutdown(struct clock_event_device *cedev) static int mchp_pit64b_clkevt_set_periodic(struct clock_event_device *cedev) { - struct mchp_pit64b_timer *timer = to_mchp_pit64b_timer(cedev); + struct mchp_pit64b_timer *timer = clkevt_to_mchp_pit64b_timer(cedev); mchp_pit64b_reset(timer, mchp_pit64b_ce_cycles, MCHP_PIT64B_MR_CONT, MCHP_PIT64B_IER_PERIOD); @@ -148,7 +192,7 @@ static int mchp_pit64b_clkevt_set_periodic(struct clock_event_device *cedev) static int mchp_pit64b_clkevt_set_next_event(unsigned long evt, struct clock_event_device *cedev) { - struct mchp_pit64b_timer *timer = to_mchp_pit64b_timer(cedev); + struct mchp_pit64b_timer *timer = clkevt_to_mchp_pit64b_timer(cedev); mchp_pit64b_reset(timer, evt, MCHP_PIT64B_MR_ONE_SHOT, MCHP_PIT64B_IER_PERIOD); @@ -158,21 +202,16 @@ static int mchp_pit64b_clkevt_set_next_event(unsigned long evt, static void mchp_pit64b_clkevt_suspend(struct clock_event_device *cedev) { - struct mchp_pit64b_timer *timer = to_mchp_pit64b_timer(cedev); + struct mchp_pit64b_timer *timer = clkevt_to_mchp_pit64b_timer(cedev); - writel_relaxed(MCHP_PIT64B_CR_SWRST, timer->base + MCHP_PIT64B_CR); - if (timer->mode & MCHP_PIT64B_MR_SGCLK) - clk_disable_unprepare(timer->gclk); - clk_disable_unprepare(timer->pclk); + mchp_pit64b_suspend(timer); } static void mchp_pit64b_clkevt_resume(struct clock_event_device *cedev) { - struct mchp_pit64b_timer *timer = to_mchp_pit64b_timer(cedev); + struct mchp_pit64b_timer *timer = clkevt_to_mchp_pit64b_timer(cedev); - clk_prepare_enable(timer->pclk); - if (timer->mode & MCHP_PIT64B_MR_SGCLK) - clk_prepare_enable(timer->gclk); + mchp_pit64b_resume(timer); } static irqreturn_t mchp_pit64b_interrupt(int irq, void *dev_id) @@ -296,20 +335,37 @@ done: static int __init mchp_pit64b_init_clksrc(struct mchp_pit64b_timer *timer, u32 clk_rate) { + struct mchp_pit64b_clksrc *cs; int ret; + cs = kzalloc(sizeof(*cs), GFP_KERNEL); + if (!cs) + return -ENOMEM; + mchp_pit64b_reset(timer, ULLONG_MAX, MCHP_PIT64B_MR_CONT, 0); mchp_pit64b_cs_base = timer->base; - ret = clocksource_mmio_init(timer->base, MCHP_PIT64B_NAME, clk_rate, - 210, 64, mchp_pit64b_clksrc_read); + cs->timer.base = timer->base; + cs->timer.pclk = timer->pclk; + cs->timer.gclk = timer->gclk; + cs->timer.mode = timer->mode; + cs->clksrc.name = MCHP_PIT64B_NAME; + cs->clksrc.mask = CLOCKSOURCE_MASK(64); + cs->clksrc.flags = CLOCK_SOURCE_IS_CONTINUOUS; + cs->clksrc.rating = 210; + cs->clksrc.read = mchp_pit64b_clksrc_read; + cs->clksrc.suspend = mchp_pit64b_clksrc_suspend; + cs->clksrc.resume = mchp_pit64b_clksrc_resume; + + ret = clocksource_register_hz(&cs->clksrc, clk_rate); if (ret) { pr_debug("clksrc: Failed to register PIT64B clocksource!\n"); /* Stop timer. */ writel_relaxed(MCHP_PIT64B_CR_SWRST, timer->base + MCHP_PIT64B_CR); + kfree(cs); return ret; }