1 |
/* |
---|
2 |
* Copyright (C) 2005-2008 by Pieter Palmers |
---|
3 |
* |
---|
4 |
* This file is part of FFADO |
---|
5 |
* FFADO = Free Firewire (pro-)audio drivers for linux |
---|
6 |
* |
---|
7 |
* FFADO is based upon FreeBoB |
---|
8 |
* |
---|
9 |
* This program is free software: you can redistribute it and/or modify |
---|
10 |
* it under the terms of the GNU General Public License as published by |
---|
11 |
* the Free Software Foundation, either version 2 of the License, or |
---|
12 |
* (at your option) version 3 of the License. |
---|
13 |
* |
---|
14 |
* This program is distributed in the hope that it will be useful, |
---|
15 |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
---|
16 |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
---|
17 |
* GNU General Public License for more details. |
---|
18 |
* |
---|
19 |
* You should have received a copy of the GNU General Public License |
---|
20 |
* along with this program. If not, see <http://www.gnu.org/licenses/>. |
---|
21 |
* |
---|
22 |
*/ |
---|
23 |
|
---|
24 |
#include "config.h" |
---|
25 |
|
---|
26 |
#include "CycleTimerHelper.h" |
---|
27 |
#include "ieee1394service.h" |
---|
28 |
#include "libutil/PosixThread.h" |
---|
29 |
#include "libutil/PosixMutex.h" |
---|
30 |
#include "libutil/Atomic.h" |
---|
31 |
#include "libutil/Watchdog.h" |
---|
32 |
|
---|
33 |
#define DLL_PI (3.141592653589793238) |
---|
34 |
#define DLL_SQRT2 (1.414213562373095049) |
---|
35 |
|
---|
36 |
// the high-bandwidth coefficients are used |
---|
37 |
// to speed up inital tracking |
---|
38 |
#define DLL_BANDWIDTH_HIGH (0.1) |
---|
39 |
#define DLL_OMEGA_HIGH (2.0*DLL_PI*DLL_BANDWIDTH_HIGH) |
---|
40 |
#define DLL_COEFF_B_HIGH (DLL_SQRT2 * DLL_OMEGA_HIGH) |
---|
41 |
#define DLL_COEFF_C_HIGH (DLL_OMEGA_HIGH * DLL_OMEGA_HIGH) |
---|
42 |
|
---|
43 |
// the low-bandwidth coefficients are used once we have a |
---|
44 |
// good estimate of the internal parameters |
---|
45 |
#define DLL_BANDWIDTH (0.1) |
---|
46 |
#define DLL_OMEGA (2.0*DLL_PI*DLL_BANDWIDTH) |
---|
47 |
#define DLL_COEFF_B (DLL_SQRT2 * DLL_OMEGA) |
---|
48 |
#define DLL_COEFF_C (DLL_OMEGA * DLL_OMEGA) |
---|
49 |
|
---|
50 |
// is 1 sec |
---|
51 |
#define UPDATES_WITH_HIGH_BANDWIDTH \ |
---|
52 |
(1000000 / IEEE1394SERVICE_CYCLETIMER_DLL_UPDATE_INTERVAL_USEC) |
---|
53 |
|
---|
54 |
IMPL_DEBUG_MODULE( CycleTimerHelper, CycleTimerHelper, DEBUG_LEVEL_NORMAL ); |
---|
55 |
|
---|
56 |
CycleTimerHelper::CycleTimerHelper(Ieee1394Service &parent, unsigned int update_period_us) |
---|
57 |
: m_Parent ( parent ) |
---|
58 |
, m_ticks_per_update ( ((uint64_t)TICKS_PER_SECOND) * ((uint64_t)update_period_us) / 1000000ULL ) |
---|
59 |
, m_usecs_per_update ( update_period_us ) |
---|
60 |
, m_avg_wakeup_delay ( 0.0 ) |
---|
61 |
, m_dll_e2 ( 0.0 ) |
---|
62 |
, m_current_time_usecs ( 0 ) |
---|
63 |
, m_next_time_usecs ( 0 ) |
---|
64 |
, m_current_time_ticks ( 0 ) |
---|
65 |
, m_next_time_ticks ( 0 ) |
---|
66 |
, m_first_run ( true ) |
---|
67 |
, m_sleep_until ( 0 ) |
---|
68 |
, m_cycle_timer_prev ( 0 ) |
---|
69 |
, m_cycle_timer_ticks_prev ( 0 ) |
---|
70 |
, m_high_bw_updates ( UPDATES_WITH_HIGH_BANDWIDTH ) |
---|
71 |
, m_current_shadow_idx ( 0 ) |
---|
72 |
, m_Thread ( NULL ) |
---|
73 |
, m_realtime ( false ) |
---|
74 |
, m_priority ( 0 ) |
---|
75 |
, m_update_lock( new Util::PosixMutex() ) |
---|
76 |
, m_busreset_functor ( NULL) |
---|
77 |
, m_unhandled_busreset ( false ) |
---|
78 |
{ |
---|
79 |
debugOutput( DEBUG_LEVEL_VERBOSE, "Create %p...\n", this); |
---|
80 |
} |
---|
81 |
|
---|
82 |
CycleTimerHelper::CycleTimerHelper(Ieee1394Service &parent, unsigned int update_period_us, bool rt, int prio) |
---|
83 |
: m_Parent ( parent ) |
---|
84 |
, m_ticks_per_update ( ((uint64_t)TICKS_PER_SECOND) * ((uint64_t)update_period_us) / 1000000ULL ) |
---|
85 |
, m_usecs_per_update ( update_period_us ) |
---|
86 |
, m_avg_wakeup_delay ( 0.0 ) |
---|
87 |
, m_dll_e2 ( 0.0 ) |
---|
88 |
, m_current_time_usecs ( 0 ) |
---|
89 |
, m_next_time_usecs ( 0 ) |
---|
90 |
, m_current_time_ticks ( 0 ) |
---|
91 |
, m_next_time_ticks ( 0 ) |
---|
92 |
, m_first_run ( true ) |
---|
93 |
, m_sleep_until ( 0 ) |
---|
94 |
, m_cycle_timer_prev ( 0 ) |
---|
95 |
, m_cycle_timer_ticks_prev ( 0 ) |
---|
96 |
, m_high_bw_updates ( UPDATES_WITH_HIGH_BANDWIDTH ) |
---|
97 |
, m_current_shadow_idx ( 0 ) |
---|
98 |
, m_Thread ( NULL ) |
---|
99 |
, m_realtime ( rt ) |
---|
100 |
, m_priority ( prio ) |
---|
101 |
, m_update_lock( new Util::PosixMutex() ) |
---|
102 |
, m_busreset_functor ( NULL) |
---|
103 |
, m_unhandled_busreset ( false ) |
---|
104 |
{ |
---|
105 |
debugOutput( DEBUG_LEVEL_VERBOSE, "Create %p...\n", this); |
---|
106 |
} |
---|
107 |
|
---|
108 |
CycleTimerHelper::~CycleTimerHelper() |
---|
109 |
{ |
---|
110 |
if (m_Thread) { |
---|
111 |
m_Thread->Stop(); |
---|
112 |
delete m_Thread; |
---|
113 |
} |
---|
114 |
|
---|
115 |
// unregister the bus reset handler |
---|
116 |
if(m_busreset_functor) { |
---|
117 |
m_Parent.remBusResetHandler( m_busreset_functor ); |
---|
118 |
delete m_busreset_functor; |
---|
119 |
} |
---|
120 |
delete m_update_lock; |
---|
121 |
} |
---|
122 |
|
---|
123 |
bool |
---|
124 |
CycleTimerHelper::Start() |
---|
125 |
{ |
---|
126 |
debugOutput( DEBUG_LEVEL_VERBOSE, "Start %p...\n", this); |
---|
127 |
|
---|
128 |
if(!initValues()) { |
---|
129 |
debugFatal("(%p) Could not init values\n", this); |
---|
130 |
return false; |
---|
131 |
} |
---|
132 |
|
---|
133 |
m_Thread = new Util::PosixThread(this, m_realtime, m_priority, |
---|
134 |
PTHREAD_CANCEL_DEFERRED); |
---|
135 |
if(!m_Thread) { |
---|
136 |
debugFatal("No thread\n"); |
---|
137 |
return false; |
---|
138 |
} |
---|
139 |
// register the thread with the RT watchdog |
---|
140 |
Util::Watchdog *watchdog = m_Parent.getWatchdog(); |
---|
141 |
if(watchdog) { |
---|
142 |
if(!watchdog->registerThread(m_Thread)) { |
---|
143 |
debugWarning("could not register update thread with watchdog\n"); |
---|
144 |
} |
---|
145 |
} else { |
---|
146 |
debugWarning("could not find valid watchdog\n"); |
---|
147 |
} |
---|
148 |
|
---|
149 |
if (m_Thread->Start() != 0) { |
---|
150 |
debugFatal("Could not start update thread\n"); |
---|
151 |
return false; |
---|
152 |
} |
---|
153 |
return true; |
---|
154 |
} |
---|
155 |
|
---|
156 |
bool |
---|
157 |
CycleTimerHelper::initValues() |
---|
158 |
{ |
---|
159 |
debugOutput( DEBUG_LEVEL_VERBOSE, "(%p) Init values...\n", this ); |
---|
160 |
Util::MutexLockHelper lock(*m_update_lock); |
---|
161 |
|
---|
162 |
// initialize the 'prev ctr' values |
---|
163 |
uint64_t local_time; |
---|
164 |
int maxtries2 = 10; |
---|
165 |
do { |
---|
166 |
debugOutput( DEBUG_LEVEL_VERBOSE, "Read CTR...\n" ); |
---|
167 |
if(!m_Parent.readCycleTimerReg(&m_cycle_timer_prev, &local_time)) { |
---|
168 |
debugError("Could not read cycle timer register\n"); |
---|
169 |
return false; |
---|
170 |
} |
---|
171 |
if (m_cycle_timer_prev == 0) { |
---|
172 |
debugOutput(DEBUG_LEVEL_VERBOSE, |
---|
173 |
"Bogus CTR: %08X on try %02d\n", |
---|
174 |
m_cycle_timer_prev, maxtries2); |
---|
175 |
} |
---|
176 |
debugOutput( DEBUG_LEVEL_VERBOSE, " read : CTR: %11lu, local: %17llu\n", |
---|
177 |
m_cycle_timer_prev, local_time); |
---|
178 |
debugOutput(DEBUG_LEVEL_VERBOSE, |
---|
179 |
" ctr : 0x%08X %11llu (%03us %04ucy %04uticks)\n", |
---|
180 |
(uint32_t)m_cycle_timer_prev, (uint64_t)CYCLE_TIMER_TO_TICKS(m_cycle_timer_prev), |
---|
181 |
(unsigned int)CYCLE_TIMER_GET_SECS( m_cycle_timer_prev ), |
---|
182 |
(unsigned int)CYCLE_TIMER_GET_CYCLES( m_cycle_timer_prev ), |
---|
183 |
(unsigned int)CYCLE_TIMER_GET_OFFSET( m_cycle_timer_prev ) ); |
---|
184 |
|
---|
185 |
} while (m_cycle_timer_prev == 0 && maxtries2--); |
---|
186 |
m_cycle_timer_ticks_prev = CYCLE_TIMER_TO_TICKS(m_cycle_timer_prev); |
---|
187 |
|
---|
188 |
#if IEEE1394SERVICE_USE_CYCLETIMER_DLL |
---|
189 |
debugOutput( DEBUG_LEVEL_VERBOSE, "requesting DLL re-init...\n" ); |
---|
190 |
Util::SystemTimeSource::SleepUsecRelative(1000); // some time to settle |
---|
191 |
m_high_bw_updates = UPDATES_WITH_HIGH_BANDWIDTH; |
---|
192 |
if(!initDLL()) { |
---|
193 |
debugError("(%p) Could not init DLL\n", this); |
---|
194 |
return false; |
---|
195 |
} |
---|
196 |
// make the DLL re-init itself as if it were started up |
---|
197 |
m_first_run = true; |
---|
198 |
#endif |
---|
199 |
debugOutput( DEBUG_LEVEL_VERBOSE, "ready...\n" ); |
---|
200 |
return true; |
---|
201 |
} |
---|
202 |
|
---|
203 |
bool |
---|
204 |
CycleTimerHelper::Init() |
---|
205 |
{ |
---|
206 |
debugOutput( DEBUG_LEVEL_VERBOSE, "Initialize %p...\n", this); |
---|
207 |
|
---|
208 |
// register a bus reset handler |
---|
209 |
m_busreset_functor = new Util::MemberFunctor0< CycleTimerHelper*, |
---|
210 |
void (CycleTimerHelper::*)() > |
---|
211 |
( this, &CycleTimerHelper::busresetHandler, false ); |
---|
212 |
if ( !m_busreset_functor ) { |
---|
213 |
debugFatal( "(%p) Could not create busreset handler\n", this ); |
---|
214 |
return false; |
---|
215 |
} |
---|
216 |
m_Parent.addBusResetHandler( m_busreset_functor ); |
---|
217 |
|
---|
218 |
#ifdef DEBUG |
---|
219 |
m_last_loop_entry = 0; |
---|
220 |
m_successive_short_loops = 0; |
---|
221 |
#endif |
---|
222 |
|
---|
223 |
return true; |
---|
224 |
} |
---|
225 |
|
---|
226 |
void |
---|
227 |
CycleTimerHelper::busresetHandler() |
---|
228 |
{ |
---|
229 |
debugOutput( DEBUG_LEVEL_VERBOSE, "Bus reset...\n" ); |
---|
230 |
m_unhandled_busreset = true; |
---|
231 |
// whenever a bus reset occurs, the root node can change, |
---|
232 |
// and the CTR timer can be reset. We should hence reinit |
---|
233 |
// the DLL |
---|
234 |
if(!initValues()) { |
---|
235 |
debugError("(%p) Could not re-init values\n", this); |
---|
236 |
} |
---|
237 |
m_unhandled_busreset = false; |
---|
238 |
} |
---|
239 |
|
---|
240 |
bool |
---|
241 |
CycleTimerHelper::setThreadParameters(bool rt, int priority) { |
---|
242 |
debugOutput( DEBUG_LEVEL_VERBOSE, "(%p) switch to: (rt=%d, prio=%d)...\n", this, rt, priority); |
---|
243 |
if (priority > THREAD_MAX_RTPRIO) priority = THREAD_MAX_RTPRIO; // cap the priority |
---|
244 |
m_realtime = rt; |
---|
245 |
m_priority = priority; |
---|
246 |
|
---|
247 |
#if IEEE1394SERVICE_USE_CYCLETIMER_DLL |
---|
248 |
if (m_Thread) { |
---|
249 |
if (m_realtime) { |
---|
250 |
m_Thread->AcquireRealTime(m_priority); |
---|
251 |
} else { |
---|
252 |
m_Thread->DropRealTime(); |
---|
253 |
} |
---|
254 |
} |
---|
255 |
#endif |
---|
256 |
|
---|
257 |
return true; |
---|
258 |
} |
---|
259 |
|
---|
260 |
#if IEEE1394SERVICE_USE_CYCLETIMER_DLL |
---|
261 |
float |
---|
262 |
CycleTimerHelper::getRate() |
---|
263 |
{ |
---|
264 |
float rate = (float)(diffTicks((uint64_t)m_next_time_ticks, (uint64_t)m_current_time_ticks)); |
---|
265 |
rate /= (float)(m_next_time_usecs - m_current_time_usecs); |
---|
266 |
return rate; |
---|
267 |
} |
---|
268 |
|
---|
269 |
float |
---|
270 |
CycleTimerHelper::getNominalRate() |
---|
271 |
{ |
---|
272 |
float rate = ((double)TICKS_PER_SECOND) / 1000000.0; |
---|
273 |
return rate; |
---|
274 |
} |
---|
275 |
|
---|
276 |
/* |
---|
277 |
* call with lock held |
---|
278 |
*/ |
---|
279 |
bool |
---|
280 |
CycleTimerHelper::initDLL() { |
---|
281 |
uint32_t cycle_timer; |
---|
282 |
uint64_t local_time; |
---|
283 |
uint64_t cycle_timer_ticks; |
---|
284 |
|
---|
285 |
if(!readCycleTimerWithRetry(&cycle_timer, &local_time, 10)) { |
---|
286 |
debugError("Could not read cycle timer register\n"); |
---|
287 |
return false; |
---|
288 |
} |
---|
289 |
cycle_timer_ticks = CYCLE_TIMER_TO_TICKS(cycle_timer); |
---|
290 |
|
---|
291 |
debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, " read : CTR: %11lu, local: %17llu\n", |
---|
292 |
cycle_timer, local_time); |
---|
293 |
debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, |
---|
294 |
" ctr : 0x%08X %11llu (%03us %04ucy %04uticks)\n", |
---|
295 |
(uint32_t)cycle_timer, (uint64_t)cycle_timer_ticks, |
---|
296 |
(unsigned int)TICKS_TO_SECS( (uint64_t)cycle_timer_ticks ), |
---|
297 |
(unsigned int)TICKS_TO_CYCLES( (uint64_t)cycle_timer_ticks ), |
---|
298 |
(unsigned int)TICKS_TO_OFFSET( (uint64_t)cycle_timer_ticks ) ); |
---|
299 |
|
---|
300 |
m_sleep_until = local_time + m_usecs_per_update; |
---|
301 |
m_dll_e2 = m_ticks_per_update; |
---|
302 |
m_current_time_usecs = local_time; |
---|
303 |
m_next_time_usecs = m_current_time_usecs + m_usecs_per_update; |
---|
304 |
m_current_time_ticks = CYCLE_TIMER_TO_TICKS( cycle_timer ); |
---|
305 |
m_next_time_ticks = addTicks( (uint64_t)m_current_time_ticks, (uint64_t)m_dll_e2); |
---|
306 |
debugOutput(DEBUG_LEVEL_VERBOSE, " (%p) First run\n", this); |
---|
307 |
debugOutput(DEBUG_LEVEL_VERBOSE, |
---|
308 |
" usecs/update: %lu, ticks/update: %lu, m_dll_e2: %f\n", |
---|
309 |
m_usecs_per_update, m_ticks_per_update, m_dll_e2); |
---|
310 |
debugOutput(DEBUG_LEVEL_VERBOSE, |
---|
311 |
" usecs current: %f, next: %f\n", |
---|
312 |
m_current_time_usecs, m_next_time_usecs); |
---|
313 |
debugOutput(DEBUG_LEVEL_VERBOSE, |
---|
314 |
" ticks current: %f, next: %f\n", |
---|
315 |
m_current_time_ticks, m_next_time_ticks); |
---|
316 |
return true; |
---|
317 |
} |
---|
318 |
|
---|
319 |
bool |
---|
320 |
CycleTimerHelper::Execute() |
---|
321 |
{ |
---|
322 |
debugOutput( DEBUG_LEVEL_ULTRA_VERBOSE, "Execute %p...\n", this); |
---|
323 |
|
---|
324 |
#ifdef DEBUG |
---|
325 |
uint64_t now = m_Parent.getCurrentTimeAsUsecs(); |
---|
326 |
int diff = now - m_last_loop_entry; |
---|
327 |
if(diff < 100) { |
---|
328 |
debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, |
---|
329 |
"(%p) short loop detected (%d usec), cnt: %d\n", |
---|
330 |
this, diff, m_successive_short_loops); |
---|
331 |
m_successive_short_loops++; |
---|
332 |
if(m_successive_short_loops > 100) { |
---|
333 |
debugError("Shutting down runaway thread\n"); |
---|
334 |
return false; |
---|
335 |
} |
---|
336 |
} else { |
---|
337 |
// reset the counter |
---|
338 |
m_successive_short_loops = 0; |
---|
339 |
} |
---|
340 |
m_last_loop_entry = now; |
---|
341 |
#endif |
---|
342 |
|
---|
343 |
if (!m_first_run) { |
---|
344 |
// wait for the next update period |
---|
345 |
//#if DEBUG_EXTREME_ENABLE |
---|
346 |
#ifdef DEBUG |
---|
347 |
ffado_microsecs_t now = Util::SystemTimeSource::getCurrentTimeAsUsecs(); |
---|
348 |
int sleep_time = m_sleep_until - now; |
---|
349 |
debugOutput( DEBUG_LEVEL_ULTRA_VERBOSE, "(%p) Sleep until %lld/%f (now: %lld, diff=%d) ...\n", |
---|
350 |
this, m_sleep_until, m_next_time_usecs, now, sleep_time); |
---|
351 |
#endif |
---|
352 |
Util::SystemTimeSource::SleepUsecAbsolute(m_sleep_until); |
---|
353 |
debugOutput( DEBUG_LEVEL_ULTRA_VERBOSE, " (%p) back...\n", this); |
---|
354 |
} |
---|
355 |
|
---|
356 |
uint32_t cycle_timer; |
---|
357 |
uint64_t local_time; |
---|
358 |
int64_t usecs_late; |
---|
359 |
int ntries=4; |
---|
360 |
uint64_t cycle_timer_ticks; |
---|
361 |
double diff_ticks; |
---|
362 |
|
---|
363 |
// if the difference between the predicted value and the |
---|
364 |
// actual value seems to be too large, retry reading the cycle timer |
---|
365 |
// some host controllers return bogus values on some reads |
---|
366 |
// (looks like a non-atomic update of the register) |
---|
367 |
do { |
---|
368 |
debugOutput( DEBUG_LEVEL_ULTRA_VERBOSE, "(%p) reading cycle timer register...\n", this); |
---|
369 |
if(!readCycleTimerWithRetry(&cycle_timer, &local_time, 10)) { |
---|
370 |
debugError("Could not read cycle timer register\n"); |
---|
371 |
return false; |
---|
372 |
} |
---|
373 |
usecs_late = local_time - m_sleep_until; |
---|
374 |
cycle_timer_ticks = CYCLE_TIMER_TO_TICKS(cycle_timer); |
---|
375 |
diff_ticks = diffTicks(cycle_timer_ticks, (int64_t)m_next_time_ticks); |
---|
376 |
|
---|
377 |
// check for unrealistic CTR reads (NEC controller does that sometimes) |
---|
378 |
if(diff_ticks < -((double)TICKS_PER_HALFCYCLE)) { |
---|
379 |
debugOutput(DEBUG_LEVEL_ULTRA_VERBOSE, |
---|
380 |
"(%p) have to retry CTR read, diff unrealistic: diff: %f, max: %f (try: %d)\n", |
---|
381 |
this, diff_ticks, -((double)TICKS_PER_HALFCYCLE), ntries); |
---|
382 |
} |
---|
383 |
|
---|
384 |
} while( diff_ticks < -((double)TICKS_PER_HALFCYCLE) |
---|
385 |
&& --ntries && !m_first_run && !m_unhandled_busreset); |
---|
386 |
|
---|
387 |
// grab the lock after sleeping, otherwise we can't be interrupted by |
---|
388 |
// the busreset thread (lower prio) |
---|
389 |
// also grab it after reading the CTR register such that the jitter between |
---|
390 |
// wakeup and read is as small as possible |
---|
391 |
Util::MutexLockHelper lock(*m_update_lock); |
---|
392 |
|
---|
393 |
// // simulate a random scheduling delay between (0-10ms) |
---|
394 |
// ffado_microsecs_t tmp = Util::SystemTimeSource::SleepUsecRandom(10000); |
---|
395 |
// debugOutput( DEBUG_LEVEL_VERBOSE, " (%p) random sleep of %llu usecs...\n", this, tmp); |
---|
396 |
|
---|
397 |
if(m_unhandled_busreset) { |
---|
398 |
debugOutput(DEBUG_LEVEL_VERBOSE, |
---|
399 |
"(%p) Skipping DLL update due to unhandled busreset\n", this); |
---|
400 |
m_sleep_until += m_usecs_per_update; |
---|
401 |
// keep the thread running |
---|
402 |
return true; |
---|
403 |
} |
---|
404 |
|
---|
405 |
debugOutputExtreme( DEBUG_LEVEL_ULTRA_VERBOSE, " read : CTR: %11lu, local: %17llu\n", |
---|
406 |
cycle_timer, local_time); |
---|
407 |
debugOutputExtreme(DEBUG_LEVEL_ULTRA_VERBOSE, |
---|
408 |
" ctr : 0x%08X %11llu (%03us %04ucy %04uticks)\n", |
---|
409 |
(uint32_t)cycle_timer, (uint64_t)cycle_timer_ticks, |
---|
410 |
(unsigned int)TICKS_TO_SECS( (uint64_t)cycle_timer_ticks ), |
---|
411 |
(unsigned int)TICKS_TO_CYCLES( (uint64_t)cycle_timer_ticks ), |
---|
412 |
(unsigned int)TICKS_TO_OFFSET( (uint64_t)cycle_timer_ticks ) ); |
---|
413 |
|
---|
414 |
if (m_first_run) { |
---|
415 |
if(!initDLL()) { |
---|
416 |
debugError("(%p) Could not init DLL\n", this); |
---|
417 |
return false; |
---|
418 |
} |
---|
419 |
m_first_run = false; |
---|
420 |
} else if (diff_ticks > 20.0*m_ticks_per_update) { |
---|
421 |
debugOutput(DEBUG_LEVEL_VERBOSE, |
---|
422 |
"re-init dll due to too large tick diff: %f >> %f\n", |
---|
423 |
diff_ticks, (float)(20.0*m_ticks_per_update)); |
---|
424 |
if(!initDLL()) { |
---|
425 |
debugError("(%p) Could not init DLL\n", this); |
---|
426 |
return false; |
---|
427 |
} |
---|
428 |
} else { |
---|
429 |
// calculate next sleep time |
---|
430 |
m_sleep_until += m_usecs_per_update; |
---|
431 |
|
---|
432 |
// correct for the latency between the wakeup and the actual CTR |
---|
433 |
// read. The only time we can trust is the time returned by the |
---|
434 |
// CTR read kernel call, since that (should be) atomically read |
---|
435 |
// together with the ctr register itself. |
---|
436 |
|
---|
437 |
// if we are usecs_late usecs late |
---|
438 |
// the cycle timer has ticked approx ticks_late ticks too much |
---|
439 |
// if we are woken up early (which shouldn't happen according to POSIX) |
---|
440 |
// the cycle timer has ticked approx -ticks_late too little |
---|
441 |
int64_t ticks_late = (usecs_late * TICKS_PER_SECOND) / 1000000LL; |
---|
442 |
// the corrected difference between predicted and actual ctr |
---|
443 |
// i.e. DLL error signal |
---|
444 |
double diff_ticks_corr; |
---|
445 |
if (ticks_late > 0) { |
---|
446 |
diff_ticks_corr = diff_ticks - ticks_late; |
---|
447 |
debugOutputExtreme(DEBUG_LEVEL_ULTRA_VERBOSE, |
---|
448 |
"diff_ticks_corr=%f, diff_ticks = %f, ticks_late = %lld\n", |
---|
449 |
diff_ticks_corr, diff_ticks, ticks_late); |
---|
450 |
} else { |
---|
451 |
debugError("Early wakeup, should not happen!\n"); |
---|
452 |
// recover |
---|
453 |
diff_ticks_corr = diff_ticks + ticks_late; |
---|
454 |
} |
---|
455 |
|
---|
456 |
#ifdef DEBUG |
---|
457 |
// makes no sense if not running realtime |
---|
458 |
if(m_realtime && usecs_late > 1000) { |
---|
459 |
debugOutput(DEBUG_LEVEL_VERBOSE, "Rather late wakeup: %lld usecs\n", usecs_late); |
---|
460 |
} |
---|
461 |
#endif |
---|
462 |
|
---|
463 |
// update the x-axis values |
---|
464 |
m_current_time_ticks = m_next_time_ticks; |
---|
465 |
|
---|
466 |
// decide what coefficients to use |
---|
467 |
double coeff_b, coeff_c; |
---|
468 |
if (m_high_bw_updates > 0) { |
---|
469 |
coeff_b = DLL_COEFF_B_HIGH; |
---|
470 |
coeff_c = DLL_COEFF_C_HIGH; |
---|
471 |
m_high_bw_updates--; |
---|
472 |
if (m_high_bw_updates == 0) { |
---|
473 |
debugOutput(DEBUG_LEVEL_VERBOSE, |
---|
474 |
"Switching to low-bandwidth coefficients\n"); |
---|
475 |
} |
---|
476 |
} else { |
---|
477 |
coeff_b = DLL_COEFF_B; |
---|
478 |
coeff_c = DLL_COEFF_C; |
---|
479 |
} |
---|
480 |
|
---|
481 |
// it should be ok to not do this in tick space |
---|
482 |
// since diff_ticks_corr should not be near wrapping |
---|
483 |
// (otherwise we are out of range. we need a few calls |
---|
484 |
// w/o wrapping for this to work. That should not be |
---|
485 |
// an issue as long as the update interval is smaller |
---|
486 |
// than the wrapping interval.) |
---|
487 |
// and coeff_b < 1, hence tmp is not near wrapping |
---|
488 |
|
---|
489 |
double step_ticks = (coeff_b * diff_ticks_corr); |
---|
490 |
debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, |
---|
491 |
"diff_ticks_corr=%f, step_ticks=%f\n", |
---|
492 |
diff_ticks_corr, step_ticks); |
---|
493 |
|
---|
494 |
// the same goes for m_dll_e2, which should be approx equal |
---|
495 |
// to the ticks/usec rate (= 24.576) hence also not near |
---|
496 |
// wrapping |
---|
497 |
step_ticks += m_dll_e2; |
---|
498 |
debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, |
---|
499 |
"add %f ticks to step_ticks => step_ticks=%f\n", |
---|
500 |
m_dll_e2, step_ticks); |
---|
501 |
|
---|
502 |
// it can't be that we have to update to a value in the past |
---|
503 |
if(step_ticks < 0) { |
---|
504 |
debugError("negative step: %f! (correcting to nominal)\n", step_ticks); |
---|
505 |
// recover to an estimated value |
---|
506 |
step_ticks = (double)m_ticks_per_update; |
---|
507 |
} |
---|
508 |
|
---|
509 |
if(step_ticks > TICKS_PER_SECOND) { |
---|
510 |
debugWarning("rather large step: %f ticks (> 1sec)\n", step_ticks); |
---|
511 |
} |
---|
512 |
|
---|
513 |
// now add the step ticks with wrapping. |
---|
514 |
m_next_time_ticks = (double)(addTicks((uint64_t)m_current_time_ticks, (uint64_t)step_ticks)); |
---|
515 |
|
---|
516 |
// update the DLL state |
---|
517 |
m_dll_e2 += coeff_c * diff_ticks_corr; |
---|
518 |
|
---|
519 |
// update the y-axis values |
---|
520 |
m_current_time_usecs = m_next_time_usecs; |
---|
521 |
m_next_time_usecs += m_usecs_per_update; |
---|
522 |
|
---|
523 |
debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, |
---|
524 |
" usecs: current: %f next: %f usecs_late=%lld ticks_late=%lld\n", |
---|
525 |
m_current_time_usecs, m_next_time_usecs, usecs_late, ticks_late); |
---|
526 |
debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, |
---|
527 |
" ticks: current: %f next: %f diff=%f\n", |
---|
528 |
m_current_time_ticks, m_next_time_ticks, diff_ticks); |
---|
529 |
debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, |
---|
530 |
" ticks: current: %011llu (%03us %04ucy %04uticks)\n", |
---|
531 |
(uint64_t)m_current_time_ticks, |
---|
532 |
(unsigned int)TICKS_TO_SECS( (uint64_t)m_current_time_ticks ), |
---|
533 |
(unsigned int)TICKS_TO_CYCLES( (uint64_t)m_current_time_ticks ), |
---|
534 |
(unsigned int)TICKS_TO_OFFSET( (uint64_t)m_current_time_ticks ) ); |
---|
535 |
debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, |
---|
536 |
" ticks: next : %011llu (%03us %04ucy %04uticks)\n", |
---|
537 |
(uint64_t)m_next_time_ticks, |
---|
538 |
(unsigned int)TICKS_TO_SECS( (uint64_t)m_next_time_ticks ), |
---|
539 |
(unsigned int)TICKS_TO_CYCLES( (uint64_t)m_next_time_ticks ), |
---|
540 |
(unsigned int)TICKS_TO_OFFSET( (uint64_t)m_next_time_ticks ) ); |
---|
541 |
|
---|
542 |
debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, |
---|
543 |
" state: local: %11llu, dll_e2: %f, rate: %f\n", |
---|
544 |
local_time, m_dll_e2, getRate()); |
---|
545 |
} |
---|
546 |
|
---|
547 |
// prepare the new compute vars |
---|
548 |
struct compute_vars new_vars; |
---|
549 |
new_vars.ticks = (uint64_t)(m_current_time_ticks); |
---|
550 |
new_vars.usecs = (uint64_t)m_current_time_usecs; |
---|
551 |
new_vars.rate = getRate(); |
---|
552 |
|
---|
553 |
// get the next index |
---|
554 |
unsigned int next_idx = (m_current_shadow_idx + 1) % CTRHELPER_NB_SHADOW_VARS; |
---|
555 |
|
---|
556 |
// update the next index position |
---|
557 |
m_shadow_vars[next_idx] = new_vars; |
---|
558 |
|
---|
559 |
// then we can update the current index |
---|
560 |
m_current_shadow_idx = next_idx; |
---|
561 |
|
---|
562 |
#ifdef DEBUG |
---|
563 |
// do some verification |
---|
564 |
// we re-read a valid ctr timestamp |
---|
565 |
// then we use the attached system time to calculate |
---|
566 |
// the DLL generated timestamp and we check what the |
---|
567 |
// difference is |
---|
568 |
|
---|
569 |
if(!readCycleTimerWithRetry(&cycle_timer, &local_time, 10)) { |
---|
570 |
debugError("Could not read cycle timer register (verify)\n"); |
---|
571 |
return true; // true since this is a check only |
---|
572 |
} |
---|
573 |
cycle_timer_ticks = CYCLE_TIMER_TO_TICKS(cycle_timer); |
---|
574 |
|
---|
575 |
// only check when successful |
---|
576 |
int64_t time_diff = local_time - new_vars.usecs; |
---|
577 |
double y_step_in_ticks = ((double)time_diff) * new_vars.rate; |
---|
578 |
int64_t y_step_in_ticks_int = (int64_t)y_step_in_ticks; |
---|
579 |
uint64_t offset_in_ticks_int = new_vars.ticks; |
---|
580 |
uint32_t dll_time; |
---|
581 |
if (y_step_in_ticks_int > 0) { |
---|
582 |
dll_time = addTicks(offset_in_ticks_int, y_step_in_ticks_int); |
---|
583 |
} else { |
---|
584 |
dll_time = substractTicks(offset_in_ticks_int, -y_step_in_ticks_int); |
---|
585 |
} |
---|
586 |
int32_t ctr_diff = cycle_timer_ticks-dll_time; |
---|
587 |
debugOutput(DEBUG_LEVEL_ULTRA_VERBOSE, "(%p) CTR DIFF: HW %010llu - DLL %010lu = %010ld (%s)\n", |
---|
588 |
this, cycle_timer_ticks, dll_time, ctr_diff, (ctr_diff>0?"lag":"lead")); |
---|
589 |
#endif |
---|
590 |
|
---|
591 |
return true; |
---|
592 |
} |
---|
593 |
|
---|
594 |
uint32_t |
---|
595 |
CycleTimerHelper::getCycleTimerTicks() |
---|
596 |
{ |
---|
597 |
uint64_t now = m_Parent.getCurrentTimeAsUsecs(); |
---|
598 |
return getCycleTimerTicks(now); |
---|
599 |
} |
---|
600 |
|
---|
601 |
uint32_t |
---|
602 |
CycleTimerHelper::getCycleTimerTicks(uint64_t now) |
---|
603 |
{ |
---|
604 |
uint32_t retval; |
---|
605 |
struct compute_vars *my_vars; |
---|
606 |
|
---|
607 |
// get pointer and copy the contents |
---|
608 |
// no locking should be needed since we have more than one |
---|
609 |
// of these vars available, and our use will always be finished before |
---|
610 |
// m_current_shadow_idx changes since this thread's priority should |
---|
611 |
// be higher than the one of the writer thread. Even if not, we only have to ensure |
---|
612 |
// that the used dataset is consistent. We can use an older dataset if it's consistent |
---|
613 |
// since it will also provide a fairly decent extrapolation. |
---|
614 |
my_vars = m_shadow_vars + m_current_shadow_idx; |
---|
615 |
|
---|
616 |
int64_t time_diff = now - my_vars->usecs; |
---|
617 |
double y_step_in_ticks = ((double)time_diff) * my_vars->rate; |
---|
618 |
int64_t y_step_in_ticks_int = (int64_t)y_step_in_ticks; |
---|
619 |
uint64_t offset_in_ticks_int = my_vars->ticks; |
---|
620 |
|
---|
621 |
if (y_step_in_ticks_int > 0) { |
---|
622 |
retval = addTicks(offset_in_ticks_int, y_step_in_ticks_int); |
---|
623 |
/* debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, "y_step_in_ticks_int > 0: %lld, time_diff: %f, rate: %f, retval: %lu\n", |
---|
624 |
y_step_in_ticks_int, time_diff, my_vars.rate, retval);*/ |
---|
625 |
} else { |
---|
626 |
retval = substractTicks(offset_in_ticks_int, -y_step_in_ticks_int); |
---|
627 |
|
---|
628 |
// this can happen if the update thread was woken up earlier than it should have been |
---|
629 |
/* debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, "y_step_in_ticks_int <= 0: %lld, time_diff: %f, rate: %f, retval: %lu\n", |
---|
630 |
y_step_in_ticks_int, time_diff, my_vars.rate, retval);*/ |
---|
631 |
} |
---|
632 |
|
---|
633 |
return retval; |
---|
634 |
} |
---|
635 |
|
---|
636 |
uint32_t |
---|
637 |
CycleTimerHelper::getCycleTimer() |
---|
638 |
{ |
---|
639 |
uint64_t now = m_Parent.getCurrentTimeAsUsecs(); |
---|
640 |
return getCycleTimer(now); |
---|
641 |
} |
---|
642 |
|
---|
643 |
uint32_t |
---|
644 |
CycleTimerHelper::getCycleTimer(uint64_t now) |
---|
645 |
{ |
---|
646 |
uint32_t ticks = getCycleTimerTicks(now); |
---|
647 |
uint32_t result = TICKS_TO_CYCLE_TIMER(ticks); |
---|
648 |
#ifdef DEBUG |
---|
649 |
if(CYCLE_TIMER_TO_TICKS(result) != ticks) { |
---|
650 |
debugWarning("Bad ctr conversion"); |
---|
651 |
} |
---|
652 |
#endif |
---|
653 |
return result; |
---|
654 |
} |
---|
655 |
|
---|
656 |
#else |
---|
657 |
|
---|
658 |
float |
---|
659 |
CycleTimerHelper::getRate() |
---|
660 |
{ |
---|
661 |
return getNominalRate(); |
---|
662 |
} |
---|
663 |
|
---|
664 |
float |
---|
665 |
CycleTimerHelper::getNominalRate() |
---|
666 |
{ |
---|
667 |
float rate = ((double)TICKS_PER_SECOND) / 1000000.0; |
---|
668 |
return rate; |
---|
669 |
} |
---|
670 |
|
---|
671 |
bool |
---|
672 |
CycleTimerHelper::Execute() |
---|
673 |
{ |
---|
674 |
usleep(1000*1000); |
---|
675 |
return true; |
---|
676 |
} |
---|
677 |
|
---|
678 |
uint32_t |
---|
679 |
CycleTimerHelper::getCycleTimerTicks() |
---|
680 |
{ |
---|
681 |
return CYCLE_TIMER_TO_TICKS(getCycleTimer()); |
---|
682 |
} |
---|
683 |
|
---|
684 |
uint32_t |
---|
685 |
CycleTimerHelper::getCycleTimerTicks(uint64_t now) |
---|
686 |
{ |
---|
687 |
debugWarning("not implemented!\n"); |
---|
688 |
return getCycleTimerTicks(); |
---|
689 |
} |
---|
690 |
|
---|
691 |
uint32_t |
---|
692 |
CycleTimerHelper::getCycleTimer() |
---|
693 |
{ |
---|
694 |
uint32_t cycle_timer; |
---|
695 |
uint64_t local_time; |
---|
696 |
readCycleTimerWithRetry(&cycle_timer, &local_time, 10); |
---|
697 |
return cycle_timer; |
---|
698 |
} |
---|
699 |
|
---|
700 |
uint32_t |
---|
701 |
CycleTimerHelper::getCycleTimer(uint64_t now) |
---|
702 |
{ |
---|
703 |
debugWarning("not implemented!\n"); |
---|
704 |
return getCycleTimer(); |
---|
705 |
} |
---|
706 |
|
---|
707 |
#endif |
---|
708 |
|
---|
709 |
bool |
---|
710 |
CycleTimerHelper::readCycleTimerWithRetry(uint32_t *cycle_timer, uint64_t *local_time, int ntries) |
---|
711 |
{ |
---|
712 |
bool good=false; |
---|
713 |
int maxtries = ntries; |
---|
714 |
|
---|
715 |
do { |
---|
716 |
// the ctr read can return 0 sometimes. if that happens, reread the ctr. |
---|
717 |
int maxtries2=ntries; |
---|
718 |
do { |
---|
719 |
if(!m_Parent.readCycleTimerReg(cycle_timer, local_time)) { |
---|
720 |
debugError("Could not read cycle timer register\n"); |
---|
721 |
return false; |
---|
722 |
} |
---|
723 |
if (*cycle_timer == 0) { |
---|
724 |
debugOutput(DEBUG_LEVEL_VERBOSE, |
---|
725 |
"Bogus CTR: %08X on try %02d\n", |
---|
726 |
*cycle_timer, maxtries2); |
---|
727 |
} |
---|
728 |
} while (*cycle_timer == 0 && maxtries2--); |
---|
729 |
|
---|
730 |
// catch bogus ctr reads (can happen) |
---|
731 |
uint64_t cycle_timer_ticks = CYCLE_TIMER_TO_TICKS(*cycle_timer); |
---|
732 |
|
---|
733 |
if (diffTicks(cycle_timer_ticks, m_cycle_timer_ticks_prev) < 0) { |
---|
734 |
debugOutput( DEBUG_LEVEL_VERY_VERBOSE, |
---|
735 |
"non-monotonic CTR (try %02d): %llu -> %llu\n", |
---|
736 |
maxtries, m_cycle_timer_ticks_prev, cycle_timer_ticks); |
---|
737 |
debugOutput( DEBUG_LEVEL_VERY_VERBOSE, |
---|
738 |
" : %08X -> %08X\n", |
---|
739 |
m_cycle_timer_prev, *cycle_timer); |
---|
740 |
debugOutput( DEBUG_LEVEL_VERY_VERBOSE, |
---|
741 |
" current: %011llu (%03us %04ucy %04uticks)\n", |
---|
742 |
cycle_timer_ticks, |
---|
743 |
(unsigned int)TICKS_TO_SECS( cycle_timer_ticks ), |
---|
744 |
(unsigned int)TICKS_TO_CYCLES( cycle_timer_ticks ), |
---|
745 |
(unsigned int)TICKS_TO_OFFSET( cycle_timer_ticks ) ); |
---|
746 |
debugOutput( DEBUG_LEVEL_VERY_VERBOSE, |
---|
747 |
" prev : %011llu (%03us %04ucy %04uticks)\n", |
---|
748 |
m_cycle_timer_ticks_prev, |
---|
749 |
(unsigned int)TICKS_TO_SECS( m_cycle_timer_ticks_prev ), |
---|
750 |
(unsigned int)TICKS_TO_CYCLES( m_cycle_timer_ticks_prev ), |
---|
751 |
(unsigned int)TICKS_TO_OFFSET( m_cycle_timer_ticks_prev ) ); |
---|
752 |
} else { |
---|
753 |
good = true; |
---|
754 |
m_cycle_timer_prev = *cycle_timer; |
---|
755 |
m_cycle_timer_ticks_prev = cycle_timer_ticks; |
---|
756 |
} |
---|
757 |
} while (!good && maxtries--); |
---|
758 |
return true; |
---|
759 |
} |
---|
760 |
|
---|
761 |
void |
---|
762 |
CycleTimerHelper::setVerboseLevel(int l) |
---|
763 |
{ |
---|
764 |
setDebugLevel(l); |
---|
765 |
} |
---|