@@ -16,175 +16,226 @@
#include <linux/of_irq.h>
#include <linux/clk.h>
#include <linux/reset.h>
+#include <linux/sched_clock.h>
+#include <linux/slab.h>
#define TIM_CR1 0x00
#define TIM_DIER 0x0c
#define TIM_SR 0x10
#define TIM_EGR 0x14
+#define TIM_CNT 0x24
#define TIM_PSC 0x28
#define TIM_ARR 0x2c
+#define TIM_CCR1 0x34
#define TIM_CR1_CEN BIT(0)
-#define TIM_CR1_OPM BIT(3)
+#define TIM_CR1_UDIS BIT(1)
#define TIM_CR1_ARPE BIT(7)
-#define TIM_DIER_UIE BIT(0)
-
-#define TIM_SR_UIF BIT(0)
+#define TIM_DIER_CC1IE BIT(1)
#define TIM_EGR_UG BIT(0)
-struct stm32_clock_event_ddata {
+struct stm32_clock_event {
struct clock_event_device evtdev;
unsigned periodic_top;
- void __iomem *base;
+ void __iomem *regs;
};
static int stm32_clock_event_shutdown(struct clock_event_device *evtdev)
{
- struct stm32_clock_event_ddata *data =
- container_of(evtdev, struct stm32_clock_event_ddata, evtdev);
- void *base = data->base;
+ struct stm32_clock_event *ce =
+ container_of(evtdev, struct stm32_clock_event, evtdev);
+
+ writel_relaxed(0, ce->regs + TIM_DIER);
- writel_relaxed(0, base + TIM_CR1);
return 0;
}
-static int stm32_clock_event_set_periodic(struct clock_event_device *evtdev)
+static int stm32_clock_event_set_next_event(unsigned long evt,
+ struct clock_event_device *evtdev)
{
- struct stm32_clock_event_ddata *data =
- container_of(evtdev, struct stm32_clock_event_ddata, evtdev);
- void *base = data->base;
+ struct stm32_clock_event *ce =
+ container_of(evtdev, struct stm32_clock_event, evtdev);
+ unsigned long cnt;
+
+ cnt = readl_relaxed(ce->regs + TIM_CNT);
+ writel_relaxed(cnt + evt, ce->regs + TIM_CCR1);
+ writel_relaxed(TIM_DIER_CC1IE, ce->regs + TIM_DIER);
- writel_relaxed(data->periodic_top, base + TIM_ARR);
- writel_relaxed(TIM_CR1_ARPE | TIM_CR1_CEN, base + TIM_CR1);
return 0;
}
-static int stm32_clock_event_set_next_event(unsigned long evt,
- struct clock_event_device *evtdev)
+static int stm32_clock_event_set_periodic(struct clock_event_device *evtdev)
{
- struct stm32_clock_event_ddata *data =
- container_of(evtdev, struct stm32_clock_event_ddata, evtdev);
+ struct stm32_clock_event *ce =
+ container_of(evtdev, struct stm32_clock_event, evtdev);
- writel_relaxed(evt, data->base + TIM_ARR);
- writel_relaxed(TIM_CR1_ARPE | TIM_CR1_OPM | TIM_CR1_CEN,
- data->base + TIM_CR1);
+ return stm32_clock_event_set_next_event(ce->periodic_top, evtdev);
+}
- return 0;
+static int stm32_clock_event_set_oneshot(struct clock_event_device *evtdev)
+{
+ return stm32_clock_event_set_next_event(0, evtdev);
}
static irqreturn_t stm32_clock_event_handler(int irq, void *dev_id)
{
- struct stm32_clock_event_ddata *data = dev_id;
+ struct stm32_clock_event *ce = dev_id;
+
+ writel_relaxed(0, ce->regs + TIM_SR);
- writel_relaxed(0, data->base + TIM_SR);
+ if (clockevent_state_periodic(&ce->evtdev))
+ stm32_clock_event_set_periodic(&ce->evtdev);
- data->evtdev.event_handler(&data->evtdev);
+ if (clockevent_state_oneshot(&ce->evtdev))
+ stm32_clock_event_shutdown(&ce->evtdev);
+
+ ce->evtdev.event_handler(&ce->evtdev);
return IRQ_HANDLED;
}
-static struct stm32_clock_event_ddata clock_event_ddata = {
- .evtdev = {
- .name = "stm32 clockevent",
- .features = CLOCK_EVT_FEAT_ONESHOT | CLOCK_EVT_FEAT_PERIODIC,
- .set_state_shutdown = stm32_clock_event_shutdown,
- .set_state_periodic = stm32_clock_event_set_periodic,
- .set_state_oneshot = stm32_clock_event_shutdown,
- .tick_resume = stm32_clock_event_shutdown,
- .set_next_event = stm32_clock_event_set_next_event,
- .rating = 200,
- },
-};
+static int __init stm32_clockevent_init(struct device_node *np,
+ void __iomem *base,
+ struct clk *clk, int irq)
+{
+ struct stm32_clock_event *ce;
+ unsigned long rate;
+ int err;
+
+ ce = kzalloc(sizeof(*ce), GFP_KERNEL);
+ if (!ce)
+ return -ENOMEM;
+
+ ce->regs = base;
+ ce->evtdev.name = "stm32_clockevent";
+ ce->evtdev.features = CLOCK_EVT_FEAT_ONESHOT | CLOCK_EVT_FEAT_PERIODIC;
+ ce->evtdev.set_state_shutdown = stm32_clock_event_shutdown;
+ ce->evtdev.set_state_periodic = stm32_clock_event_set_periodic;
+ ce->evtdev.set_state_oneshot = stm32_clock_event_set_oneshot;
+ ce->evtdev.tick_resume = stm32_clock_event_shutdown;
+ ce->evtdev.set_next_event = stm32_clock_event_set_next_event;
+ ce->evtdev.rating = 200;
-static int __init stm32_clockevent_init(struct device_node *np)
+ rate = clk_get_rate(clk);
+ ce->periodic_top = DIV_ROUND_CLOSEST(rate, HZ);
+
+ writel_relaxed(0, ce->regs + TIM_DIER);
+ writel_relaxed(0, ce->regs + TIM_SR);
+
+ err = request_irq(irq, stm32_clock_event_handler, IRQF_TIMER,
+ "stm32 clockevent", ce);
+ if (err) {
+ kfree(ce);
+ return err;
+ }
+
+ clockevents_config_and_register(&ce->evtdev, rate, 0x60, ~0U);
+
+ return 0;
+}
+
+static void __iomem *stm32_timer_cnt __read_mostly;
+static u64 notrace stm32_read_sched_clock(void)
+{
+ return readl_relaxed(stm32_timer_cnt);
+}
+
+static int __init stm32_clocksource_init(struct device_node *node,
+ void __iomem *regs,
+ struct clk *clk)
+{
+ unsigned long rate;
+
+ rate = clk_get_rate(clk);
+
+ writel_relaxed(~0U, regs + TIM_ARR);
+ writel_relaxed(0, regs + TIM_PSC);
+ writel_relaxed(0, regs + TIM_SR);
+ writel_relaxed(0, regs + TIM_DIER);
+ writel_relaxed(0, regs + TIM_SR);
+ writel_relaxed(TIM_CR1_ARPE | TIM_CR1_UDIS, regs + TIM_CR1);
+
+ /* Make sure that registers are updated */
+ writel_relaxed(TIM_EGR_UG, regs + TIM_EGR);
+
+ /* Enable controller */
+ writel_relaxed(TIM_CR1_ARPE | TIM_CR1_UDIS | TIM_CR1_CEN,
+ regs + TIM_CR1);
+
+ stm32_timer_cnt = regs + TIM_CNT;
+ sched_clock_register(stm32_read_sched_clock, 32, rate);
+
+ return clocksource_mmio_init(stm32_timer_cnt, "stm32_timer",
+ rate, 250, 32, clocksource_mmio_readl_up);
+}
+
+static int __init stm32_timer_init(struct device_node *node)
{
- struct stm32_clock_event_ddata *data = &clock_event_ddata;
- struct clk *clk;
struct reset_control *rstc;
- unsigned long rate, max_delta;
- int irq, ret, bits, prescaler = 1;
+ void __iomem *timer_base;
+ unsigned long max_arr;
+ struct clk *clk;
+ int irq, err = -EINVAL;
- clk = of_clk_get(np, 0);
- if (IS_ERR(clk)) {
- ret = PTR_ERR(clk);
- pr_err("failed to get clock for clockevent (%d)\n", ret);
- goto err_clk_get;
+ timer_base = of_io_request_and_map(node, 0, of_node_full_name(node));
+ if (IS_ERR(timer_base)) {
+ pr_err("Can't map registers\n");
+ goto out;
}
- ret = clk_prepare_enable(clk);
- if (ret) {
- pr_err("failed to enable timer clock for clockevent (%d)\n",
- ret);
- goto err_clk_enable;
+ irq = irq_of_parse_and_map(node, 0);
+ if (irq <= 0) {
+ pr_err("Can't parse IRQ\n");
+ goto out_unmap;
}
- rate = clk_get_rate(clk);
+ clk = of_clk_get(node, 0);
+ if (IS_ERR(clk)) {
+ pr_err("Can't get timer clock\n");
+ goto out_unmap;
+ }
- rstc = of_reset_control_get(np, NULL);
+ rstc = of_reset_control_get(node, NULL);
if (!IS_ERR(rstc)) {
reset_control_assert(rstc);
reset_control_deassert(rstc);
}
- data->base = of_iomap(np, 0);
- if (!data->base) {
- ret = -ENXIO;
- pr_err("failed to map registers for clockevent\n");
- goto err_iomap;
- }
-
- irq = irq_of_parse_and_map(np, 0);
- if (!irq) {
- ret = -EINVAL;
- pr_err("%pOF: failed to get irq.\n", np);
- goto err_get_irq;
+ err = clk_prepare_enable(clk);
+ if (err) {
+ pr_err("Couldn't enable parent clock\n");
+ goto out_clk;
}
/* Detect whether the timer is 16 or 32 bits */
- writel_relaxed(~0U, data->base + TIM_ARR);
- max_delta = readl_relaxed(data->base + TIM_ARR);
- if (max_delta == ~0U) {
- prescaler = 1;
- bits = 32;
- } else {
- prescaler = 1024;
- bits = 16;
+ writel_relaxed(~0U, timer_base + TIM_ARR);
+ max_arr = readl_relaxed(timer_base + TIM_ARR);
+ if (max_arr != ~0U) {
+ err = -EINVAL;
+ pr_err("32 bits timer is needed\n");
+ goto out_unprepare;
}
- writel_relaxed(0, data->base + TIM_ARR);
-
- writel_relaxed(prescaler - 1, data->base + TIM_PSC);
- writel_relaxed(TIM_EGR_UG, data->base + TIM_EGR);
- writel_relaxed(TIM_DIER_UIE, data->base + TIM_DIER);
- writel_relaxed(0, data->base + TIM_SR);
-
- data->periodic_top = DIV_ROUND_CLOSEST(rate, prescaler * HZ);
- clockevents_config_and_register(&data->evtdev,
- DIV_ROUND_CLOSEST(rate, prescaler),
- 0x1, max_delta);
+ err = stm32_clocksource_init(node, timer_base, clk);
+ if (err)
+ goto out_unprepare;
- ret = request_irq(irq, stm32_clock_event_handler, IRQF_TIMER,
- "stm32 clockevent", data);
- if (ret) {
- pr_err("%pOF: failed to request irq.\n", np);
- goto err_get_irq;
- }
-
- pr_info("%pOF: STM32 clockevent driver initialized (%d bits)\n",
- np, bits);
+ err = stm32_clockevent_init(node, timer_base, clk, irq);
+ if (err)
+ goto out_unprepare;
- return ret;
+ return 0;
-err_get_irq:
- iounmap(data->base);
-err_iomap:
+out_unprepare:
clk_disable_unprepare(clk);
-err_clk_enable:
+out_clk:
clk_put(clk);
-err_clk_get:
- return ret;
+out_unmap:
+ iounmap(timer_base);
+out:
+ return err;
}
-TIMER_OF_DECLARE(stm32, "st,stm32-timer", stm32_clockevent_init);
+CLOCKSOURCE_OF_DECLARE(stm32, "st,stm32-timer", stm32_timer_init);