2 files changed, 40 insertions, 3 deletions

diff --git a/include/linux/clocksource.h b/include/linux/clocksource.h
index 0d96cde9ee5d..5a40d14daa9f 100644
--- a/include/linux/clocksource.h
+++ b/include/linux/clocksource.h
@@ -144,6 +144,8 @@ extern u64 timecounter_cyc2time(struct timecounter *tc,
  *                              The ideal clocksource. A must-use where
  *                              available.
  * @read:               returns a cycle value, passes clocksource as argument
+ * @enable:             optional function to enable the clocksource
+ * @disable:            optional function to disable the clocksource
  * @mask:               bitmask for two's complement
  *                      subtraction of non 64 bit counters
  * @mult:               cycle to nanosecond multiplier (adjusted by NTP)
@@ -163,6 +165,8 @@ struct clocksource {
         struct list_head list;
         int rating;
         cycle_t (*read)(struct clocksource *cs);
+        int (*enable)(struct clocksource *cs);
+        void (*disable)(struct clocksource *cs);
         cycle_t mask;
         u32 mult;
         u32 mult_orig;
@@ -275,6 +279,33 @@ static inline cycle_t clocksource_read(struct clocksource *cs)
 }
 
 /**
+ * clocksource_enable: - enable clocksource
+ * @cs:         pointer to clocksource
+ *
+ * Enables the specified clocksource. The clocksource callback
+ * function should start up the hardware and setup mult and field
+ * members of struct clocksource to reflect hardware capabilities.
+ */
+static inline int clocksource_enable(struct clocksource *cs)
+{
+        return cs->enable ? cs->enable(cs) : 0;
+}
+
+/**
+ * clocksource_disable: - disable clocksource
+ * @cs:         pointer to clocksource
+ *
+ * Disables the specified clocksource. The clocksource callback
+ * function should power down the now unused hardware block to
+ * save power.
+ */
+static inline void clocksource_disable(struct clocksource *cs)
+{
+        if (cs->disable)
+                cs->disable(cs);
+}
+
+/**
  * cyc2ns - converts clocksource cycles to nanoseconds
  * @cs:         Pointer to clocksource
  * @cycles:     Cycles
diff --git a/kernel/time/timekeeping.c b/kernel/time/timekeeping.c
index 900f1b6598d1..687dff49f6e7 100644
--- a/kernel/time/timekeeping.c
+++ b/kernel/time/timekeeping.c
@@ -182,7 +182,7 @@ EXPORT_SYMBOL(do_settimeofday);
  */
 static void change_clocksource(void)
 {
-        struct clocksource *new;
+        struct clocksource *new, *old;
 
         new = clocksource_get_next();
 
@@ -191,11 +191,16 @@ static void change_clocksource(void)
 
         clocksource_forward_now();
 
-        new->raw_time = clock->raw_time;
+        if (clocksource_enable(new))
+                return;
 
+        new->raw_time = clock->raw_time;
+        old = clock;
         clock = new;
+        clocksource_disable(old);
+
         clock->cycle_last = 0;
-        clock->cycle_last = clocksource_read(new);
+        clock->cycle_last = clocksource_read(clock);
         clock->error = 0;
         clock->xtime_nsec = 0;
         clocksource_calculate_interval(clock, NTP_INTERVAL_LENGTH);
@@ -292,6 +297,7 @@ void __init timekeeping_init(void)
         ntp_init();
 
         clock = clocksource_get_next();
+        clocksource_enable(clock);
         clocksource_calculate_interval(clock, NTP_INTERVAL_LENGTH);
         clock->cycle_last = clocksource_read(clock);