/*
 * rtl_time.c
 *
 * architecture-dependent clock support
 *
 * Written by Michael Barabanov
 * Copyright  Finite State Machine Labs Inc. 1998-1999
 * Released under the terms of the GPL Version 2
 *
 * StandAlone RTLinux integration 
 * written by Vicente Esteve LLoret <viesllo@inf.upv.es> 
 * Copyright (C) May, 2003 OCERA Consortium.
 *
 *
 * 
 */

#include <rtl_conf.h>
#include <arch/hw_irq.h>
#include <arch/timer.h>
#include <arch/rtl_io.h>
#include <arch/mprot.h>

#include <rtl_core.h>
#include <rtl_debug.h>
#include <rtl_sync.h>
#include <rtl_time.h>

//        Structs and Vars
//        

#define rdtscll(val) \
	__asm__ __volatile__("rdtsc" : "=A" (val))

#define LATCH_CNT2() rtl_outb(0xd8,0x43);

#define READ_CNT2(var) \
do {var = rtl_inb(0x42); var |= (rtl_inb(0x42) << 8 );} while (0);

#define WRITE_COUNTER_ZERO16(x) do { \
	rtl_outb(x&0xff,0x40); rtl_outb((x>>8)&0xff,0x40);\
      	clock_counter =x; \
} while (0)

#define WRITE_COUNTER_ZERO_ONESHOT(x) WRITE_COUNTER_ZERO16(x)

#define wait_value(x) do {; } while ((rtl_inb(0x61) & 0x20) != (x))
#define wait_cycle() do { wait_value(0); wait_value(0x20); } while (0)

#define CLATCH (1024 * 32)
#define LATCH2 0x8000
#define NLOOPS 50
unsigned long scaler_8254_to_hrtime;
unsigned long scaler_hrtime_to_8254;
/* getting global time from Pentium TSC */
unsigned long scaler_pentium_to_hrtime = 0;
int can_change_latch2;


static volatile int last_c2;

/*static */spinlock_t lock8254;
struct rtl_clock _i8254_clock;
static hrtime_t (*rtl_do_get_time)(void);

static hrtime_t hrtime_resolution;

static long latch_ns;
static long max_latch_oneshot;

static unsigned int clock_counter; /* current latch value */

hrtime_t base_time;
hrtime_t last_8254_time;
long offset_time;


hrtime_t gethrtime(void)
{
  hrtime_t aux;
#if CONFIG_KERNEL_MEMORYPROT  
  mprot_t mprot;
#endif  
  
  STARTKERNELCODE(mprot); 
  aux = rtl_do_get_time();
  ENDKERNELCODE(mprot);
  return aux;
}

hrtime_t _gethrtime(struct rtl_clock *c)
{
  return gethrtime();
}

hrtime_t gethrtimeres(void)
{
  return hrtime_resolution;
}

hrtime_t pent_gettime(void)
{

	hrtime_t t;
	/* time = counter * scaler_pentium_to_hrtime / 2^32 * 2^5; */
	/* Why 2^5? Because the slowest Pentiums run at 60 MHz */

	__asm__("rdtsc\n\t"
		"mov %%edx, %%ecx\n\t"
		"mul %%ebx\n\t"  	/* multiply the low 32 bits of the counter by the scaler_pentium */
		"mov %%ecx, %%eax\n\t"
		"mov %%edx, %%ecx\n\t"	/* save the high 32 bits of the product */
		"mul %%ebx\n\t" 	/* now the high 32 bits of the counter */
		"add %%ecx, %%eax\n\t"
		"adc $0, %%edx\n\t"
#if HRTICKS_PER_SEC == NSECS_PER_SEC
		"shld $5, %%eax, %%edx\n\t"
		"shl $5, %%eax\n\t"
#endif
		:"=A" (t) : "b" (scaler_pentium_to_hrtime) : "cx");
	return t;
}



hrtime_t global_8254_gettime (void)
{
  register unsigned int c2;
  int flags;
  long t;
  
  rtl_spin_lock_irqsave(&lock8254, flags);

  LATCH_CNT2();
  READ_CNT2(c2);
  offset_time += ((c2 < last_c2) ? (last_c2 - c2) / 2 : (last_c2 - c2 + LATCH2) / 2);
  last_c2 = c2;
  if (offset_time >= CLOCK_TICK_RATE) {
    offset_time -= CLOCK_TICK_RATE;
    base_time += HRTICKS_PER_SEC;
  };

  
#if HRTICKS_PER_SEC != CLOCK_TICK_RATE
  __asm__("shl $10, %%eax\n\t"
          "mul %%ebx\n\t"
          :"=d" (t) : "b" (scaler_8254_to_hrtime), "a" (offset_time));
#else
  t = offset_time;
#endif
  
  last_8254_time = base_time + t;

  rtl_spin_unlock_irqrestore(&lock8254, flags);

  return last_8254_time;
}



