1 files changed, 86 insertions, 35 deletions
diff --git a/drivers/clocksource/cadence_ttc_timer.c b/drivers/clocksource/cadence_ttc_timer.c
index 63f176de0d02..49fbe2847c84 100644
--- a/drivers/clocksource/cadence_ttc_timer.c
+++ b/drivers/clocksource/cadence_ttc_timer.c
@@ -16,6 +16,7 @@
 */
 #include <linux/clk.h>
+#include <linux/clk-provider.h>
 #include <linux/interrupt.h>
 #include <linux/clockchips.h>
 #include <linux/of_address.h>
@@ -52,6 +53,8 @@
 #define TTC_CNT_CNTRL_DISABLE_MASK      0x1
 #define TTC_CLK_CNTRL_CSRC_MASK         (1 << 5)        /* clock source */
+#define TTC_CLK_CNTRL_PSV_MASK          0x1e
+#define TTC_CLK_CNTRL_PSV_SHIFT         1
 /*
 * Setup the timers to use pre-scaling, using a fixed value for now that will
@@ -63,6 +66,8 @@
 #define CLK_CNTRL_PRESCALE_EN   1
 #define CNT_CNTRL_RESET         (1 << 4)
+#define MAX_F_ERR 50
 /**
 * struct ttc_timer - This definition defines local timer structure
 *
@@ -82,6 +87,8 @@ struct ttc_timer {
                container_of(x, struct ttc_timer, clk_rate_change_nb)
 struct ttc_timer_clocksource {
+        u32                     scale_clk_ctrl_reg_old;
+        u32                     scale_clk_ctrl_reg_new;
        struct ttc_timer        ttc;
        struct clocksource      cs;
 };
@@ -229,32 +236,89 @@ static int ttc_rate_change_clocksource_cb(struct notifier_block *nb,
                        struct ttc_timer_clocksource, ttc);
        switch (event) {
-        case POST_RATE_CHANGE:
+        case PRE_RATE_CHANGE:
+        {
+                u32 psv;
+                unsigned long factor, rate_low, rate_high;
+                if (ndata->new_rate > ndata->old_rate) {
+                        factor = DIV_ROUND_CLOSEST(ndata->new_rate,
+                                        ndata->old_rate);
+                        rate_low = ndata->old_rate;
+                        rate_high = ndata->new_rate;
+                } else {
+                        factor = DIV_ROUND_CLOSEST(ndata->old_rate,
+                                        ndata->new_rate);
+                        rate_low = ndata->new_rate;
+                        rate_high = ndata->old_rate;
+                }
+                if (!is_power_of_2(factor))
+                                return NOTIFY_BAD;
+                if (abs(rate_high - (factor * rate_low)) > MAX_F_ERR)
+                        return NOTIFY_BAD;
+                factor = __ilog2_u32(factor);
                /*
-                 * Do whatever is necessary to maintain a proper time base
+                 * store timer clock ctrl register so we can restore it in case
-                 *
+                 * of an abort.
-                 * I cannot find a way to adjust the currently used clocksource
-                 * to the new frequency. __clocksource_updatefreq_hz() sounds
-                 * good, but does not work. Not sure what's that missing.
-                 *
-                 * This approach works, but triggers two clocksource switches.
-                 * The first after unregister to clocksource jiffies. And
-                 * another one after the register to the newly registered timer.
-                 *
-                 * Alternatively we could 'waste' another HW timer to ping pong
-                 * between clock sources. That would also use one register and
-                 * one unregister call, but only trigger one clocksource switch
-                 * for the cost of another HW timer used by the OS.
                 */
