clocksource: Add support for timekeeping on MStar MSC313e (WIP)

drivers/clocksource/Kconfig

@@ -708,4 +708,11 @@ config MICROCHIP_PIT64B
 	  modes and high resolution. It is used as a clocksource
 	  and a clockevent.
 
+config MSC313E_TIMER
+        bool "MSC313E timer driver"
+        select CLKSRC_MMIO
+        select TIMER_OF
+        help
+          Enables support for the MStar MSC313E timer.
+
 endmenu

drivers/clocksource/Makefile

@@ -88,3 +88,4 @@ obj-$(CONFIG_CSKY_MP_TIMER)		+= timer-mp-csky.o
 obj-$(CONFIG_GX6605S_TIMER)		+= timer-gx6605s.o
 obj-$(CONFIG_HYPERV_TIMER)		+= hyperv_timer.o
 obj-$(CONFIG_MICROCHIP_PIT64B)		+= timer-microchip-pit64b.o
+obj-$(CONFIG_MSC313E_TIMER)		+= timer-msc313e.o

drivers/clocksource/timer-msc313e.c

@@ -0,0 +1,241 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * MStar timer driver
+ *
+ * Copyright (C) 2021 Daniel Palmer
+ * Copyright (C) 2021 Romain Perier
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/clockchips.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/irqreturn.h>
+#include <linux/sched_clock.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/delay.h>
+
+#include "timer-of.h"
+
+#define TIMER_NAME "msc313e_timer"
+
+#define	MSC313E_REG_CTRL		0x00
+#define MSC313E_REG_CTRL_TIMER_EN	BIT(0)
+#define MSC313E_REG_CTRL_TIMER_TRIG	BIT(1)
+#define MSC313E_REG_CTRL_TIMER_INT_EN	BIT(8)
+#define MSC313E_REG_TIMER_MAX_LOW	0x08
+#define MSC313E_REG_TIMER_MAX_HIGH	0x0c
+#define MSC313E_REG_COUNTER_LOW		0x10
+#define MSC313E_REG_COUNTER_HIGH	0x14
+
+#define TIMER_SYNC_TICKS	3
+
+struct msc313e_delay {
+	void __iomem * base;
+	struct delay_timer delay;
+};
+
+static struct timer_of *msc313e_clkevt;
+static void __iomem *msc313e_clksrc;
+static struct msc313e_delay *msc313e_delay;
+
+static void msc313e_timer_stop(void __iomem *base)
+{
+	writew(0, base + MSC313E_REG_CTRL);
+}
+
+static void msc313e_timer_start(void __iomem *base, bool periodic)
+{
+	u16 reg;
+
+	reg = readw(base + MSC313E_REG_CTRL);
+	if (periodic)
+		reg |= MSC313E_REG_CTRL_TIMER_EN;
+	else
+		reg |= MSC313E_REG_CTRL_TIMER_TRIG;
+	writew(reg | MSC313E_REG_CTRL_TIMER_INT_EN, base + MSC313E_REG_CTRL);
+}
+
+static void msc313e_timer_setup(void __iomem *base, unsigned long delay)
+{
+	writew(delay >> 16, base + MSC313E_REG_TIMER_MAX_HIGH);
+	writew(delay & 0xffff, base + MSC313E_REG_TIMER_MAX_LOW);
+}
+
+static unsigned long msc313e_timer_current_value(void __iomem *base)
+{
+	unsigned long result;
+
+	result = readw(base + MSC313E_REG_COUNTER_LOW);
+	result |= readw(base + MSC313E_REG_COUNTER_HIGH) << 16;
+
+	return result;
+}
+
+static int msc313e_timer_clkevt_shutdown(struct clock_event_device *evt)
+{
+	struct timer_of *timer = to_timer_of(evt);
+
+	msc313e_timer_stop(timer_of_base(timer));
+
+	return 0;
+}
+
+static int msc313e_timer_clkevt_set_oneshot(struct clock_event_device *evt)
+{
+	struct timer_of *timer = to_timer_of(evt);
+
+	msc313e_timer_stop(timer_of_base(timer));
+	msc313e_timer_start(timer_of_base(timer), false);
+
+	return 0;
+}
+
+static int msc313e_timer_clkevt_set_periodic(struct clock_event_device *evt)
+{
+	struct timer_of *timer = to_timer_of(evt);
+
+	msc313e_timer_stop(timer_of_base(timer));
+	msc313e_timer_setup(timer_of_base(timer), timer_of_period(timer));
+	msc313e_timer_start(timer_of_base(timer), true);
+
+	return 0;
+}
+
+static int msc313e_timer_clkevt_next_event(unsigned long evt, struct clock_event_device *clkevt)
+{
+	struct timer_of *timer = to_timer_of(clkevt);
+
+	msc313e_timer_stop(timer_of_base(timer));
+	msc313e_timer_setup(timer_of_base(timer), evt);
+	msc313e_timer_start(timer_of_base(timer), false);
+
+	return 0;
+}
+
+static irqreturn_t msc313e_timer_clkevt_irq(int irq, void *dev_id)
+{
+	struct clock_event_device *evt = dev_id;
+
+	evt->event_handler(evt);
+
+	return IRQ_HANDLED;
+}
+
+static u64 msc313e_timer_clksrc_read(struct clocksource *cs)
+{
+	return msc313e_timer_current_value(msc313e_clksrc) & cs->mask;
+}
+
+static unsigned long msc313e_read_delay_timer_read(void)
+{
+	return msc313e_timer_current_value(msc313e_delay->base);
+}
+
+static u64 msc313e_timer_sched_clock_read(void)
+{
+	return msc313e_timer_current_value(msc313e_clksrc);
+}
+
+static int __init msc313e_clkevt_init(struct device_node *np)
+{
+	int ret;
+
+	msc313e_clkevt = kzalloc(sizeof(struct timer_of), GFP_KERNEL);
+	if (!msc313e_clkevt)
+		return -ENOMEM;
+
+	msc313e_clkevt->flags = TIMER_OF_IRQ | TIMER_OF_CLOCK | TIMER_OF_BASE;
+	msc313e_clkevt->clkevt.name = TIMER_NAME;
+	msc313e_clkevt->clkevt.rating = 300;
+	msc313e_clkevt->clkevt.features = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT;
+	msc313e_clkevt->clkevt.set_state_shutdown = msc313e_timer_clkevt_shutdown;
+	msc313e_clkevt->clkevt.set_state_periodic = msc313e_timer_clkevt_set_periodic;
+	msc313e_clkevt->clkevt.set_state_oneshot = msc313e_timer_clkevt_set_oneshot;
+	msc313e_clkevt->clkevt.tick_resume = msc313e_timer_clkevt_shutdown;
+	msc313e_clkevt->clkevt.set_next_event = msc313e_timer_clkevt_next_event;
+	msc313e_clkevt->clkevt.cpumask = cpumask_of(0);
+	msc313e_clkevt->of_irq.handler = msc313e_timer_clkevt_irq;
+
+	ret = timer_of_init(np, msc313e_clkevt);
+	if (ret)
+		goto err_clkevt_kfree;
+
+	msc313e_clkevt->clkevt.irq = msc313e_clkevt->of_irq.irq;
+
+	clockevents_config_and_register(&msc313e_clkevt->clkevt, timer_of_rate(msc313e_clkevt),
+					TIMER_SYNC_TICKS, 0xffffffff);
+	return 0;
+
+err_clkevt_kfree:
+	kfree(msc313e_clkevt);
+	return ret;
+}
+
+static int __init msc313e_clksrc_init(struct device_node *np)
+{
+	struct timer_of to = { 0 };
+	int ret;
+	u16 reg;
+
+	to.flags = TIMER_OF_BASE | TIMER_OF_CLOCK;
+	ret = timer_of_init(np, &to);
+	if (ret)
+		return ret;
+
+	msc313e_clksrc = timer_of_base(&to);
+	reg = readw(msc313e_clksrc + MSC313E_REG_CTRL);
+	reg |= MSC313E_REG_CTRL_TIMER_EN;
+	writew(reg, msc313e_clksrc + MSC313E_REG_CTRL);
+
+	sched_clock_register(msc313e_timer_sched_clock_read, 32, timer_of_rate(&to));
+	return clocksource_mmio_init(timer_of_base(&to), TIMER_NAME, timer_of_rate(&to), 300, 32,
+				     msc313e_timer_clksrc_read);
+}
+
+static int __init msc313e_delay_init(struct device_node *np)
+{
+	struct timer_of to = { 0 };
+	int ret;
+	u16 reg;
+
+	to.flags = TIMER_OF_BASE | TIMER_OF_CLOCK;
+	ret = timer_of_init(np, &to);
+	if (ret)
+		return ret;
+
+	msc313e_delay = kzalloc(sizeof(struct msc313e_delay), GFP_KERNEL);
+	if (!msc313e_delay)
+		return -ENOMEM;
+
+	msc313e_delay->base = timer_of_base(&to);
+	msc313e_delay->delay.read_current_timer = msc313e_read_delay_timer_read;
+	msc313e_delay->delay.freq = timer_of_rate(&to);
+
+	reg = readw(msc313e_delay->base + MSC313E_REG_CTRL);
+	reg |= MSC313E_REG_CTRL_TIMER_EN;
+	writew(reg, msc313e_delay->base + MSC313E_REG_CTRL);
+
+	register_current_timer_delay(&msc313e_delay->delay);
+
+	return 0;
+}
+
+static int __init msc313e_timer_init(struct device_node *np)
+{
+	if (!msc313e_clkevt)
+		return msc313e_clkevt_init(np);
+
+	if (!msc313e_clksrc)
+		return msc313e_clksrc_init(np);
+
+	if (!msc313e_delay)
+		return msc313e_delay_init(np);
+
+	return -EINVAL;
+}
+
+TIMER_OF_DECLARE(msc313, "mstar,msc313e-timer", msc313e_timer_init);

kernel/time/clocksource.c

@@ -954,7 +954,7 @@ static void __clocksource_select(bool skipcur)
 
 found:
 	if (curr_clocksource != best && !timekeeping_notify(best)) {
-		pr_info("Switched to clocksource %s\n", best->name);
+		//pr_info("Switched to clocksource %s\n", best->name);
 		curr_clocksource = best;
 	}
 }