static hrtime_t periodic_gethrtime (struct rtl_clock *c) 
{ 
  return c->value; 
}

static hrtime_t oneshot_gethrtime (struct rtl_clock *c) 
{ 
//  return gethrtime(); 
  return gethrtime();
}

/* the 8254 clock */



static unsigned int _8254_irq(unsigned int irq, struct pt_regs *regs)
{
	int flags;
	
	rtl_spin_lock_irqsave (&lock8254, flags);
	if (_i8254_clock.mode == RTL_CLOCK_MODE_PERIODIC) {
//		if (test_and_set_bit(0, &_i8254_clock.arch.count_irqs)) {
	  _i8254_clock.value += _i8254_clock.resolution;
//		}
} else {
	_i8254_clock.arch.istimerset = 0;
}

         rtl_spin_unlock_irqrestore (&lock8254, flags);
         _i8254_clock.handler(regs);

return 0;
}

static inline long RTIME_to_8254_ticks(long t)
{
#if HRTICKS_PER_SEC != CLOCK_TICK_RATE
int dummy;
__asm__("mull %2"
	:"=a" (dummy), "=d" (t)
	:"g" (scaler_hrtime_to_8254), "0" (t)
	);
#endif
	
return (t);
}


static int _8254_setperiodic (rtl_clockid_t c, hrtime_t interval)
{
long t;
int flags;

rtl_spin_lock_irqsave (&lock8254, flags);
t = RTIME_to_8254_ticks (interval) + 1;


WRITE_COUNTER_ZERO16 (t);

_i8254_clock.value = gethrtime();
_i8254_clock.resolution = interval;
_i8254_clock.arch.istimerset = 1;
rtl_spin_unlock_irqrestore(&lock8254, flags);

return 0;
}

static int _8254_setoneshot (rtl_clockid_t c, hrtime_t interval)
{
  rtl_irqstate_t flags;
  long t;

  rtl_spin_lock_irqsave (&lock8254, flags);

    
	
  if (interval > max_latch_oneshot) {
    interval = max_latch_oneshot;
  }
          
  t = RTIME_to_8254_ticks (interval); 
  if (t < 1) {
    t = 1;
  };
  WRITE_COUNTER_ZERO_ONESHOT(t);
  _i8254_clock.arch.istimerset = 1;
	
  rtl_spin_unlock_irqrestore(&lock8254, flags);
	
  return 0;
}



int _8254_settimermode (struct rtl_clock *c, int mode)
{

  if (mode == _i8254_clock.mode) 
  {
    return 0;
  }
  
  if (mode == RTL_CLOCK_MODE_PERIODIC) 
  {
    rtl_outb_p(0x30, 0x43);
    rtl_outb_p(0x34, 0x43);
    _i8254_clock.mode = mode;
    _i8254_clock.gethrtime = periodic_gethrtime;
    _i8254_clock.settimer = _8254_setperiodic;
    _i8254_clock.arch.count_irqs = 0;
  } 
  else if (mode == RTL_CLOCK_MODE_ONESHOT) {
    rtl_outb_p(0x30, 0x43);
    _i8254_clock.mode = mode;
    _i8254_clock.gethrtime = oneshot_gethrtime;
    _i8254_clock.settimer = _8254_setoneshot;
    _i8254_clock.resolution = HRTICKS_PER_SEC / CLOCK_TICK_RATE;
  } else {
    return -EINVAL;
  }
  return 0;
}

static int _8254_init (rtl_clockid_t clock)
{

  int flags;

  rtl_no_interrupts (flags);
  
  rtl_do_get_time = pent_gettime;

  rtl_request_global_irq(0, _8254_irq);

//  _8254_settimermode (clock, RTL_CLOCK_MODE_PERIODIC);
//  _i8254_clock.settimer (clock, 4000*(HRTICKS_PER_SEC/CLOCK_TICK_RATE) );

  _8254_settimermode (clock, RTL_CLOCK_MODE_ONESHOT);
  _i8254_clock.settimer (clock, HRTIME_INFINITY);

  enable_8259_irq(0); 
  
//  _i8254_clock.settimer (clock, 50);
  
  /*rtl_no_interrupts (flags);
  
  _i8254_clock.arch.linux_time = gethrtime() + latch_ns;
        
  rtl_request_global_irq(0, _8254_irq);
	
  rtl_restore_interrupts (flags);   */
  
  rtl_restore_interrupts (flags);
  return 0;
}

