diff options
Diffstat (limited to 'bsd_eth_drivers/libbsdport/rtems_udelay.c')
-rw-r--r-- | bsd_eth_drivers/libbsdport/rtems_udelay.c | 192 |
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 |