#include <rtems.h>
#include <rtems/error.h>
#include <bsp.h>

#include "rtems_udelay.h"

#if defined(__PPC__)

#include <rtems/powerpc/registers.h>

/* Ouch. stupid bookE doesn't implement mftb so we must
 * use mfspr (which wouldn't work on classic ppc if we
 * were in user mode but luckily we're not.
 */
static inline uint64_t __read_hires_clicks()
{
uint32_t tbl, tbu1, tbu2;
	asm volatile(
		"	mfspr %0, %4	\n"
		"	mfspr %1, %3	\n"
		"	mfspr %2, %4	\n"
		:"=r"(tbu1),"=r"(tbl),"=r"(tbu2)
		:"i"(TBRL), "i"(TBRU)
		);
	if ( tbu1 != tbu2 )
		asm volatile("mfspr %0, %1":"=r"(tbl):"i"(TBRL));
	return ((uint64_t)tbu2<<32) | tbl;
}

#define __rtems_hires_kHz (BSP_bus_frequency/BSP_time_base_divisor)

#elif defined(__i386__)

static inline uint64_t __read_hires_clicks()
{
uint32_t lo,hi;
	asm volatile("rdtsc":"=a"(lo),"=d"(hi));
	return ( (uint64_t)hi << 32 ) | lo;
}

#else
#error "rtems_udelay.c not ported to this CPU yet"
#endif

#ifndef __rtems_hires_kHz
/* Clock frequency of high-resolution timer */
uint32_t __rtems_hires_kHz = 0;
uint32_t rtems_udelay_calibrate();
#endif


void rtems_usec_delay(uint32_t usecs)
{
uint64_t clicks = __read_hires_clicks();
int      ticks;
	if (usecs > 10) {
		if ( _ISR_Is_in_progress() ) {
			rtems_panic("rtems_usec_delay for more than 10us in ISR!!");
		}
		if ( _ISR_Get_level() > 0 ) {
			rtems_panic("rtems_usec_delay for more than 10us with IRQs disabled!!");
		}
	}
	ticks   = usecs/rtems_configuration_get_microseconds_per_tick();
#ifndef __rtems_hires_kHz
	/* If it's not a macro; do lazy init */
	if ( 0 == __rtems_hires_kHz ) {
		uint64_t clicks = rtems_udelay_calibrate();
		__rtems_hires_kHz = (clicks * 1000) / rtems_configuration_get_microseconds_per_tick();
		ticks--;
	}
#endif
	clicks += (usecs * __rtems_hires_kHz)/1000;
	if ( ticks > 0 )
		rtems_task_wake_after(ticks);

	while ( clicks > __read_hires_clicks() )
		/* busy wait */;
}

/* This doesn't belong here; also, the RTEMS timeout() implementation is buggy:
 * if a timeout is added when the networking task is asleep then I believe 'timeout()'
 * is unable to schedule a wakeup. Therefore, I implemented the 'callout' facility.
 */
#ifdef UNTESTED
/* Must be executed with the network semaphore held */
void
rtems_bsdnet_untimeout(timeout_func_t fn, void *arg)
{
register struct callout *l, *p;

	for ( l = &calltodo; (p=l->c_next); l=p ) {
		if ( p->c_func == fn && p->c_arg == arg ) {
			register struct callout *n
			/* found it */
			if ( (n = p->c_next) && p->c_time > 0 ) {
				/* adjust time of following entry */
				n->c_time += p->c_time;
			}
			/* extract */
			l->c_next = n;
			/* return to extract first occurrence; continue
			 * to extract all
			 */
#if 0
			return;
#else
			p = l;
#endif
		}
	}
}
#endif

struct caldat {
	uint64_t t0,t1,t2,t3;
	rtems_id thetid;
};

#ifndef __rtems_hires_kHz
static void
tickmeas1(rtems_id myself, void *arg)
{
struct caldat *p = arg;
	p->t2 = __read_hires_clicks();
	rtems_event_send(p->thetid, RTEMS_EVENT_0);
}

static void
tickmeas0(rtems_id myself, void *arg)
{
struct caldat *p = arg;
	p->t1 = __read_hires_clicks();
	rtems_timer_fire_after( myself, 1, tickmeas1, arg );
}

/* Calibrate high-resolution timer */
uint32_t
rtems_udelay_calibrate()
{
rtems_id          timer;
rtems_status_code sc;
rtems_event_set   ev;
struct caldat     d;

	/* measure a clock tick with the hires timer;
	 * note that we can't just sleep for 1 tick because
	 * that results in sleeping for an unknown fraction
	 * of a tick...
	 */
	sc = rtems_timer_create( rtems_build_name('h','r','e','s'), &timer );
	if ( RTEMS_SUCCESSFUL != sc ) {
		rtems_panic("Unable to create timer:%i\n", sc);
	}
	sc = rtems_task_ident(RTEMS_SELF, RTEMS_LOCAL, &d.thetid);
	if ( RTEMS_SUCCESSFUL != sc ) {
		rtems_panic("Unable to read my own TID:%i\n", sc);
	}
#ifdef DEBUG
	d.t0 = __read_hires_clicks();
#endif
	sc = rtems_timer_fire_after(timer, 1, tickmeas0, &d);
	if ( RTEMS_SUCCESSFUL != sc ) {
		rtems_panic("Unable to fire timer:%i\n", sc);
	}
	sc = rtems_event_receive( RTEMS_EVENT_0, RTEMS_EVENT_ANY | RTEMS_WAIT , RTEMS_NO_TIMEOUT, &ev); 
#ifdef DEBUG
	d.t3 = __read_hires_clicks();
#endif
	if ( RTEMS_SUCCESSFUL != sc ) {
		rtems_panic("Unable to synchronize with timer:%i\n", sc);
	}
	rtems_timer_delete(timer);
#ifdef DEBUG
	printf("Diffs: %llu %llu %llu\n",
	d.t3-d.t2, d.t2-d.t1, d.t1-d.t0);
#endif
	return d.t2-d.t1;
}
#endif

#ifdef DEBUG
unsigned
hdiff(unsigned s)
{
uint64_t now = __read_hires_clicks();
	rtems_task_wake_after(s);
	now = __read_hires_clicks() - now;
	printf("Diff was %llu clicks\n",now);
	return (unsigned)now;
}
#endif