1 files changed, 192 insertions, 0 deletions
diff --git a/bsd_eth_drivers/libbsdport/rtems_udelay.c b/bsd_eth_drivers/libbsdport/rtems_udelay.c
new file mode 100644
index 0000000..d8f80f4
--- /dev/null
+++ b/bsd_eth_drivers/libbsdport/rtems_udelay.c
@@ -0,0 +1,192 @@
+#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