[PATCH] improve idle cputime accounting

Distinguish the cputime of the idle process where idle is actually using
cpu cycles from the cputime where idle is sleeping on an enabled wait psw.
The former is accounted as system time, the later as idle time.

Signed-off-by: Martin Schwidefsky <schwidefsky@de.ibm.com>

9 files changed, 248 insertions, 265 deletions

diff --git a/arch/s390/include/asm/cpu.h b/arch/s390/include/asm/cpu.h
index 89456df43c4a..d60a2eefb17b 100644
--- a/arch/s390/include/asm/cpu.h
+++ b/arch/s390/include/asm/cpu.h
@@ -21,12 +21,12 @@ struct s390_idle_data {
 
 DECLARE_PER_CPU(struct s390_idle_data, s390_idle);
 
-void s390_idle_leave(void);
+void vtime_start_cpu(void);
 
 static inline void s390_idle_check(void)
 {
         if ((&__get_cpu_var(s390_idle))->idle_enter != 0ULL)
-                s390_idle_leave();
+                vtime_start_cpu();
 }
 
 #endif /* _ASM_S390_CPU_H_ */
diff --git a/arch/s390/include/asm/timer.h b/arch/s390/include/asm/timer.h
index 61705d60f995..e4bcab739c19 100644
--- a/arch/s390/include/asm/timer.h
+++ b/arch/s390/include/asm/timer.h
@@ -23,20 +23,18 @@ struct vtimer_list {
         __u64 expires;
         __u64 interval;
 
-        spinlock_t lock;
-        unsigned long magic;
-
         void (*function)(unsigned long);
         unsigned long data;
 };
 
-/* the offset value will wrap after ca. 71 years */
+/* the vtimer value will wrap after ca. 71 years */
 struct vtimer_queue {
         struct list_head list;
         spinlock_t lock;
-        __u64 to_expire;          /* current event expire time */
-        __u64 offset;             /* list offset to zero */
-        __u64 idle;               /* temp var for idle */
+        __u64 timer;            /* last programmed timer */
+        __u64 elapsed;          /* elapsed time of timer expire values */
+        __u64 idle;             /* temp var for idle */
+        int do_spt;             /* =1: reprogram cpu timer in idle */
 };
 
 extern void init_virt_timer(struct vtimer_list *timer);
@@ -48,8 +46,8 @@ extern int del_virt_timer(struct vtimer_list *timer);
 extern void init_cpu_vtimer(void);
 extern void vtime_init(void);
 
-extern void vtime_start_cpu_timer(void);
-extern void vtime_stop_cpu_timer(void);
+extern void vtime_stop_cpu(void);
+extern void vtime_start_leave(void);
 
 #endif /* __KERNEL__ */
 
diff --git a/arch/s390/kernel/entry.S b/arch/s390/kernel/entry.S
index 55de521aef77..1268aa2991bf 100644
--- a/arch/s390/kernel/entry.S
+++ b/arch/s390/kernel/entry.S
@@ -583,8 +583,8 @@ kernel_per:
 
         .globl io_int_handler
 io_int_handler:
-        stpt    __LC_ASYNC_ENTER_TIMER
         stck    __LC_INT_CLOCK
+        stpt    __LC_ASYNC_ENTER_TIMER
         SAVE_ALL_BASE __LC_SAVE_AREA+16
         SAVE_ALL_ASYNC __LC_IO_OLD_PSW,__LC_SAVE_AREA+16
         CREATE_STACK_FRAME __LC_IO_OLD_PSW,__LC_SAVE_AREA+16
@@ -723,8 +723,8 @@ io_notify_resume:
 
         .globl  ext_int_handler
 ext_int_handler:
-        stpt    __LC_ASYNC_ENTER_TIMER
         stck    __LC_INT_CLOCK
+        stpt    __LC_ASYNC_ENTER_TIMER
         SAVE_ALL_BASE __LC_SAVE_AREA+16
         SAVE_ALL_ASYNC __LC_EXT_OLD_PSW,__LC_SAVE_AREA+16
         CREATE_STACK_FRAME __LC_EXT_OLD_PSW,__LC_SAVE_AREA+16
@@ -750,6 +750,7 @@ __critical_end:
 
         .globl mcck_int_handler
 mcck_int_handler:
+        stck    __LC_INT_CLOCK
         spt     __LC_CPU_TIMER_SAVE_AREA        # revalidate cpu timer
         lm      %r0,%r15,__LC_GPREGS_SAVE_AREA  # revalidate gprs
         SAVE_ALL_BASE __LC_SAVE_AREA+32
diff --git a/arch/s390/kernel/entry64.S b/arch/s390/kernel/entry64.S
index 16bb4fd1a403..ae83c195171c 100644
--- a/arch/s390/kernel/entry64.S
+++ b/arch/s390/kernel/entry64.S
@@ -559,8 +559,8 @@ kernel_per:
  */
         .globl io_int_handler
 io_int_handler:
-        stpt    __LC_ASYNC_ENTER_TIMER
         stck    __LC_INT_CLOCK
+        stpt    __LC_ASYNC_ENTER_TIMER
         SAVE_ALL_BASE __LC_SAVE_AREA+32
         SAVE_ALL_ASYNC __LC_IO_OLD_PSW,__LC_SAVE_AREA+32
         CREATE_STACK_FRAME __LC_IO_OLD_PSW,__LC_SAVE_AREA+32
@@ -721,8 +721,8 @@ io_notify_resume:
  */
         .globl  ext_int_handler
 ext_int_handler:
-        stpt    __LC_ASYNC_ENTER_TIMER
         stck    __LC_INT_CLOCK
+        stpt    __LC_ASYNC_ENTER_TIMER
         SAVE_ALL_BASE __LC_SAVE_AREA+32
         SAVE_ALL_ASYNC __LC_EXT_OLD_PSW,__LC_SAVE_AREA+32
         CREATE_STACK_FRAME __LC_EXT_OLD_PSW,__LC_SAVE_AREA+32
@@ -746,6 +746,7 @@ __critical_end:
  */
         .globl mcck_int_handler
 mcck_int_handler:
+        stck    __LC_INT_CLOCK
         la      %r1,4095                # revalidate r1
         spt     __LC_CPU_TIMER_SAVE_AREA-4095(%r1)      # revalidate cpu timer
         lmg     %r0,%r15,__LC_GPREGS_SAVE_AREA-4095(%r1)# revalidate gprs
diff --git a/arch/s390/kernel/process.c b/arch/s390/kernel/process.c
index 1e06436f07c2..b6110bdf8dc2 100644
--- a/arch/s390/kernel/process.c
+++ b/arch/s390/kernel/process.c
@@ -46,7 +46,6 @@
 #include <asm/processor.h>
 #include <asm/irq.h>
 #include <asm/timer.h>
-#include <asm/cpu.h>
 #include "entry.h"
 
 asmlinkage void ret_from_fork(void) asm ("ret_from_fork");
@@ -76,35 +75,12 @@ unsigned long thread_saved_pc(struct task_struct *tsk)
         return sf->gprs[8];
 }
 
