root/trunk/libffado/src/libieee1394/CycleTimerHelper.cpp

Revision 864, 11.1 kB (checked in by ppalmers, 13 years ago)

update license to GPLv2 or GPLv3 instead of GPLv2 or any later version. Update copyrights to reflect the new year

Line 
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
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 }
Note: See TracBrowser for help on using the browser.