static void _8254_uninit (rtl_clockid_t clock)
{
/*int flags;
  if (clock -> mode == RTL_CLOCK_MODE_UNINITIALIZED) 
  {
    return;
  }
  clock->handler = RTL_CLOCK_DEFAULTS.handler;
  rtl_spin_lock_irqsave (&lock8254, flags);
  do_gettimeoffset = save_do_gettimeoffset;
  use_tsc = save_use_tsc;
  outb_p(0x34,0x43);		
  WRITE_COUNTER_ZERO16(LATCH);
  rtl_free_global_irq(0);
  clock -> mode = RTL_CLOCK_MODE_UNINITIALIZED;
  rtl_spin_unlock_irqrestore (&lock8254, flags);*/
}


/* sort of a constructor */
int rtl_create_clock_8254(void)
{
  _i8254_clock = RTL_CLOCK_DEFAULTS;
  _i8254_clock.init = _8254_init;
  _i8254_clock.uninit = _8254_uninit;
  _i8254_clock.settimermode = _8254_settimermode;
  _i8254_clock.value = 0;
  return 0;
}


/* returns a pointer to the clock structure of the best controlling hw clock 
 * for this CPU */

rtl_clockid_t rtl_getbestclock (unsigned int cpu)
{
  return &_i8254_clock;
}


/* scaler_pentium ==  2^32 / (2^5 * (cpu clocks per ns)) */
static void do_calibration(int do_tsc)
{
	long long t1 = 0;
	long long t2 = 0;
	long pps;
	int j;
	long result = 0;

	rtl_irqstate_t flags;
	rtl_no_interrupts(flags);

	rtl_outb((rtl_inb(0x61) & ~0x02) | 0x01, 0x61);

	rtl_outb_p(0xb6, 0x43);     /* binary, mode 3, LSB/MSB, ch 2 */
	rtl_outb(CLATCH & 0xff, 0x42);	/* LSB of count */
	rtl_outb(CLATCH >> 8, 0x42);	/* MSB of count */

	wait_cycle();
	if (do_tsc)
		rdtscll(t1);

	for (j = 0; j < NLOOPS; j++) {
		wait_cycle();
	}
	if (do_tsc)
		rdtscll(t2);
	if (do_tsc)
		result = t2 - t1;

	rtl_restore_interrupts(flags);

	if (do_tsc) {
		pps = muldiv (result, CLOCK_TICK_RATE, CLATCH * NLOOPS);
#if HRTICKS_PER_SEC == NSECS_PER_SEC
		scaler_pentium_to_hrtime = muldiv (1 << 27, HRTICKS_PER_SEC, pps);
#else
		scaler_pentium_to_hrtime = muldiv (1 << 31, HRTICKS_PER_SEC * 2, pps);
#endif
	} else  {
		scaler_pentium_to_hrtime = 0;
	}
}



static void init_hrtime (void)
{
  int flags;
  
  rtl_do_get_time = global_8254_gettime;
  hrtime_resolution = HRTICKS_PER_SEC / CLOCK_TICK_RATE;

#if HRTICKS_PER_SEC != CLOCK_TICK_RATE
  scaler_8254_to_hrtime = muldiv (HRTICKS_PER_SEC, 1 << 22, CLOCK_TICK_RATE);
  scaler_hrtime_to_8254 = muldiv (CLOCK_TICK_RATE, 1 << 31, HRTICKS_PER_SEC / 2);
  latch_ns = muldiv (LATCH, HRTICKS_PER_SEC, CLOCK_TICK_RATE);
#else
  latch_ns = LATCH;
#endif
  
  max_latch_oneshot = latch_ns * 3 / 4;

  rtl_no_interrupts(flags);
  

       /* program channel 2 of the 8254 chip for periodic counting */
  rtl_outb_p(0xb6, 0x43);     /* binary, mode 3, LSB/MSB, ch 2 */
  rtl_outb_p(LATCH2 & 0xff, 0x42);
  rtl_outb_p((LATCH2 >> 8) & 0xff, 0x42);

  rtl_outb_p((rtl_inb_p(0x61) & 0xfd) | 1, 0x61); /* shut up the speaker and enable counting */
//  rtl_outb_p((rtl_inb_p(0x61) & 0xfd) | 1, 0x61); /* shut up the speaker and enable counting */

  LATCH_CNT2();
  READ_CNT2(last_c2);
  
  offset_time = 0;
  base_time = 0;
  
  rtl_do_get_time = global_8254_gettime;
  
  hrtime_resolution = HRTICKS_PER_SEC / CLOCK_TICK_RATE;

  do_calibration(1);
 
  rtl_do_get_time = pent_gettime;
  hrtime_resolution = 32;
  rtl_restore_interrupts(flags);
}


int init_clocks (void)
{
  int flags;
  
  rtl_spin_lock_init (&lock8254);

  init_hrtime();

  
  rtl_no_interrupts(flags);
  rtl_create_clock_8254();
  rtl_init_standard_clocks();
  rtl_restore_interrupts(flags);
  return 0;
}