1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
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
|
