stg     %r3,__SF_EMPTY(%r15)
        larl    %r1,.Lpsw_idle_lpsw+4
        stg     %r1,__SF_EMPTY+8(%r15)
+#ifdef CONFIG_SMP
+       larl    %r1,smp_cpu_mtid
+       llgf    %r1,0(%r1)
+       ltgr    %r1,%r1
+       jz      .Lpsw_idle_stcctm
+       .insn   rsy,0xeb0000000017,%r1,5,__SF_EMPTY+16(%r15)
+.Lpsw_idle_stcctm:
+#endif
        STCK    __CLOCK_IDLE_ENTER(%r2)
        stpt    __TIMER_IDLE_ENTER(%r2)
 .Lpsw_idle_lpsw:
        jhe     1f
        mvc     __CLOCK_IDLE_ENTER(8,%r2),__CLOCK_IDLE_EXIT(%r2)
        mvc     __TIMER_IDLE_ENTER(8,%r2),__TIMER_IDLE_EXIT(%r2)
-1:     # account system time going idle
+1:     # calculate idle cycles
+#ifdef CONFIG_SMP
+       clg     %r9,BASED(.Lcleanup_idle_insn)
+       jl      3f
+       larl    %r1,smp_cpu_mtid
+       llgf    %r1,0(%r1)
+       ltgr    %r1,%r1
+       jz      3f
+       .insn   rsy,0xeb0000000017,%r1,5,__SF_EMPTY+80(%r15)
+       larl    %r3,mt_cycles
+       ag      %r3,__LC_PERCPU_OFFSET
+       la      %r4,__SF_EMPTY+16(%r15)
+2:     lg      %r0,0(%r3)
+       slg     %r0,0(%r4)
+       alg     %r0,64(%r4)
+       stg     %r0,0(%r3)
+       la      %r3,8(%r3)
+       la      %r4,8(%r4)
+       brct    %r1,2b
+#endif
+3:     # account system time going idle
        lg      %r9,__LC_STEAL_TIMER
        alg     %r9,__CLOCK_IDLE_ENTER(%r2)
        slg     %r9,__LC_LAST_UPDATE_CLOCK
 
 static atomic64_t virt_timer_current;
 static atomic64_t virt_timer_elapsed;
 
-static DEFINE_PER_CPU(u64, mt_cycles[32]);
+DEFINE_PER_CPU(u64, mt_cycles[8]);
 static DEFINE_PER_CPU(u64, mt_scaling_mult) = { 1 };
 static DEFINE_PER_CPU(u64, mt_scaling_div) = { 1 };
 static DEFINE_PER_CPU(u64, mt_scaling_jiffies);
        return elapsed >= atomic64_read(&virt_timer_current);
 }
 
+static void update_mt_scaling(void)
+{
+       u64 cycles_new[8], *cycles_old;
+       u64 delta, fac, mult, div;
+       int i;
+
+       stcctm5(smp_cpu_mtid + 1, cycles_new);
+       cycles_old = this_cpu_ptr(mt_cycles);
+       fac = 1;
+       mult = div = 0;
+       for (i = 0; i <= smp_cpu_mtid; i++) {
+               delta = cycles_new[i] - cycles_old[i];
+               div += delta;
+               mult *= i + 1;
+               mult += delta * fac;
+               fac *= i + 1;
+       }
+       div *= fac;
+       if (div > 0) {
+               /* Update scaling factor */
+               __this_cpu_write(mt_scaling_mult, mult);
+               __this_cpu_write(mt_scaling_div, div);
+               memcpy(cycles_old, cycles_new,
+                      sizeof(u64) * (smp_cpu_mtid + 1));
+       }
+       __this_cpu_write(mt_scaling_jiffies, jiffies_64);
+}
+
 /*
  * Update process times based on virtual cpu times stored by entry.S
  * to the lowcore fields user_timer, system_timer & steal_clock.
        struct thread_info *ti = task_thread_info(tsk);
        u64 timer, clock, user, system, steal;
        u64 user_scaled, system_scaled;
-       int i;
 
        timer = S390_lowcore.last_update_timer;
        clock = S390_lowcore.last_update_clock;
        S390_lowcore.system_timer += timer - S390_lowcore.last_update_timer;
        S390_lowcore.steal_timer += S390_lowcore.last_update_clock - clock;
 
-       /* Do MT utilization calculation */
+       /* Update MT utilization calculation */
        if (smp_cpu_mtid &&
-           time_after64(jiffies_64, __this_cpu_read(mt_scaling_jiffies))) {
-               u64 cycles_new[32], *cycles_old;
-               u64 delta, fac, mult, div;
-
-               cycles_old = this_cpu_ptr(mt_cycles);
-               if (stcctm5(smp_cpu_mtid + 1, cycles_new) < 2) {
-                       fac = 1;
-                       mult = div = 0;
-                       for (i = 0; i <= smp_cpu_mtid; i++) {
-                               delta = cycles_new[i] - cycles_old[i];
-                               div += delta;
-                               mult *= i + 1;
-                               mult += delta * fac;
-                               fac *= i + 1;
-                       }
-                       div *= fac;
-                       if (div > 0) {
-                               /* Update scaling factor */
-                               __this_cpu_write(mt_scaling_mult, mult);
-                               __this_cpu_write(mt_scaling_div, div);
-                               memcpy(cycles_old, cycles_new,
-                                      sizeof(u64) * (smp_cpu_mtid + 1));
-                       }
-               }
-               __this_cpu_write(mt_scaling_jiffies, jiffies_64);
-       }
+           time_after64(jiffies_64, this_cpu_read(mt_scaling_jiffies)))
+               update_mt_scaling();
 
        user = S390_lowcore.user_timer - ti->user_timer;
        S390_lowcore.steal_timer -= user;
        S390_lowcore.last_update_timer = get_vtimer();
        S390_lowcore.system_timer += timer - S390_lowcore.last_update_timer;
 
+       /* Update MT utilization calculation */
+       if (smp_cpu_mtid &&
+           time_after64(jiffies_64, this_cpu_read(mt_scaling_jiffies)))
+               update_mt_scaling();
+
        system = S390_lowcore.system_timer - ti->system_timer;
        S390_lowcore.steal_timer -= system;
        ti->system_timer = S390_lowcore.system_timer;