-DEFINE_PER_CPU(struct s390_idle_data, s390_idle) = {
-        .lock = __SPIN_LOCK_UNLOCKED(s390_idle.lock)
-};
-
-void s390_idle_leave(void)
-{
-        struct s390_idle_data *idle;
-        unsigned long long idle_time;
-
-        idle = &__get_cpu_var(s390_idle);
-        idle_time = S390_lowcore.int_clock - idle->idle_enter;
-        spin_lock(&idle->lock);
-        idle->idle_time += idle_time;
-        idle->idle_enter = 0ULL;
-        idle->idle_count++;
-        spin_unlock(&idle->lock);
-        vtime_start_cpu_timer();
-}
-
 extern void s390_handle_mcck(void);
 /*
  * The idle loop on a S390...
  */
 static void default_idle(void)
 {
-        struct s390_idle_data *idle = &__get_cpu_var(s390_idle);
-        unsigned long addr;
-        psw_t psw;
-
         /* CPU is going idle. */
         local_irq_disable();
         if (need_resched()) {
@@ -120,7 +96,6 @@ static void default_idle(void)
         local_mcck_disable();
         if (test_thread_flag(TIF_MCCK_PENDING)) {
                 local_mcck_enable();
-                s390_idle_leave();
                 local_irq_enable();
                 s390_handle_mcck();
                 return;
@@ -128,42 +103,9 @@ static void default_idle(void)
         trace_hardirqs_on();
         /* Don't trace preempt off for idle. */
         stop_critical_timings();
-        vtime_stop_cpu_timer();
-
-        /*
-         * The inline assembly is equivalent to
-         *      idle->idle_enter = get_clock();
-         *      __load_psw_mask(psw_kernel_bits | PSW_MASK_WAIT |
-         *                         PSW_MASK_IO | PSW_MASK_EXT);
-         * The difference is that the inline assembly makes sure that
-         * the stck instruction is right before the lpsw instruction.
-         * This is done to increase the precision.
-         */
-
-        /* Wait for external, I/O or machine check interrupt. */
-        psw.mask = psw_kernel_bits|PSW_MASK_WAIT|PSW_MASK_IO|PSW_MASK_EXT;
-#ifndef __s390x__
-        asm volatile(
-                "       basr    %0,0\n"
-                "0:     ahi     %0,1f-0b\n"
-                "       st      %0,4(%2)\n"
-                "       stck    0(%3)\n"
-                "       lpsw    0(%2)\n"
-                "1:"
-                : "=&d" (addr), "=m" (idle->idle_enter)
-                : "a" (&psw), "a" (&idle->idle_enter), "m" (psw)
-                : "memory", "cc");
-#else /* __s390x__ */
-        asm volatile(
-                "       larl    %0,1f\n"
-                "       stg     %0,8(%2)\n"
-                "       stck    0(%3)\n"
-                "       lpswe   0(%2)\n"
-                "1:"
-                : "=&d" (addr), "=m" (idle->idle_enter)
-                : "a" (&psw), "a" (&idle->idle_enter), "m" (psw)
-                : "memory", "cc");
-#endif /* __s390x__ */
+        /* Stop virtual timer and halt the cpu. */
+        vtime_stop_cpu();
+        /* Reenable preemption tracer. */
         start_critical_timings();
 }
 
diff --git a/arch/s390/kernel/s390_ext.c b/arch/s390/kernel/s390_ext.c
index e019b419efc6..a0d2d55d7fb3 100644
--- a/arch/s390/kernel/s390_ext.c
+++ b/arch/s390/kernel/s390_ext.c
@@ -119,8 +119,8 @@ void do_extint(struct pt_regs *regs, unsigned short code)
         struct pt_regs *old_regs;
 
         old_regs = set_irq_regs(regs);
-        irq_enter();
         s390_idle_check();
+        irq_enter();
         if (S390_lowcore.int_clock >= S390_lowcore.clock_comparator)
                 /* Serve timer interrupts first. */
                 clock_comparator_work();
diff --git a/arch/s390/kernel/vtime.c b/arch/s390/kernel/vtime.c
index 25d21fef76ba..2fb36e462194 100644
--- a/arch/s390/kernel/vtime.c
+++ b/arch/s390/kernel/vtime.c
@@ -23,10 +23,35 @@
 #include <asm/s390_ext.h>
 #include <asm/timer.h>
 #include <asm/irq_regs.h>
+#include <asm/cpu.h>
 
 static ext_int_info_t ext_int_info_timer;
+
 static DEFINE_PER_CPU(struct vtimer_queue, virt_cpu_timer);
 
+DEFINE_PER_CPU(struct s390_idle_data, s390_idle) = {
+        .lock = __SPIN_LOCK_UNLOCKED(s390_idle.lock)
+};
+
+static inline __u64 get_vtimer(void)
+{
+        __u64 timer;
+
+        asm volatile("STPT %0" : "=m" (timer));
+        return timer;
+}
+
+static inline void set_vtimer(__u64 expires)
+{
+        __u64 timer;
+
+        asm volatile ("  STPT %0\n"  /* Store current cpu timer value */
+                      "  SPT %1"     /* Set new value immediatly afterwards */
+                      : "=m" (timer) : "m" (expires) );
+        S390_lowcore.system_timer += S390_lowcore.last_update_timer - timer;
+        S390_lowcore.last_update_timer = expires;
+}
+
 /*
  * Update process times based on virtual cpu times stored by entry.S
  * to the lowcore fields user_timer, system_timer & steal_clock.
@@ -53,18 +78,12 @@ static void do_account_vtime(struct task_struct *tsk, int hardirq_offset)
         system = S390_lowcore.system_timer - ti->system_timer;
         S390_lowcore.steal_timer -= system;
         ti->system_timer = S390_lowcore.system_timer;
-        if (idle_task(smp_processor_id()) != current)
-                account_system_time(tsk, hardirq_offset, system, system);
-        else
-                account_idle_time(system);
+        account_system_time(tsk, hardirq_offset, system, system);
 
         steal = S390_lowcore.steal_timer;
         if ((s64) steal > 0) {
                 S390_lowcore.steal_timer = 0;
-                if (idle_task(smp_processor_id()) != current)
-                        account_steal_time(steal);
-                else
-                        account_idle_time(steal);
+                account_steal_time(steal);
         }
 }
 
@@ -96,80 +115,127 @@ void account_system_vtime(struct task_struct *tsk)
         __u64 timer, system;
 
         timer = S390_lowcore.last_update_timer;
-        asm volatile ("  STPT %0"    /* Store current cpu timer value */
-                      : "=m" (S390_lowcore.last_update_timer) );
+        S390_lowcore.last_update_timer = get_vtimer();
         S390_lowcore.system_timer += timer - S390_lowcore.last_update_timer;
 
         system = S390_lowcore.system_timer - ti->system_timer;
         S390_lowcore.steal_timer -= system;
         ti->system_timer = S390_lowcore.system_timer;
-        if (in_irq() || idle_task(smp_processor_id()) != current)
-                account_system_time(tsk, 0, system, system);
-        else
-                account_idle_time(system);
+        account_system_time(tsk, 0, system, system);
 }
 EXPORT_SYMBOL_GPL(account_system_vtime);
 