-                clocksource_unregister(&ttccs->cs);
+                ttccs->scale_clk_ctrl_reg_old =
-                clocksource_register_hz(&ttccs->cs,
+                        __raw_readl(ttccs->ttc.base_addr +
-                                ndata->new_rate / PRESCALE);
+                                        TTC_CLK_CNTRL_OFFSET);
-                /* fall through */
-        case PRE_RATE_CHANGE:
+                psv = (ttccs->scale_clk_ctrl_reg_old &
+                                TTC_CLK_CNTRL_PSV_MASK) >>
+                                TTC_CLK_CNTRL_PSV_SHIFT;
+                if (ndata->new_rate < ndata->old_rate)
+                        psv -= factor;
+                else
+                        psv += factor;
+                /* prescaler within legal range? */
+                if (psv & ~(TTC_CLK_CNTRL_PSV_MASK >> TTC_CLK_CNTRL_PSV_SHIFT))
+                        return NOTIFY_BAD;
+                ttccs->scale_clk_ctrl_reg_new = ttccs->scale_clk_ctrl_reg_old &
+                        ~TTC_CLK_CNTRL_PSV_MASK;
+                ttccs->scale_clk_ctrl_reg_new |= psv << TTC_CLK_CNTRL_PSV_SHIFT;
+                /* scale down: adjust divider in post-change notification */
+                if (ndata->new_rate < ndata->old_rate)
+                        return NOTIFY_DONE;
+                /* scale up: adjust divider now - before frequency change */
+                __raw_writel(ttccs->scale_clk_ctrl_reg_new,
+                                ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
+                break;
+        }
+        case POST_RATE_CHANGE:
+                /* scale up: pre-change notification did the adjustment */
+                if (ndata->new_rate > ndata->old_rate)
+                        return NOTIFY_OK;
+                /* scale down: adjust divider now - after frequency change */
+                __raw_writel(ttccs->scale_clk_ctrl_reg_new,
+                                ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
+                break;
        case ABORT_RATE_CHANGE:
+                /* we have to undo the adjustment in case we scale up */
+                if (ndata->new_rate < ndata->old_rate)
+                        return NOTIFY_OK;
+                /* restore original register value */
+                __raw_writel(ttccs->scale_clk_ctrl_reg_old,
+                                ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
+                /* fall through */
        default:
                return NOTIFY_DONE;
        }
+        return NOTIFY_DONE;
 }
 static void __init ttc_setup_clocksource(struct clk *clk, void __iomem *base)
