1 |
/* |
---|
2 |
* Copyright (C) 2005-2007 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 3 of the License, or |
---|
12 |
* (at your option) any later version. |
---|
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 |
|
---|
30 |
#define DLL_BANDWIDTH (0.1) |
---|
31 |
#define DLL_PI (3.141592653589793238) |
---|
32 |
#define DLL_SQRT2 (1.414213562373095049) |
---|
33 |
#define DLL_OMEGA (2.0*DLL_PI*DLL_BANDWIDTH) |
---|
34 |
#define DLL_COEFF_B (DLL_SQRT2 * DLL_OMEGA) |
---|
35 |
#define DLL_COEFF_C (DLL_OMEGA * DLL_OMEGA) |
---|
36 |
|
---|
37 |
#define OFFSET_AVERAGE_COEFF 0.01 |
---|
38 |
/* |
---|
39 |
#define ENTER_CRITICAL_SECTION { \ |
---|
40 |
if (pthread_mutex_trylock(&m_compute_vars_lock) == EBUSY) { \ |
---|
41 |
debugWarning(" (%p) lock clash\n", this); \ |
---|
42 |
ENTER_CRITICAL_SECTION; \ |
---|
43 |
} \ |
---|
44 |
} |
---|
45 |
*/ |
---|
46 |
#define ENTER_CRITICAL_SECTION { \ |
---|
47 |
pthread_mutex_lock(&m_compute_vars_lock); \ |
---|
48 |
} |
---|
49 |
#define EXIT_CRITICAL_SECTION { \ |
---|
50 |
pthread_mutex_unlock(&m_compute_vars_lock); \ |
---|
51 |
} |
---|
52 |
|
---|
53 |
IMPL_DEBUG_MODULE( CycleTimerHelper, CycleTimerHelper, DEBUG_LEVEL_NORMAL ); |
---|
54 |
|
---|
55 |
CycleTimerHelper::CycleTimerHelper(Ieee1394Service &parent, unsigned int update_period_us) |
---|
56 |
: m_Parent ( parent ) |
---|
57 |
, m_ticks_per_update ( ((uint64_t)TICKS_PER_SECOND) * ((uint64_t)update_period_us) / 1000000ULL ) |
---|
58 |
, m_usecs_per_update ( update_period_us ) |
---|
59 |
, m_avg_wakeup_delay ( 0.0 ) |
---|
60 |
, m_dll_e2 ( 0.0 ) |
---|
61 |
, m_current_time_usecs ( 0 ) |
---|
62 |
, m_next_time_usecs ( 0 ) |
---|
63 |
, m_current_time_ticks ( 0 ) |
---|
64 |
, m_next_time_ticks ( 0 ) |
---|
65 |
, m_first_run ( true ) |
---|
66 |
, m_sleep_until ( 0 ) |
---|
67 |
, m_Thread ( NULL ) |
---|
68 |
, m_realtime ( false ) |
---|
69 |
, m_priority ( 0 ) |
---|
70 |
{ |
---|
71 |
debugOutput( DEBUG_LEVEL_VERBOSE, "Create %p...\n", this); |
---|
72 |
} |
---|
73 |
|
---|
74 |
CycleTimerHelper::CycleTimerHelper(Ieee1394Service &parent, unsigned int update_period_us, bool rt, int prio) |
---|
75 |
: m_Parent ( parent ) |
---|
76 |
, m_ticks_per_update ( ((uint64_t)TICKS_PER_SECOND) * ((uint64_t)update_period_us) / 1000000ULL ) |
---|
77 |
, m_usecs_per_update ( update_period_us ) |
---|
78 |
, m_avg_wakeup_delay ( 0.0 ) |
---|
79 |
, m_dll_e2 ( 0.0 ) |
---|
80 |
, m_current_time_usecs ( 0 ) |
---|
81 |
, m_next_time_usecs ( 0 ) |
---|
82 |
, m_current_time_ticks ( 0 ) |
---|
83 |
, m_next_time_ticks ( 0 ) |
---|
84 |
, m_first_run ( true ) |
---|
85 |
, m_sleep_until ( 0 ) |
---|
86 |
, m_Thread ( NULL ) |
---|
87 |
, m_realtime ( rt ) |
---|
88 |
, m_priority ( prio ) |
---|
89 |
{ |
---|
90 |
debugOutput( DEBUG_LEVEL_VERBOSE, "Create %p...\n", this); |
---|
91 |
} |
---|
92 |
|
---|
93 |
CycleTimerHelper::~CycleTimerHelper() |
---|
94 |
{ |
---|
95 |
if (m_Thread) { |
---|
96 |
m_Thread->Stop(); |
---|
97 |
delete m_Thread; |
---|
98 |
} |
---|
99 |
} |
---|
100 |
|
---|
101 |
bool |
---|
102 |
CycleTimerHelper::Start() |
---|
103 |
{ |
---|
104 |
debugOutput( DEBUG_LEVEL_VERBOSE, "Start %p...\n", this); |
---|
105 |
#if IEEE1394SERVICE_USE_CYCLETIMER_DLL |
---|
106 |
m_Thread = new Util::PosixThread(this, m_realtime, m_priority, |
---|
107 |
PTHREAD_CANCEL_DEFERRED); |
---|
108 |
if(!m_Thread) { |
---|
109 |
debugFatal("No thread\n"); |
---|
110 |
return false; |
---|
111 |
} |
---|
112 |
if (m_Thread->Start() != 0) { |
---|
113 |
debugFatal("Could not start update thread\n"); |
---|
114 |
return false; |
---|
115 |
} |
---|
116 |
#endif |
---|
117 |
return true; |
---|
118 |
} |
---|
119 |
|
---|
120 |
bool |
---|
121 |
CycleTimerHelper::Init() |
---|
122 |
{ |
---|
123 |
debugOutput( DEBUG_LEVEL_VERBOSE, "Initialize %p...\n", this); |
---|
124 |
pthread_mutex_init(&m_compute_vars_lock, NULL); |
---|
125 |
return true; |
---|
126 |
} |
---|
127 |
|
---|
128 |
bool |
---|
129 |
CycleTimerHelper::setThreadParameters(bool rt, int priority) { |
---|
130 |
debugOutput( DEBUG_LEVEL_VERBOSE, "(%p) switch to: (rt=%d, prio=%d)...\n", this, rt, priority); |
---|
131 |
if (priority > THREAD_MAX_RTPRIO) priority = THREAD_MAX_RTPRIO; // cap the priority |
---|
132 |
m_realtime = rt; |
---|
133 |
m_priority = priority; |
---|
134 |
|
---|
135 |
#if IEEE1394SERVICE_USE_CYCLETIMER_DLL |
---|
136 |
if (m_Thread) { |
---|
137 |
if (m_realtime) { |
---|
138 |
m_Thread->AcquireRealTime(m_priority); |
---|
139 |
} else { |
---|
140 |
m_Thread->DropRealTime(); |
---|
141 |
} |
---|
142 |
} |
---|
143 |
#endif |
---|
144 |
|
---|
145 |
return true; |
---|
146 |
} |
---|
147 |
|
---|
148 |
#if IEEE1394SERVICE_USE_CYCLETIMER_DLL |
---|
149 |
float |
---|
150 |
CycleTimerHelper::getRate() |
---|
151 |
{ |
---|
152 |
float rate = (float)(diffTicks((uint64_t)m_next_time_ticks, (uint64_t)m_current_time_ticks)); |
---|
153 |
rate /= (float)(m_next_time_usecs - m_current_time_usecs); |
---|
154 |
return rate; |
---|
155 |
} |
---|
156 |
|
---|
157 |
float |
---|
158 |
CycleTimerHelper::getNominalRate() |
---|
159 |
{ |
---|
160 |
float rate = ((double)TICKS_PER_SECOND) / 1000000.0; |
---|
161 |
return rate; |
---|
162 |
} |
---|
163 |
|
---|
164 |
bool |
---|
165 |
CycleTimerHelper::Execute() |
---|
166 |
{ |
---|
167 |
debugOutput( DEBUG_LEVEL_VERY_VERBOSE, "Execute %p...\n", this); |
---|
168 |
if (!m_first_run) { |
---|
169 |
// wait for the next update period |
---|
170 |
ffado_microsecs_t now = m_TimeSource.getCurrentTimeAsUsecs(); |
---|
171 |
int sleep_time = m_sleep_until - now; |
---|
172 |
debugOutput( DEBUG_LEVEL_VERY_VERBOSE, "(%p) Sleep until %lld/%f (now: %lld, diff=%d) ...\n", |
---|
173 |
this, m_sleep_until, m_next_time_usecs, now, sleep_time); |
---|
174 |
m_TimeSource.SleepUsecAbsolute(m_sleep_until); |
---|
175 |
debugOutput( DEBUG_LEVEL_VERY_VERBOSE, " (%p) back...\n", this); |
---|
176 |
} |
---|
177 |
|
---|
178 |
uint32_t cycle_timer; |
---|
179 |
uint64_t local_time; |
---|
180 |
if(!m_Parent.readCycleTimerReg(&cycle_timer, &local_time)) { |
---|
181 |
debugError("Could not read cycle timer register\n"); |
---|
182 |
return false; |
---|
183 |
} |
---|
184 |
debugOutput( DEBUG_LEVEL_VERY_VERBOSE, " read : CTR: %11lu, local: %17llu\n", |
---|
185 |
cycle_timer, local_time); |
---|
186 |
|
---|
187 |
if (m_first_run) { |
---|
188 |
m_sleep_until = local_time + m_usecs_per_update; |
---|
189 |
m_dll_e2 = m_ticks_per_update; |
---|
190 |
m_current_time_usecs = local_time; |
---|
191 |
m_next_time_usecs = m_current_time_usecs + m_usecs_per_update; |
---|
192 |
m_current_time_ticks = CYCLE_TIMER_TO_TICKS( cycle_timer ); |
---|
193 |
m_next_time_ticks = addTicks( (uint64_t)m_current_time_ticks, (uint64_t)m_dll_e2); |
---|
194 |
debugOutput( DEBUG_LEVEL_VERBOSE, " First run\n"); |
---|
195 |
debugOutput( DEBUG_LEVEL_VERBOSE, " usecs/update: %lu, ticks/update: %lu, m_dll_e2: %f\n", |
---|
196 |
m_usecs_per_update, m_ticks_per_update, m_dll_e2); |
---|
197 |
debugOutput( DEBUG_LEVEL_VERBOSE, " usecs current: %f, next: %f\n", m_current_time_usecs, m_next_time_usecs); |
---|
198 |
debugOutput( DEBUG_LEVEL_VERBOSE, " ticks current: %f, next: %f\n", m_current_time_ticks, m_next_time_ticks); |
---|
199 |
m_first_run = false; |
---|
200 |
} else { |
---|
201 |
m_sleep_until += m_usecs_per_update; |
---|
202 |
uint64_t cycle_timer_ticks = CYCLE_TIMER_TO_TICKS(cycle_timer); |
---|
203 |
double usecs_late = ((double)local_time) - (m_next_time_usecs); |
---|
204 |
|
---|
205 |
// update the x-axis values |
---|
206 |
double diff_ticks = diffTicks(cycle_timer_ticks, (int64_t)m_next_time_ticks); |
---|
207 |
m_current_time_ticks = m_next_time_ticks; |
---|
208 |
// do the calculation in 'tick space' |
---|
209 |
int64_t tmp = (uint64_t)(DLL_COEFF_B * diff_ticks); |
---|
210 |
if(m_dll_e2 > 0) { |
---|
211 |
tmp = addTicks(tmp, (uint64_t)m_dll_e2); |
---|
212 |
} else { |
---|
213 |
tmp = substractTicks(tmp, (uint64_t)(-m_dll_e2)); |
---|
214 |
} |
---|
215 |
if(tmp < 0) { |
---|
216 |
debugWarning("negative slope: %lld!\n", tmp); |
---|
217 |
} |
---|
218 |
m_next_time_ticks = addTicks((uint64_t)m_current_time_ticks, tmp); |
---|
219 |
|
---|
220 |
// it should be ok to not do this in tick space since it's value |
---|
221 |
// is approx equal to the rate, being 24.576 ticks/usec |
---|
222 |
m_dll_e2 += DLL_COEFF_C * diff_ticks; |
---|
223 |
|
---|
224 |
// update the y-axis values |
---|
225 |
m_current_time_usecs = m_next_time_usecs; |
---|
226 |
m_next_time_usecs = local_time + m_usecs_per_update; |
---|
227 |
|
---|
228 |
debugOutput( DEBUG_LEVEL_VERY_VERBOSE, " usecs: current: %f next: %f usecs_late=%f\n", |
---|
229 |
m_current_time_usecs, m_next_time_usecs, usecs_late); |
---|
230 |
debugOutput( DEBUG_LEVEL_VERY_VERBOSE, " ticks: current: %f next: %f diff=%f\n", |
---|
231 |
m_current_time_ticks, m_next_time_ticks, diff_ticks); |
---|
232 |
|
---|
233 |
debugOutput( DEBUG_LEVEL_VERY_VERBOSE, " state: local: %11llu, dll_e2: %f, rate: %f\n", |
---|
234 |
local_time, m_dll_e2, getRate()); |
---|
235 |
} |
---|
236 |
|
---|
237 |
// FIXME: priority inversion! |
---|
238 |
ENTER_CRITICAL_SECTION; |
---|
239 |
m_current_vars.ticks = (uint64_t)(m_current_time_ticks);// + m_average_offset_ticks); |
---|
240 |
m_current_vars.usecs = (uint64_t)m_current_time_usecs; |
---|
241 |
m_current_vars.rate = getRate(); |
---|
242 |
EXIT_CRITICAL_SECTION; |
---|
243 |
|
---|
244 |
return true; |
---|
245 |
} |
---|
246 |
|
---|
247 |
uint32_t |
---|
248 |
CycleTimerHelper::getCycleTimerTicks() |
---|
249 |
{ |
---|
250 |
uint64_t now = m_Parent.getCurrentTimeAsUsecs(); |
---|
251 |
return getCycleTimerTicks(now); |
---|
252 |
} |
---|
253 |
|
---|
254 |
uint32_t |
---|
255 |
CycleTimerHelper::getCycleTimerTicks(uint64_t now) |
---|
256 |
{ |
---|
257 |
uint32_t retval; |
---|
258 |
struct compute_vars my_vars; |
---|
259 |
|
---|
260 |
// reduce lock contention |
---|
261 |
ENTER_CRITICAL_SECTION; |
---|
262 |
my_vars = m_current_vars; |
---|
263 |
EXIT_CRITICAL_SECTION; |
---|
264 |
|
---|
265 |
int64_t time_diff = now - my_vars.usecs; |
---|
266 |
double y_step_in_ticks = ((double)time_diff) * my_vars.rate; |
---|
267 |
int64_t y_step_in_ticks_int = (int64_t)y_step_in_ticks; |
---|
268 |
uint64_t offset_in_ticks_int = my_vars.ticks; |
---|
269 |
|
---|
270 |
if (y_step_in_ticks_int > 0) { |
---|
271 |
retval = addTicks(offset_in_ticks_int, y_step_in_ticks_int); |
---|
272 |
debugOutput(DEBUG_LEVEL_VERY_VERBOSE, "y_step_in_ticks_int > 0: %lld, time_diff: %f, rate: %f, retval: %lu\n", |
---|
273 |
y_step_in_ticks_int, time_diff, my_vars.rate, retval); |
---|
274 |
} else { |
---|
275 |
retval = substractTicks(offset_in_ticks_int, -y_step_in_ticks_int); |
---|
276 |
|
---|
277 |
// this can happen if the update thread was woken up earlier than it should have been |
---|
278 |
debugOutput(DEBUG_LEVEL_VERY_VERBOSE, "y_step_in_ticks_int <= 0: %lld, time_diff: %f, rate: %f, retval: %lu\n", |
---|
279 |
y_step_in_ticks_int, time_diff, my_vars.rate, retval); |
---|
280 |
} |
---|
281 |
|
---|
282 |
return retval; |
---|
283 |
} |
---|
284 |
|
---|
285 |
uint32_t |
---|
286 |
CycleTimerHelper::getCycleTimer() |
---|
287 |
{ |
---|
288 |
uint64_t now = m_Parent.getCurrentTimeAsUsecs(); |
---|
289 |
return getCycleTimer(now); |
---|
290 |
} |
---|
291 |
|
---|
292 |
uint32_t |
---|
293 |
CycleTimerHelper::getCycleTimer(uint64_t now) |
---|
294 |
{ |
---|
295 |
uint32_t ticks = getCycleTimerTicks(now); |
---|
296 |
uint32_t result = TICKS_TO_CYCLE_TIMER(ticks); |
---|
297 |
#ifdef DEBUG |
---|
298 |
if(CYCLE_TIMER_TO_TICKS(result) != ticks) { |
---|
299 |
debugWarning("Bad ctr conversion"); |
---|
300 |
} |
---|
301 |
#endif |
---|
302 |
return result; |
---|
303 |
} |
---|
304 |
|
---|
305 |
#else |
---|
306 |
|
---|
307 |
float |
---|
308 |
CycleTimerHelper::getRate() |
---|
309 |
{ |
---|
310 |
return getNominalRate(); |
---|
311 |
} |
---|
312 |
|
---|
313 |
float |
---|
314 |
CycleTimerHelper::getNominalRate() |
---|
315 |
{ |
---|
316 |
float rate = ((double)TICKS_PER_SECOND) / 1000000.0; |
---|
317 |
return rate; |
---|
318 |
} |
---|
319 |
|
---|
320 |
bool |
---|
321 |
CycleTimerHelper::Execute() |
---|
322 |
{ |
---|
323 |
usleep(1000*1000); |
---|
324 |
return true; |
---|
325 |
} |
---|
326 |
uint32_t |
---|
327 |
CycleTimerHelper::getCycleTimerTicks() |
---|
328 |
{ |
---|
329 |
uint32_t cycle_timer; |
---|
330 |
uint64_t local_time; |
---|
331 |
if(!m_Parent.readCycleTimerReg(&cycle_timer, &local_time)) { |
---|
332 |
debugError("Could not read cycle timer register\n"); |
---|
333 |
return 0; |
---|
334 |
} |
---|
335 |
return CYCLE_TIMER_TO_TICKS(cycle_timer); |
---|
336 |
} |
---|
337 |
|
---|
338 |
uint32_t |
---|
339 |
CycleTimerHelper::getCycleTimerTicks(uint64_t now) |
---|
340 |
{ |
---|
341 |
debugWarning("not implemented!\n"); |
---|
342 |
return getCycleTimerTicks(); |
---|
343 |
} |
---|
344 |
|
---|
345 |
uint32_t |
---|
346 |
CycleTimerHelper::getCycleTimer() |
---|
347 |
{ |
---|
348 |
uint32_t cycle_timer; |
---|
349 |
uint64_t local_time; |
---|
350 |
if(!m_Parent.readCycleTimerReg(&cycle_timer, &local_time)) { |
---|
351 |
debugError("Could not read cycle timer register\n"); |
---|
352 |
return 0; |
---|
353 |
} |
---|
354 |
return cycle_timer; |
---|
355 |
} |
---|
356 |
|
---|
357 |
uint32_t |
---|
358 |
CycleTimerHelper::getCycleTimer(uint64_t now) |
---|
359 |
{ |
---|
360 |
debugWarning("not implemented!\n"); |
---|
361 |
return getCycleTimer(); |
---|
362 |
} |
---|
363 |
|
---|
364 |
#endif |
---|
365 |
|
---|
366 |
void |
---|
367 |
CycleTimerHelper::setVerboseLevel(int l) |
---|
368 |
{ |
---|
369 |
setDebugLevel(l); |
---|
370 |
} |
---|