-static inline void set_vtimer(__u64 expires)
+void vtime_start_cpu(void)
 {
+        struct s390_idle_data *idle = &__get_cpu_var(s390_idle);
         struct vtimer_queue *vq = &__get_cpu_var(virt_cpu_timer);
-        __u64 timer;
-
-        asm volatile ("  STPT %0\n"  /* Store current cpu timer value */
-                      "  SPT %1"     /* Set new value immediatly afterwards */
-                      : "=m" (timer) : "m" (expires) );
-        S390_lowcore.system_timer += S390_lowcore.last_update_timer - timer;
-        S390_lowcore.last_update_timer = expires;
-
-        /* store expire time for this CPU timer */
-        vq->to_expire = expires;
-}
-
-void vtime_start_cpu_timer(void)
-{
-        struct vtimer_queue *vt_list;
-
-        vt_list = &__get_cpu_var(virt_cpu_timer);
-
-        /* CPU timer interrupt is pending, don't reprogramm it */
-        if (vt_list->idle & 1LL<<63)
-                return;
+        __u64 idle_time, expires;
+
+        /* Account time spent with enabled wait psw loaded as idle time. */
+        idle_time = S390_lowcore.int_clock - idle->idle_enter;
+        account_idle_time(idle_time);
+        S390_lowcore.last_update_clock = S390_lowcore.int_clock;
+
+        /* Account system time spent going idle. */
+        S390_lowcore.system_timer += S390_lowcore.last_update_timer - vq->idle;
+        S390_lowcore.last_update_timer = S390_lowcore.async_enter_timer;
+
+        /* Restart vtime CPU timer */
+        if (vq->do_spt) {
+                /* Program old expire value but first save progress. */
+                expires = vq->idle - S390_lowcore.async_enter_timer;
+                expires += get_vtimer();
+                set_vtimer(expires);
+        } else {
+                /* Don't account the CPU timer delta while the cpu was idle. */
+                vq->elapsed -= vq->idle - S390_lowcore.async_enter_timer;
+        }
 
-        if (!list_empty(&vt_list->list))
-                set_vtimer(vt_list->idle);
+        spin_lock(&idle->lock);
+        idle->idle_time += idle_time;
+        idle->idle_enter = 0ULL;
+        idle->idle_count++;
+        spin_unlock(&idle->lock);
 }
 