@@ -321,25 +385,12 @@ static int ttc_rate_change_clockevent_cb(struct notifier_block *nb,
        switch (event) {
        case POST_RATE_CHANGE:
-        {
-                unsigned long flags;
-                /*
-                 * clockevents_update_freq should be called with IRQ disabled on
-                 * the CPU the timer provides events for. The timer we use is
-                 * common to both CPUs, not sure if we need to run on both
-                 * cores.
-                 */
-                local_irq_save(flags);
-                clockevents_update_freq(&ttcce->ce,
-                                ndata->new_rate / PRESCALE);
-                local_irq_restore(flags);
                /* update cached frequency */
                ttc->freq = ndata->new_rate;
+                clockevents_update_freq(&ttcce->ce, ndata->new_rate / PRESCALE);
                /* fall through */
-        }
        case PRE_RATE_CHANGE:
        case ABORT_RATE_CHANGE:
        default:

diff --git a/drivers/clocksource/cadence_ttc_timer.c b/drivers/clocksource/cadence_ttc_timer.c index 63f176de0d02..49fbe2847c84 100644 --- a/drivers/clocksource/cadence_ttc_timer.c +++ b/drivers/clocksource/cadence_ttc_timer.c
@@ -16,6 +16,7 @@
16	*/	16	*/
17		17
18	#include <linux/clk.h>	18	#include <linux/clk.h>
		19	#include <linux/clk-provider.h>
19	#include <linux/interrupt.h>	20	#include <linux/interrupt.h>
20	#include <linux/clockchips.h>	21	#include <linux/clockchips.h>
21	#include <linux/of_address.h>	22	#include <linux/of_address.h>
@@ -52,6 +53,8 @@
52	#define TTC_CNT_CNTRL_DISABLE_MASK 0x1	53	#define TTC_CNT_CNTRL_DISABLE_MASK 0x1
53		54
54	#define TTC_CLK_CNTRL_CSRC_MASK (1 << 5) /* clock source */	55	#define TTC_CLK_CNTRL_CSRC_MASK (1 << 5) /* clock source */
		56	#define TTC_CLK_CNTRL_PSV_MASK 0x1e
		57	#define TTC_CLK_CNTRL_PSV_SHIFT 1
55		58
56	/*	59	/*
57	* Setup the timers to use pre-scaling, using a fixed value for now that will	60	* Setup the timers to use pre-scaling, using a fixed value for now that will
@@ -63,6 +66,8 @@
63	#define CLK_CNTRL_PRESCALE_EN 1	66	#define CLK_CNTRL_PRESCALE_EN 1
64	#define CNT_CNTRL_RESET (1 << 4)	67	#define CNT_CNTRL_RESET (1 << 4)
65		68
		69	#define MAX_F_ERR 50
		70
66	/**	71	/**
67	* struct ttc_timer - This definition defines local timer structure	72	* struct ttc_timer - This definition defines local timer structure
68	*	73	*
@@ -82,6 +87,8 @@ struct ttc_timer {
82	container_of(x, struct ttc_timer, clk_rate_change_nb)	87	container_of(x, struct ttc_timer, clk_rate_change_nb)
83		88
84	struct ttc_timer_clocksource {	89	struct ttc_timer_clocksource {
		90	u32 scale_clk_ctrl_reg_old;
		91	u32 scale_clk_ctrl_reg_new;
85	struct ttc_timer ttc;	92	struct ttc_timer ttc;
86	struct clocksource cs;	93	struct clocksource cs;
87	};	94	};
@@ -229,32 +236,89 @@ static int ttc_rate_change_clocksource_cb(struct notifier_block *nb,
229	struct ttc_timer_clocksource, ttc);	236	struct ttc_timer_clocksource, ttc);
230		237
231	switch (event) {	238	switch (event) {
232	case POST_RATE_CHANGE:	239	case PRE_RATE_CHANGE:
		240	{
		241	u32 psv;
		242	unsigned long factor, rate_low, rate_high;
		243
		244	if (ndata->new_rate > ndata->old_rate) {
		245	factor = DIV_ROUND_CLOSEST(ndata->new_rate,
		246	ndata->old_rate);
		247	rate_low = ndata->old_rate;
		248	rate_high = ndata->new_rate;
		249	} else {
		250	factor = DIV_ROUND_CLOSEST(ndata->old_rate,
		251	ndata->new_rate);
		252	rate_low = ndata->new_rate;
		253	rate_high = ndata->old_rate;
		254	}
		255
		256	if (!is_power_of_2(factor))
		257	return NOTIFY_BAD;
		258
		259	if (abs(rate_high - (factor * rate_low)) > MAX_F_ERR)
		260	return NOTIFY_BAD;
		261
		262	factor = __ilog2_u32(factor);
		263
233	/*	264	/*
234	* Do whatever is necessary to maintain a proper time base	265	* store timer clock ctrl register so we can restore it in case
235	*	266	* of an abort.
236	* I cannot find a way to adjust the currently used clocksource
237	* to the new frequency. __clocksource_updatefreq_hz() sounds
238	* good, but does not work. Not sure what's that missing.
239	*
240	* This approach works, but triggers two clocksource switches.
241	* The first after unregister to clocksource jiffies. And
242	* another one after the register to the newly registered timer.
243	*
244	* Alternatively we could 'waste' another HW timer to ping pong
245	* between clock sources. That would also use one register and
246	* one unregister call, but only trigger one clocksource switch
247	* for the cost of another HW timer used by the OS.
248	*/	267	*/
249	clocksource_unregister(&ttccs->cs);	268	ttccs->scale_clk_ctrl_reg_old =
250	clocksource_register_hz(&ttccs->cs,	269	__raw_readl(ttccs->ttc.base_addr +
251	ndata->new_rate / PRESCALE);	270	TTC_CLK_CNTRL_OFFSET);
252	/* fall through */	271
253	case PRE_RATE_CHANGE:	272	psv = (ttccs->scale_clk_ctrl_reg_old &
		273	TTC_CLK_CNTRL_PSV_MASK) >>
		274	TTC_CLK_CNTRL_PSV_SHIFT;
		275	if (ndata->new_rate < ndata->old_rate)
		276	psv -= factor;
		277	else
		278	psv += factor;
		279
		280	/* prescaler within legal range? */
		281	if (psv & ~(TTC_CLK_CNTRL_PSV_MASK >> TTC_CLK_CNTRL_PSV_SHIFT))
		282	return NOTIFY_BAD;
		283
		284	ttccs->scale_clk_ctrl_reg_new = ttccs->scale_clk_ctrl_reg_old &
		285	~TTC_CLK_CNTRL_PSV_MASK;
		286	ttccs->scale_clk_ctrl_reg_new \|= psv << TTC_CLK_CNTRL_PSV_SHIFT;
		287
		288
		289	/* scale down: adjust divider in post-change notification */
		290	if (ndata->new_rate < ndata->old_rate)
		291	return NOTIFY_DONE;
		292
		293	/* scale up: adjust divider now - before frequency change */
		294	__raw_writel(ttccs->scale_clk_ctrl_reg_new,
		295	ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
		296	break;
		297	}
		298	case POST_RATE_CHANGE:
		299	/* scale up: pre-change notification did the adjustment */
		300	if (ndata->new_rate > ndata->old_rate)
		301	return NOTIFY_OK;
		302
		303	/* scale down: adjust divider now - after frequency change */
		304	__raw_writel(ttccs->scale_clk_ctrl_reg_new,
		305	ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
		306	break;
		307
254	case ABORT_RATE_CHANGE:	308	case ABORT_RATE_CHANGE:
		309	/* we have to undo the adjustment in case we scale up */
		310	if (ndata->new_rate < ndata->old_rate)
		311	return NOTIFY_OK;
		312
		313	/* restore original register value */
		314	__raw_writel(ttccs->scale_clk_ctrl_reg_old,
		315	ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
		316	/* fall through */
255	default:	317	default:
256	return NOTIFY_DONE;	318	return NOTIFY_DONE;
257	}	319	}
		320
		321	return NOTIFY_DONE;
258	}	322	}
259		323
260	static void __init ttc_setup_clocksource(struct clk clk, void __iomem base)	324	static void __init ttc_setup_clocksource(struct clk clk, void __iomem base)
@@ -321,25 +385,12 @@ static int ttc_rate_change_clockevent_cb(struct notifier_block *nb,
321		385
322	switch (event) {	386	switch (event) {
323	case POST_RATE_CHANGE:	387	case POST_RATE_CHANGE:
324	{
325	unsigned long flags;
326
327	/*
328	* clockevents_update_freq should be called with IRQ disabled on
329	* the CPU the timer provides events for. The timer we use is
330	* common to both CPUs, not sure if we need to run on both
331	* cores.
332	*/
333	local_irq_save(flags);
334	clockevents_update_freq(&ttcce->ce,
335	ndata->new_rate / PRESCALE);
336	local_irq_restore(flags);
337
338	/* update cached frequency */	388	/* update cached frequency */
339	ttc->freq = ndata->new_rate;	389	ttc->freq = ndata->new_rate;
340		390
		391	clockevents_update_freq(&ttcce->ce, ndata->new_rate / PRESCALE);
		392
341	/* fall through */	393	/* fall through */
342	}
343	case PRE_RATE_CHANGE:	394	case PRE_RATE_CHANGE:
344	case ABORT_RATE_CHANGE:	395	case ABORT_RATE_CHANGE:
345	default:	396	default: