1.1 --- a/xen/arch/x86/acpi/cpu_idle.c	Thu May 01 10:49:38 2008 +0100
     1.2 +++ b/xen/arch/x86/acpi/cpu_idle.c	Thu May 01 10:50:09 2008 +0100
     1.3 @@ -442,8 +442,8 @@ static void acpi_processor_idle(void)
     1.4           * stopped by H/W. Without carefully handling of TSC/APIC stop issues,
     1.5           * deep C state can't work correctly.
     1.6           */
     1.7 -        /* placeholder for preparing TSC stop */
     1.8 -
     1.9 +        /* preparing TSC stop */
    1.10 +        cstate_save_tsc();
    1.11          /* placeholder for preparing APIC stop */
    1.12  
    1.13          /* Invoke C3 */
    1.14 @@ -451,7 +451,8 @@ static void acpi_processor_idle(void)
    1.15  
    1.16          /* placeholder for recovering APIC */
    1.17  
    1.18 -        /* placeholder for recovering TSC */
    1.19 +        /* recovering TSC */
    1.20 +        cstate_restore_tsc();
    1.21  
    1.22          /* Get end time (ticks) */
    1.23          t2 = inl(pmtmr_ioport);

     2.1 --- a/xen/arch/x86/time.c	Thu May 01 10:49:38 2008 +0100
     2.2 +++ b/xen/arch/x86/time.c	Thu May 01 10:50:09 2008 +0100
     2.3 @@ -51,9 +51,11 @@ struct time_scale {
     2.4  
     2.5  struct cpu_time {
     2.6      u64 local_tsc_stamp;
     2.7 +    u64 cstate_tsc_stamp;
     2.8      s_time_t stime_local_stamp;
     2.9      s_time_t stime_master_stamp;
    2.10      struct time_scale tsc_scale;
    2.11 +    u32 cstate_plt_count_stamp;
    2.12      struct timer calibration_timer;
    2.13  };
    2.14  
    2.15 @@ -66,6 +68,8 @@ struct platform_timesource {
    2.16  
    2.17  static DEFINE_PER_CPU(struct cpu_time, cpu_time);
    2.18  
    2.19 +static u8 tsc_invariant=0;  /* TSC is invariant upon C state entry */
    2.20 +
    2.21  /*
    2.22   * We simulate a 32-bit platform timer from the 16-bit PIT ch2 counter.
    2.23   * Otherwise overflow happens too quickly (~50ms) for us to guarantee that
    2.24 @@ -594,6 +598,36 @@ static void init_platform_timer(void)
    2.25             freq_string(pts->frequency), pts->name);
    2.26  }
    2.27  
    2.28 +void cstate_save_tsc(void)
    2.29 +{
    2.30 +    struct cpu_time *t = &this_cpu(cpu_time);
    2.31 +
    2.32 +    if (!tsc_invariant){
    2.33 +        t->cstate_plt_count_stamp = plt_src.read_counter();
    2.34 +        rdtscll(t->cstate_tsc_stamp);
    2.35 +    }
    2.36 +}
    2.37 +
    2.38 +void cstate_restore_tsc(void)
    2.39 +{
    2.40 +    struct cpu_time *t;
    2.41 +    u32    plt_count_delta;
    2.42 +    u64    tsc_delta;
    2.43 +
    2.44 +    if (!tsc_invariant){
    2.45 +        t = &this_cpu(cpu_time);
    2.46 +
    2.47 +        /* if platform counter overflow happens, interrupt will bring CPU from
    2.48 +           C state to working state, so the platform counter won't wrap the
    2.49 +           cstate_plt_count_stamp, and the 32 bit unsigned platform counter
    2.50 +           is enough for delta calculation
    2.51 +         */
    2.52 +        plt_count_delta = 
    2.53 +            (plt_src.read_counter() - t->cstate_plt_count_stamp) & plt_mask;
    2.54 +        tsc_delta = scale_delta(plt_count_delta, &plt_scale)*cpu_khz/1000000UL;
    2.55 +        wrmsrl(MSR_IA32_TSC,  t->cstate_tsc_stamp + tsc_delta);
    2.56 +    }
    2.57 +}
    2.58  
    2.59  /***************************************************************************
    2.60   * CMOS Timer functions
    2.61 @@ -973,6 +1007,11 @@ int __init init_xen_time(void)
    2.62      stime_platform_stamp = 0;
    2.63      init_platform_timer();
    2.64  
    2.65 +    /* check if TSC is invariant during deep C state
    2.66 +       this is a new feature introduced by Nehalem*/
    2.67 +    if ( cpuid_edx(0x80000007) & (1U<<8) )
    2.68 +        tsc_invariant = 1;
    2.69 +
    2.70      local_irq_enable();
    2.71  
    2.72      return 0;

     3.1 --- a/xen/include/xen/time.h	Thu May 01 10:49:38 2008 +0100
     3.2 +++ b/xen/include/xen/time.h	Thu May 01 10:50:09 2008 +0100
     3.3 @@ -13,6 +13,8 @@
     3.4  #include <asm/time.h>
     3.5  
     3.6  extern int init_xen_time(void);
     3.7 +extern void cstate_save_tsc(void);
     3.8 +extern void cstate_restore_tsc(void);
     3.9  
    3.10  extern unsigned long cpu_khz;
    3.11