-void vtime_stop_cpu_timer(void)
+void vtime_stop_cpu(void)
 {
-        struct vtimer_queue *vt_list;
-
-        vt_list = &__get_cpu_var(virt_cpu_timer);
-
-        /* nothing to do */
-        if (list_empty(&vt_list->list)) {
-                vt_list->idle = VTIMER_MAX_SLICE;
-                goto fire;
+        struct s390_idle_data *idle = &__get_cpu_var(s390_idle);
+        struct vtimer_queue *vq = &__get_cpu_var(virt_cpu_timer);
+        psw_t psw;
+
+        /* Wait for external, I/O or machine check interrupt. */
+        psw.mask = psw_kernel_bits | PSW_MASK_WAIT | PSW_MASK_IO | PSW_MASK_EXT;
+
+        /* Check if the CPU timer needs to be reprogrammed. */
+        if (vq->do_spt) {
+                __u64 vmax = VTIMER_MAX_SLICE;
+                /*
+                 * The inline assembly is equivalent to
+                 *      vq->idle = get_cpu_timer();
+                 *      set_cpu_timer(VTIMER_MAX_SLICE);
+                 *      idle->idle_enter = get_clock();
+                 *      __load_psw_mask(psw_kernel_bits | PSW_MASK_WAIT |
+                 *                         PSW_MASK_IO | PSW_MASK_EXT);
+                 * The difference is that the inline assembly makes sure that
+                 * the last three instruction are stpt, stck and lpsw in that
+                 * order. This is done to increase the precision.
+                 */
+                asm volatile(
+#ifndef CONFIG_64BIT
+                        "       basr    1,0\n"
+                        "0:     ahi     1,1f-0b\n"
+                        "       st      1,4(%2)\n"
+#else /* CONFIG_64BIT */
+                        "       larl    1,1f\n"
+                        "       stg     1,8(%2)\n"
+#endif /* CONFIG_64BIT */
+                        "       stpt    0(%4)\n"
+                        "       spt     0(%5)\n"
+                        "       stck    0(%3)\n"
+#ifndef CONFIG_64BIT
+                        "       lpsw    0(%2)\n"
+#else /* CONFIG_64BIT */
+                        "       lpswe   0(%2)\n"
+#endif /* CONFIG_64BIT */
+                        "1:"
+                        : "=m" (idle->idle_enter), "=m" (vq->idle)
+                        : "a" (&psw), "a" (&idle->idle_enter),
+                          "a" (&vq->idle), "a" (&vmax), "m" (vmax), "m" (psw)
+                        : "memory", "cc", "1");
+        } else {
+                /*
+                 * The inline assembly is equivalent to
+                 *      vq->idle = get_cpu_timer();
+                 *      idle->idle_enter = get_clock();
+                 *      __load_psw_mask(psw_kernel_bits | PSW_MASK_WAIT |
+                 *                         PSW_MASK_IO | PSW_MASK_EXT);
+                 * The difference is that the inline assembly makes sure that
+                 * the last three instruction are stpt, stck and lpsw in that
+                 * order. This is done to increase the precision.
+                 */
+                asm volatile(
+#ifndef CONFIG_64BIT
+                        "       basr    1,0\n"
+                        "0:     ahi     1,1f-0b\n"
+                        "       st      1,4(%2)\n"
+#else /* CONFIG_64BIT */
+                        "       larl    1,1f\n"
+                        "       stg     1,8(%2)\n"
+#endif /* CONFIG_64BIT */
+                        "       stpt    0(%4)\n"
+                        "       stck    0(%3)\n"
+#ifndef CONFIG_64BIT
+                        "       lpsw    0(%2)\n"
+#else /* CONFIG_64BIT */
+                        "       lpswe   0(%2)\n"
+#endif /* CONFIG_64BIT */
+                        "1:"
+                        : "=m" (idle->idle_enter), "=m" (vq->idle)
+                        : "a" (&psw), "a" (&idle->idle_enter),
+                          "a" (&vq->idle), "m" (psw)
+                        : "memory", "cc", "1");
         }
-
-        /* store the actual expire value */
-        asm volatile ("STPT %0" : "=m" (vt_list->idle));
-
-        /*
-         * If the CPU timer is negative we don't reprogramm
-         * it because we will get instantly an interrupt.
-         */
-        if (vt_list->idle & 1LL<<63)
-                return;
-
-        vt_list->offset += vt_list->to_expire - vt_list->idle;
-
-        /*
-         * We cannot halt the CPU timer, we just write a value that
-         * nearly never expires (only after 71 years) and re-write
-         * the stored expire value if we continue the timer
-         */
- fire:
-        set_vtimer(VTIMER_MAX_SLICE);
 }
 
 /*
@@ -195,30 +261,23 @@ static void list_add_sorted(struct vtimer_list *timer, struct list_head *head)
  */
 static void do_callbacks(struct list_head *cb_list)
 {
-        struct vtimer_queue *vt_list;
+        struct vtimer_queue *vq;
         struct vtimer_list *event, *tmp;
-        void (*fn)(unsigned long);
-        unsigned long data;
 
         if (list_empty(cb_list))
                 return;
 
-        vt_list = &__get_cpu_var(virt_cpu_timer);
+        vq = &__get_cpu_var(virt_cpu_timer);
 
         list_for_each_entry_safe(event, tmp, cb_list, entry) {
-                fn = event->function;
-                data = event->data;
-                fn(data);
-
-                if (!event->interval)
-                        /* delete one shot timer */
-                        list_del_init(&event->entry);
-                else {
-                        /* move interval timer back to list */
-                        spin_lock(&vt_list->lock);
-                        list_del_init(&event->entry);
-                        list_add_sorted(event, &vt_list->list);
-                        spin_unlock(&vt_list->lock);
+                list_del_init(&event->entry);
+                (event->function)(event->data);
+                if (event->interval) {
+                        /* Recharge interval timer */
+                        event->expires = event->interval + vq->elapsed;
+                        spin_lock(&vq->lock);
+                        list_add_sorted(event, &vq->list);
+                        spin_unlock(&vq->lock);
                 }
         }
 }
@@ -228,64 +287,57 @@ static void do_callbacks(struct list_head *cb_list)
  */
 static void do_cpu_timer_interrupt(__u16 error_code)
 {
-        __u64 next, delta;
-        struct vtimer_queue *vt_list;
+        struct vtimer_queue *vq;
         struct vtimer_list *event, *tmp;
-        struct list_head *ptr;
-        /* the callback queue */
-        struct list_head cb_list;
+        struct list_head cb_list;       /* the callback queue */
+        __u64 elapsed, next;
 
         INIT_LIST_HEAD(&cb_list);
-        vt_list = &__get_cpu_var(virt_cpu_timer);
+        vq = &__get_cpu_var(virt_cpu_timer);
 
         /* walk timer list, fire all expired events */
-        spin_lock(&vt_list->lock);
-
-        if (vt_list->to_expire < VTIMER_MAX_SLICE)
-                vt_list->offset += vt_list->to_expire;
-
-        list_for_each_entry_safe(event, tmp, &vt_list->list, entry) {
-                if (event->expires > vt_list->offset)
-                        /* found first unexpired event, leave */
-                        break;
-
-                /* re-charge interval timer, we have to add the offset */
-                if (event->interval)
-                        event->expires = event->interval + vt_list->offset;
-
-                /* move expired timer to the callback queue */
-                list_move_tail(&event->entry, &cb_list);
+        spin_lock(&vq->lock);
+
+        elapsed = vq->elapsed + (vq->timer - S390_lowcore.async_enter_timer);
+        BUG_ON((s64) elapsed < 0);
+        vq->elapsed = 0;
+        list_for_each_entry_safe(event, tmp, &vq->list, entry) {
+                if (event->expires < elapsed)
+                        /* move expired timer to the callback queue */
+                        list_move_tail(&event->entry, &cb_list);
+                else
+                        event->expires -= elapsed;
         }
-        spin_unlock(&vt_list->lock);
+        spin_unlock(&vq->lock);
+
+        vq->do_spt = list_empty(&cb_list);
         do_callbacks(&cb_list);
 
         /* next event is first in list */
-        spin_lock(&vt_list->lock);
-        if (!list_empty(&vt_list->list)) {
-                ptr = vt_list->list.next;
-                event = list_entry(ptr, struct vtimer_list, entry);
-                next = event->expires - vt_list->offset;
-
-                /* add the expired time from this interrupt handler
-                 * and the callback functions
-                 */
-                asm volatile ("STPT %0" : "=m" (delta));
-                delta = 0xffffffffffffffffLL - delta + 1;
-                vt_list->offset += delta;
-                next -= delta;
-        } else {
-                vt_list->offset = 0;
-                next = VTIMER_MAX_SLICE;
-        }
-        spin_unlock(&vt_list->lock);
-        set_vtimer(next);
+        next = VTIMER_MAX_SLICE;
+        spin_lock(&vq->lock);
+        if (!list_empty(&vq->list)) {
+                event = list_first_entry(&vq->list, struct vtimer_list, entry);
+                next = event->expires;
+        } else
+                vq->do_spt = 0;
+        spin_unlock(&vq->lock);
+        /*
+         * To improve precision add the time spent by the
+         * interrupt handler to the elapsed time.
+         * Note: CPU timer counts down and we got an interrupt,
+         *       the current content is negative
+         */
+        elapsed = S390_lowcore.async_enter_timer - get_vtimer();
+        set_vtimer(next - elapsed);
+        vq->timer = next - elapsed;
+        vq->elapsed = elapsed;
 }
 
 void init_virt_timer(struct vtimer_list *timer)
 {
         timer->function = NULL;
         INIT_LIST_HEAD(&timer->entry);
-        spin_lock_init(&timer->lock);
 }
 EXPORT_SYMBOL(init_virt_timer);
 
@@ -299,44 +351,40 @@ static inline int vtimer_pending(struct vtimer_list *timer)
  */
 static void internal_add_vtimer(struct vtimer_list *timer)
 {
+        struct vtimer_queue *vq;
         unsigned long flags;
-        __u64 done;
-        struct vtimer_list *event;
-        struct vtimer_queue *vt_list;
+        __u64 left, expires;
 
-        vt_list = &per_cpu(virt_cpu_timer, timer->cpu);
-        spin_lock_irqsave(&vt_list->lock, flags);
+        vq = &per_cpu(virt_cpu_timer, timer->cpu);
+        spin_lock_irqsave(&vq->lock, flags);
 
         BUG_ON(timer->cpu != smp_processor_id());
 
-        /* if list is empty we only have to set the timer */
-        if (list_empty(&vt_list->list)) {
-                /* reset the offset, this may happen if the last timer was
-                 * just deleted by mod_virt_timer and the interrupt
-                 * didn't happen until here
-                 */
-                vt_list->offset = 0;
-                goto fire;
+        if (list_empty(&vq->list)) {
+                /* First timer on this cpu, just program it. */
+                list_add(&timer->entry, &vq->list);
+                set_vtimer(timer->expires);
+                vq->timer = timer->expires;
+                vq->elapsed = 0;
+        } else {
+                /* Check progress of old timers. */
+                expires = timer->expires;
+                left = get_vtimer();
+                if (likely((s64) expires < (s64) left)) {
+                        /* The new timer expires before the current timer. */
+                        set_vtimer(expires);
+                        vq->elapsed += vq->timer - left;
+                        vq->timer = expires;
+                } else {
+                        vq->elapsed += vq->timer - left;
+                        vq->timer = left;
+                }
+                /* Insert new timer into per cpu list. */
+                timer->expires += vq->elapsed;
+                list_add_sorted(timer, &vq->list);
         }
 
-        /* save progress */
-        asm volatile ("STPT %0" : "=m" (done));
-
-        /* calculate completed work */
-        done = vt_list->to_expire - done + vt_list->offset;
-        vt_list->offset = 0;
-
-        list_for_each_entry(event, &vt_list->list, entry)
-                event->expires -= done;
-
- fire:
-        list_add_sorted(timer, &vt_list->list);
-
-        /* get first element, which is the next vtimer slice */
-        event = list_entry(vt_list->list.next, struct vtimer_list, entry);
-
-        set_vtimer(event->expires);
-        spin_unlock_irqrestore(&vt_list->lock, flags);
+        spin_unlock_irqrestore(&vq->lock, flags);
         /* release CPU acquired in prepare_vtimer or mod_virt_timer() */
         put_cpu();
 }
@@ -381,14 +429,15 @@ EXPORT_SYMBOL(add_virt_timer_periodic);
  * If we change a pending timer the function must be called on the CPU
  * where the timer is running on, e.g. by smp_call_function_single()
  *
- * The original mod_timer adds the timer if it is not pending. For compatibility
- * we do the same. The timer will be added on the current CPU as a oneshot timer.
+ * The original mod_timer adds the timer if it is not pending. For
+ * compatibility we do the same. The timer will be added on the current
+ * CPU as a oneshot timer.
  *
  * returns whether it has modified a pending timer (1) or not (0)
  */
 int mod_virt_timer(struct vtimer_list *timer, __u64 expires)
 {
-        struct vtimer_queue *vt_list;
+        struct vtimer_queue *vq;
         unsigned long flags;
         int cpu;
 
@@ -404,17 +453,17 @@ int mod_virt_timer(struct vtimer_list *timer, __u64 expires)
                 return 1;
 
         cpu = get_cpu();
-        vt_list = &per_cpu(virt_cpu_timer, cpu);
+        vq = &per_cpu(virt_cpu_timer, cpu);
 
         /* check if we run on the right CPU */
         BUG_ON(timer->cpu != cpu);
 
         /* disable interrupts before test if timer is pending */
-        spin_lock_irqsave(&vt_list->lock, flags);
+        spin_lock_irqsave(&vq->lock, flags);
 
         /* if timer isn't pending add it on the current CPU */
         if (!vtimer_pending(timer)) {
-                spin_unlock_irqrestore(&vt_list->lock, flags);
+                spin_unlock_irqrestore(&vq->lock, flags);
                 /* we do not activate an interval timer with mod_virt_timer */
                 timer->interval = 0;
                 timer->expires = expires;
@@ -431,7 +480,7 @@ int mod_virt_timer(struct vtimer_list *timer, __u64 expires)
                 timer->interval = expires;
 
         /* the timer can't expire anymore so we can release the lock */
-        spin_unlock_irqrestore(&vt_list->lock, flags);
+        spin_unlock_irqrestore(&vq->lock, flags);
         internal_add_vtimer(timer);
         return 1;
 }
@@ -445,25 +494,19 @@ EXPORT_SYMBOL(mod_virt_timer);
 int del_virt_timer(struct vtimer_list *timer)
 {
         unsigned long flags;
-        struct vtimer_queue *vt_list;
+        struct vtimer_queue *vq;
 
         /* check if timer is pending */
         if (!vtimer_pending(timer))
                 return 0;
 
-        vt_list = &per_cpu(virt_cpu_timer, timer->cpu);
-        spin_lock_irqsave(&vt_list->lock, flags);
+        vq = &per_cpu(virt_cpu_timer, timer->cpu);
+        spin_lock_irqsave(&vq->lock, flags);
 
         /* we don't interrupt a running timer, just let it expire! */
         list_del_init(&timer->entry);
 
-        /* last timer removed */
-        if (list_empty(&vt_list->list)) {
-                vt_list->to_expire = 0;
-                vt_list->offset = 0;
-        }
-
-        spin_unlock_irqrestore(&vt_list->lock, flags);
+        spin_unlock_irqrestore(&vq->lock, flags);
         return 1;
 }
 EXPORT_SYMBOL(del_virt_timer);
@@ -473,24 +516,19 @@ EXPORT_SYMBOL(del_virt_timer);
  */
 void init_cpu_vtimer(void)
 {
-        struct vtimer_queue *vt_list;
+        struct vtimer_queue *vq;
 
         /* kick the virtual timer */
-        S390_lowcore.exit_timer = VTIMER_MAX_SLICE;
-        S390_lowcore.last_update_timer = VTIMER_MAX_SLICE;
         asm volatile ("STCK %0" : "=m" (S390_lowcore.last_update_clock));
-        asm volatile ("SPT %0" : : "m" (S390_lowcore.last_update_timer));
+        asm volatile ("STPT %0" : "=m" (S390_lowcore.last_update_timer));
+
+        /* initialize per cpu vtimer structure */
+        vq = &__get_cpu_var(virt_cpu_timer);
+        INIT_LIST_HEAD(&vq->list);
+        spin_lock_init(&vq->lock);
 
         /* enable cpu timer interrupts */
         __ctl_set_bit(0,10);
-
-        vt_list = &__get_cpu_var(virt_cpu_timer);
-        INIT_LIST_HEAD(&vt_list->list);
-        spin_lock_init(&vt_list->lock);
-        vt_list->to_expire = 0;
-        vt_list->offset = 0;
-        vt_list->idle = 0;
-
 }
 
 void __init vtime_init(void)
diff --git a/drivers/s390/cio/cio.c b/drivers/s390/cio/cio.c
index 8a8df7552969..06b71823f399 100644
--- a/drivers/s390/cio/cio.c
+++ b/drivers/s390/cio/cio.c
@@ -632,8 +632,8 @@ do_IRQ (struct pt_regs *regs)
         struct pt_regs *old_regs;
 
         old_regs = set_irq_regs(regs);
-        irq_enter();
         s390_idle_check();
+        irq_enter();
         if (S390_lowcore.int_clock >= S390_lowcore.clock_comparator)
                 /* Serve timer interrupts first. */
                 clock_comparator_work();
diff --git a/drivers/s390/s390mach.c b/drivers/s390/s390mach.c
index 834e9ee7e934..92b0417f8e12 100644
--- a/drivers/s390/s390mach.c
+++ b/drivers/s390/s390mach.c
@@ -18,6 +18,7 @@
 #include <asm/etr.h>
 #include <asm/lowcore.h>
 #include <asm/cio.h>
+#include <asm/cpu.h>
 #include "s390mach.h"
 
 static struct semaphore m_sem;
@@ -369,6 +370,8 @@ s390_do_machine_check(struct pt_regs *regs)
 
         lockdep_off();
 
+        s390_idle_check();
+
         mci = (struct mci *) &S390_lowcore.mcck_interruption_code;
         mcck = &__get_cpu_var(cpu_mcck);
         umode = user_mode(regs);