/* 
 * this a a copy of 
 *  tools/testing/selftests/vDSO/parse_vdso.c
 * from the Linux kernel tree, with a little main() function at the end
 * which uses the new '__vdso_linux_tsc_calibration' function to get a pointer to
 * the kernel maintained clocksource mult and shift values in the vsyscall_gtod_data : 
 */
#include <stdbool.h>
#include <stdint.h>
#include <limits.h>
#include <time.h>
#include <sys/auxv.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>

#ifndef  PATH_TO_PARSE_VDSO_C
#error   Please set compilation flag -DPATH_TO_PARSE_VDSO_C="..." .
#endif

#include PATH_TO_PARSE_VDSO_C

typedef
struct lnx_tsc_calibration_s
{ unsigned int mult, shift;
} LnxTSCCalibration_t;

typedef
struct tsc_buffer_s
{ unsigned long tsc, sec, nsec;
} LnxTSCBuffer_t;

void clock_get_time_raw( register struct timespec *ts , register const LnxTSCCalibration_t * tsc_cal, register LnxTSCBuffer_t *last )
{ volatile unsigned int  tsc_lo=0, tsc_hi=0, tsc_cpu=0; // so same instrs generated for 64-bit as for 32-bit builds
  register unsigned long tsc=0; 
  asm volatile
  ( "rdtscp"
  : "=a" (tsc_lo)
  , "=d" (tsc_hi)
  , "=c" (tsc_cpu)
  ); // : eax, edx, ecx used - NOT rax, rdx, rcx
  tsc     = ((((unsigned long)tsc_hi) & 0xffffffffUL) << 32) | (((unsigned long)tsc_lo) & 0xffffffffUL);
  tsc    *= tsc_cal->mult;
  tsc   >>= tsc_cal->shift;
  if ( last && last->tsc )
  { register unsigned long tsc_diff = tsc - last->tsc;
    if ( (tsc_diff + last->nsec) > 999999999UL )
    { ts->tv_sec  = tsc / 1000000000;      
      ts->tv_nsec = tsc % 1000000000;          
    }else
    { ts->tv_sec  = last->sec;
      ts->tv_nsec = tsc_diff + last->nsec;
    }
    last->tsc     = tsc;
    last->sec     = ts->tv_sec;
    last->nsec    = ts->tv_nsec;    
  }else
  { ts->tv_sec    = tsc / 1000000000;      
    ts->tv_nsec   = tsc % 1000000000;
    if(last)
    { last->tsc   = tsc;
      last->sec   = ts->tv_sec;
      last->nsec  = ts->tv_nsec;    
    }
  }
}

#ifndef N_SAMPLES
#define N_SAMPLES 100
#endif

int main( int argc, const char *const* argv , const char **const envp )
{
  register unsigned long sysinfo_ehdr = getauxval( AT_SYSINFO_EHDR );
  if( 0 == sysinfo_ehdr )
  { fprintf(stderr,"getauxval failed: %d : '%s'.\n", errno, strerror(errno));
    return 1;
  }
  vdso_init_from_sysinfo_ehdr( sysinfo_ehdr );
  if ( ! vdso_info.valid )
  { fprintf(stderr,"vdso_init_from_sysinfo_ehdr failed\n");
    return 1;  
  }  
  const struct lnx_tsc_calibration_s*
    (*linux_tsc_cal)(void) = vdso_sym("LINUX_2.6", "__vdso_linux_tsc_calibration");
  if( linux_tsc_cal == 0UL )
  { fprintf(stderr,"vdso_sym failed\n");
    return 1;
  }
  const struct lnx_tsc_calibration_s *clock_source = (*linux_tsc_cal)();
  fprintf(stderr,"Got TSC calibration @ %p: mult: %u shift: %u\n",
          (void*)clock_source, clock_source->mult, clock_source->shift
         );
#define TS2NS(_TS_) ((((unsigned long long)(_TS_).tv_sec)*1000000000ULL) + (((unsigned long long)((_TS_).tv_nsec))))     
  struct timespec t_s={0,0};
  LnxTSCBuffer_t  last_tsc = {0,0,0};
  clock_get_time_raw( &t_s, clock_source, &last_tsc);
  unsigned long long
    sample [ N_SAMPLES ]
  , t1, t2
  , t_start = TS2NS(t_s);
  unsigned int s=0;
  do
  { clock_get_time_raw( &t_s, clock_source, &last_tsc);
    t1 = TS2NS(t_s);
    clock_get_time_raw( &t_s, clock_source, &last_tsc);
    t2 = TS2NS(t_s);
    sample [ s ] = t2 - t1;         
  }while(++s < N_SAMPLES);
  unsigned long long sum = 0;
  for(s = 0; s < N_SAMPLES; s+=1)
    sum += sample[s];
  fprintf(stderr,"sum: %llu\n",sum);
  unsigned long long avg_ns = sum / N_SAMPLES;
  t1=(t2 - t_start);
  fprintf(stderr,
	  "Total time: %1.1llu.%9.9lluS - Average Latency: %1.1llu.%9.9lluS\n",
          t1/1000000000,       t1-((t1/1000000000)*1000000000),
          avg_ns/1000000000,   avg_ns-((avg_ns/1000000000)*1000000000)   
	  );	  
  return 0;
}