root/branches/streaming-rework/src/libstreaming/IsoHandler.cpp

Revision 385, 35.2 kB (checked in by pieterpalmers, 16 years ago)

- fixed issues with SYT timestamp processing
- SYT based sync works if syncing to the received stream

Line 
1 /* $Id$ */
2
3 /*
4  *   FreeBob Streaming API
5  *   FreeBob = Firewire (pro-)audio for linux
6  *
7  *   http://freebob.sf.net
8  *
9  *   Copyright (C) 2006 Pieter Palmers <pieterpalmers@users.sourceforge.net>
10  *
11  *   This program is free software {} you can redistribute it and/or modify
12  *   it under the terms of the GNU General Public License as published by
13  *   the Free Software Foundation {} either version 2 of the License, or
14  *   (at your option) any later version.
15  *
16  *   This program is distributed in the hope that it will be useful,
17  *   but WITHOUT ANY WARRANTY {} without even the implied warranty of
18  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
19  *   GNU General Public License for more details.
20  *
21  *   You should have received a copy of the GNU General Public License
22  *   along with this program {} if not, write to the Free Software
23  *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
24  *
25  *
26  *
27  */
28
29 #include "IsoHandler.h"
30 #include "IsoStream.h"
31 #include "cycletimer.h"
32
33 #include "libutil/TimeSource.h"
34 #include "libutil/SystemTimeSource.h"
35
36 #include <errno.h>
37 #include <netinet/in.h>
38 #include <assert.h>
39 #include <unistd.h>
40 #include <string.h>
41
42 #include <iostream>
43 using namespace std;
44
45 #define CC_SLEEP_TIME_AFTER_UPDATE    1000
46 #define CC_SLEEP_TIME_AFTER_FAILURE     10
47 #define CC_DLL_COEFF     ((0.001)*((float)(CC_SLEEP_TIME_AFTER_UPDATE/1000.0)))
48
49 #define CC_MAX_RATE_ERROR           (2.0/100.0)
50 #define CC_INIT_MAX_TRIES 10
51
52
53 namespace FreebobStreaming
54 {
55
56 IMPL_DEBUG_MODULE( IsoHandler, IsoHandler, DEBUG_LEVEL_NORMAL );
57
58 /* the C callbacks */
59 enum raw1394_iso_disposition
60 IsoXmitHandler::iso_transmit_handler(raw1394handle_t handle,
61                 unsigned char *data, unsigned int *length,
62                 unsigned char *tag, unsigned char *sy,
63                 int cycle, unsigned int dropped) {
64
65         IsoXmitHandler *xmitHandler=static_cast<IsoXmitHandler *>(raw1394_get_userdata(handle));
66         assert(xmitHandler);
67
68         return xmitHandler->getPacket(data, length, tag, sy, cycle, dropped);
69 }
70
71 enum raw1394_iso_disposition
72 IsoRecvHandler::iso_receive_handler(raw1394handle_t handle, unsigned char *data,
73                                                 unsigned int length, unsigned char channel,
74                                                 unsigned char tag, unsigned char sy, unsigned int cycle,
75                                                 unsigned int dropped) {
76
77         IsoRecvHandler *recvHandler=static_cast<IsoRecvHandler *>(raw1394_get_userdata(handle));
78         assert(recvHandler);
79
80         return recvHandler->putPacket(data, length, channel, tag, sy, cycle, dropped);
81 }
82
83 int IsoHandler::busreset_handler(raw1394handle_t handle, unsigned int generation)
84 {       
85         debugOutput( DEBUG_LEVEL_VERBOSE, "Busreset happened, generation %d...\n", generation);
86
87         IsoHandler *handler=static_cast<IsoHandler *>(raw1394_get_userdata(handle));
88         assert(handler);
89         return handler->handleBusReset(generation);
90 }
91
92
93 /* Base class implementation */
94 IsoHandler::IsoHandler(int port)
95    : TimeSource(), m_handle(0), m_handle_util(0), m_port(port),
96    m_buf_packets(400), m_max_packet_size(1024), m_irq_interval(-1),
97    m_cycletimer_ticks(0), m_lastmeas_usecs(0), m_ticks_per_usec(24.576),
98    m_ticks_per_usec_dll_err2(0),
99    m_packetcount(0), m_dropped(0), m_Client(0),
100    m_State(E_Created), m_TimeSource_LastSecs(0),m_TimeSource_NbCycleWraps(0)
101 {
102     m_TimeSource=new FreebobUtil::SystemTimeSource();
103 }
104
105 IsoHandler::IsoHandler(int port, unsigned int buf_packets, unsigned int max_packet_size, int irq)
106    : TimeSource(), m_handle(0), m_port(port),
107    m_buf_packets(buf_packets), m_max_packet_size( max_packet_size),
108    m_irq_interval(irq),
109    m_cycletimer_ticks(0), m_lastmeas_usecs(0), m_ticks_per_usec(24.576),
110    m_ticks_per_usec_dll_err2(0),
111    m_packetcount(0), m_dropped(0), m_Client(0),
112    m_State(E_Created), m_TimeSource_LastSecs(0),m_TimeSource_NbCycleWraps(0)
113 {
114     m_TimeSource=new FreebobUtil::SystemTimeSource();
115 }
116
117 IsoHandler::~IsoHandler() {
118
119 // Don't call until libraw1394's raw1394_new_handle() function has been
120 // fixed to correctly initialise the iso_packet_infos field.  Bug is
121 // confirmed present in libraw1394 1.2.1.  In any case,
122 // raw1394_destroy_handle() will do any iso system shutdown required.
123 //     raw1394_iso_shutdown(m_handle);
124
125     if(m_handle) {
126         if (m_State == E_Running) {
127             stop();
128         }
129        
130         raw1394_destroy_handle(m_handle);
131     }
132    
133     if(m_handle_util) raw1394_destroy_handle(m_handle_util);
134    
135     if (m_TimeSource) delete m_TimeSource;
136 }
137
138 bool
139 IsoHandler::init()
140 {
141     debugOutput( DEBUG_LEVEL_VERBOSE, "IsoHandler (%p) enter...\n",this);
142
143     // check the state
144     if(m_State != E_Created) {
145         debugError("Incorrect state, expected E_Created, got %d\n",(int)m_State);
146         return false;
147     }
148    
149     // the main handle for the ISO traffic
150     m_handle = raw1394_new_handle_on_port( m_port );
151     if ( !m_handle ) {
152         if ( !errno ) {
153             debugError("libraw1394 not compatible\n");
154         } else {
155             debugError("Could not get 1394 handle: %s", strerror(errno) );
156             debugError("Are ieee1394 and raw1394 drivers loaded?");
157         }
158         return false;
159     }
160     raw1394_set_userdata(m_handle, static_cast<void *>(this));
161    
162     // a second handle for utility stuff
163     m_handle_util = raw1394_new_handle_on_port( m_port );
164     if ( !m_handle_util ) {
165         if ( !errno ) {
166             debugError("libraw1394 not compatible\n");
167         } else {
168             debugError("Could not get 1394 handle: %s", strerror(errno) );
169             debugError("Are ieee1394 and raw1394 drivers loaded?");
170         }
171        
172         raw1394_destroy_handle(m_handle);
173         return false;
174     }
175     raw1394_set_userdata(m_handle_util, static_cast<void *>(this));
176        
177     // bus reset handling
178     if(raw1394_busreset_notify (m_handle, RAW1394_NOTIFY_ON)) {
179         debugWarning("Could not enable busreset notification.\n");
180         debugWarning(" Error message: %s\n",strerror(errno));
181         debugWarning("Continuing without bus reset support.\n");
182     } else {
183         // apparently this cannot fail
184         raw1394_set_bus_reset_handler(m_handle, busreset_handler);
185     }
186
187     // initialize the local timesource
188     m_TimeSource_NbCycleWraps=0;
189     unsigned int new_timer;
190    
191 #ifdef LIBRAW1394_USE_CTRREAD_API
192     struct raw1394_cycle_timer ctr;
193     int err;
194     err=raw1394_read_cycle_timer(m_handle_util, &ctr);
195     if(err) {
196         debugWarning("raw1394_read_cycle_timer: %s", strerror(err));
197     }
198     new_timer=ctr.cycle_timer;
199 #else
200     // normally we should be able to use the same handle
201     // because it is not iterated on by any other stuff
202     // but I'm not sure
203     quadlet_t buf=0;
204     raw1394_read(m_handle_util, raw1394_get_local_id(m_handle_util),
205         CSR_REGISTER_BASE | CSR_CYCLE_TIME, 4, &buf);
206     new_timer= ntohl(buf) & 0xFFFFFFFF;
207 #endif
208
209     m_TimeSource_LastSecs=CYCLE_TIMER_GET_SECS(new_timer);
210
211     // update the cycle timer value for initial value
212     initCycleTimer();
213
214     // update the internal state
215     m_State=E_Initialized;
216    
217     return true;
218 }
219
220 bool IsoHandler::prepare()
221 {
222     debugOutput( DEBUG_LEVEL_VERBOSE, "IsoHandler (%p) enter...\n",this);
223
224     // check the state
225     if(m_State != E_Initialized) {
226         debugError("Incorrect state, expected E_Initialized, got %d\n",(int)m_State);
227         return false;
228     }
229    
230     // Don't call until libraw1394's raw1394_new_handle() function has been
231     // fixed to correctly initialise the iso_packet_infos field.  Bug is
232     // confirmed present in libraw1394 1.2.1.
233
234 //     raw1394_iso_shutdown(m_handle);
235    
236     m_State = E_Prepared;
237    
238     return true;
239 }
240
241 bool IsoHandler::start(int cycle)
242 {
243     debugOutput( DEBUG_LEVEL_VERBOSE, "enter...\n");
244    
245     // check the state
246     if(m_State != E_Prepared) {
247         debugError("Incorrect state, expected E_Prepared, got %d\n",(int)m_State);
248         return false;
249     }
250
251     m_State=E_Running;
252
253     return true;
254 }
255
256 bool IsoHandler::stop()
257 {
258     debugOutput( DEBUG_LEVEL_VERBOSE, "enter...\n");
259    
260     // check state
261     if(m_State != E_Running) {
262         debugError("Incorrect state, expected E_Running, got %d\n",(int)m_State);
263         return false;
264     }
265    
266     // this is put here to try and avoid the
267     // Runaway context problem
268     // don't know if it will help though.
269     raw1394_iso_xmit_sync(m_handle);
270    
271     raw1394_iso_stop(m_handle);
272    
273     m_State=E_Prepared;
274    
275     return true;
276 }
277
278 bool
279 IsoHandler::setSyncMaster(FreebobUtil::TimeSource *t)
280 {
281     m_TimeSource=t;
282    
283     // update the cycle timer value for initial value
284     initCycleTimer();
285    
286     return true;
287 }
288
289 /**
290  * Bus reset handler
291  *
292  * @return ?
293  */
294  
295 int IsoHandler::handleBusReset(unsigned int generation) {
296     debugOutput( DEBUG_LEVEL_VERBOSE, "bus reset...\n");
297    
298     // as busreset can elect a new cycle master,
299     // we need to re-initialize our timing code
300     initCycleTimer();
301    
302     return 0;
303 }
304
305 /**
306  * Returns the current value of the cycle timer (in ticks)
307  *
308  * @return the current value of the cycle timer (in ticks)
309  */
310
311 unsigned int IsoHandler::getCycleTimerTicks() {
312
313 #ifdef LIBRAW1394_USE_CTRREAD_API
314     // the new api should be realtime safe.
315     // it might cause a reschedule when turning preemption,
316     // back on but that won't hurt us if we have sufficient
317     // priority
318     struct raw1394_cycle_timer ctr;
319     int err;
320     err=raw1394_read_cycle_timer(m_handle_util, &ctr);
321     if(err) {
322         debugWarning("raw1394_read_cycle_timer: %s", strerror(err));
323     }
324     return CYCLE_TIMER_TO_TICKS(ctr.cycle_timer);
325
326 #else
327     // use the estimated version
328     freebob_microsecs_t now;
329     now=m_TimeSource->getCurrentTimeAsUsecs();
330     return mapToCycleTimer(now);
331 #endif
332
333 }
334
335 /**
336  * Returns the current value of the cycle timer (as is)
337  *
338  * @return the current value of the cycle timer (as is)
339  */
340
341 unsigned int IsoHandler::getCycleTimer() {
342
343 #ifdef LIBRAW1394_USE_CTRREAD_API
344     // the new api should be realtime safe.
345     // it might cause a reschedule when turning preemption,
346     // back on but that won't hurt us if we have sufficient
347     // priority
348     struct raw1394_cycle_timer ctr;
349     int err;
350     err=raw1394_read_cycle_timer(m_handle_util, &ctr);
351     if(err) {
352         debugWarning("raw1394_read_cycle_timer: %s", strerror(err));
353     }
354     return ctr.cycle_timer;
355
356 #else
357     // use the estimated version
358     freebob_microsecs_t now;
359     now=m_TimeSource->getCurrentTimeAsUsecs();
360     return TICKS_TO_CYCLE_TIMER(mapToCycleTimer(now));
361 #endif
362
363 }
364 /**
365  * Maps a value of the active TimeSource to a Cycle Timer value.
366  *
367  * This is usefull if you know a time value and want the corresponding
368  * Cycle Timer value. Note that the value shouldn't be too far off
369  * the current time, because then the mapping can be bad.
370  *
371  * @return the value of the cycle timer (in ticks)
372  */
373
374 unsigned int IsoHandler::mapToCycleTimer(freebob_microsecs_t now) {
375
376     // linear interpolation
377     int delta_usecs=now-m_lastmeas_usecs;
378
379     float offset=m_ticks_per_usec * ((float)delta_usecs);
380
381     int64_t pred_ticks=(int64_t)m_cycletimer_ticks+(int64_t)offset;
382
383     if (pred_ticks < 0) {
384         debugWarning("Predicted ticks < 0\n");
385     }
386     debugOutput(DEBUG_LEVEL_VERBOSE,"now=%llu, m_lastmeas_usec=%llu, delta_usec=%d\n",
387             now, m_lastmeas_usecs, delta_usecs);
388     debugOutput(DEBUG_LEVEL_VERBOSE,"t/usec=%f, offset=%f, m_cc_t=%llu, pred_ticks=%lld\n",
389             m_ticks_per_usec, offset, m_cycletimer_ticks, pred_ticks);
390
391     // if we need to wrap, do it
392     if (pred_ticks > TICKS_PER_SECOND * 128L) {
393         pred_ticks -= TICKS_PER_SECOND * 128L;
394     }
395    
396     return pred_ticks;
397 }
398
399 /**
400  * Maps a Cycle Timer value (in ticks) of the active TimeSource's unit.
401  *
402  * This is usefull if you know a Cycle Timer value and want the corresponding
403  * timesource value. Note that the value shouldn't be too far off
404  * the current cycle timer, because then the mapping can be bad.
405  *
406  * @return the mapped value
407  */
408
409 freebob_microsecs_t IsoHandler::mapToTimeSource(unsigned int cc) {
410
411     // linear interpolation
412     int delta_cc=cc-m_cycletimer_ticks;
413
414     float offset= ((float)delta_cc) / m_ticks_per_usec;
415
416     int64_t pred_time=(int64_t)m_lastmeas_usecs+(int64_t)offset;
417
418     if (pred_time < 0) {
419         debugWarning("Predicted time < 0\n");
420         debugOutput(DEBUG_LEVEL_VERBOSE,"cc=%u, m_cycletimer_ticks=%llu, delta_cc=%d\n",
421                 cc, m_cycletimer_ticks, delta_cc);
422         debugOutput(DEBUG_LEVEL_VERBOSE,"t/usec=%f, offset=%f, m_lastmeas_usecs=%llu, pred_time=%lld\n",
423                 m_ticks_per_usec, offset, m_lastmeas_usecs, pred_time);   
424     }
425
426
427     return pred_time;
428 }
429
430 bool IsoHandler::updateCycleTimer() {
431     freebob_microsecs_t prev_usecs=m_lastmeas_usecs;
432     uint64_t prev_ticks=m_cycletimer_ticks;
433    
434     freebob_microsecs_t new_usecs;
435     uint64_t new_ticks;
436     unsigned int new_timer;
437    
438     /* To estimate the cycle timer, we implement a
439        DLL based routine, that maps the cycle timer
440        on the system clock.
441        
442        For more info, refer to:
443         "Using a DLL to filter time"
444         Fons Adriaensen
445        
446         Can be found at:
447         http://users.skynet.be/solaris/linuxaudio/downloads/usingdll.pdf
448         or maybe at:
449         http://www.kokkinizita.net/linuxaudio
450    
451         Basically what we do is estimate the next point (T1,CC1_est)
452         based upon the previous point (T0, CC0) and the estimated rate (R).
453         Then we compare our estimation with the measured cycle timer
454         at T1 (=CC1_meas). We then calculate the estimation error on R:
455         err=(CC1_meas-CC0)/(T1-T2) - (CC1_est-CC0)/(T1-T2)
456         and try to minimize this on average (DLL)
457        
458         Note that in order to have a contignous mapping, we should
459         update CC0<=CC1_est instead of CC0<=CC1_meas. The measurement
460         serves only to correct the error 'on average'.
461        
462         In the code, the following variable names are used:
463         T0=prev_usecs
464         T1=next_usecs
465        
466         CC0=prev_ticks
467         CC1_est=est_ticks
468         CC1_meas=meas_ticks
469        
470      */
471 #ifdef LIBRAW1394_USE_CTRREAD_API
472     struct raw1394_cycle_timer ctr;
473     int err;
474     err=raw1394_read_cycle_timer(m_handle_util, &ctr);
475     if(err) {
476         debugWarning("raw1394_read_cycle_timer: %s", strerror(err));
477     }
478     new_usecs=(freebob_microsecs_t)ctr.local_time;
479     new_timer=ctr.cycle_timer;
480 #else
481     // normally we should be able to use the same handle
482     // because it is not iterated on by any other stuff
483     // but I'm not sure
484     quadlet_t buf=0;
485     raw1394_read(m_handle_util, raw1394_get_local_id(m_handle_util),
486         CSR_REGISTER_BASE | CSR_CYCLE_TIME, 4, &buf);
487     new_usecs=m_TimeSource->getCurrentTimeAsUsecs();
488     new_timer= ntohl(buf) & 0xFFFFFFFF;
489 #endif   
490
491     new_ticks=CYCLE_TIMER_TO_TICKS(new_timer);
492
493     // the difference in system time
494     int64_t delta_usecs=new_usecs-prev_usecs;
495     // this cannot be 0, because m_TimeSource->getCurrentTimeAsUsecs should
496     // never return the same value (maybe in future terrahz processors?)
497     assert(delta_usecs);
498    
499     // the measured cycle timer difference
500     int64_t delta_ticks_meas;
501     if (new_ticks >= prev_ticks) {
502         delta_ticks_meas=new_ticks - prev_ticks;
503     } else { // wraparound
504         delta_ticks_meas=CYCLE_TIMER_UNWRAP_TICKS(new_ticks) - prev_ticks;
505     }
506    
507     // the estimated cycle timer difference
508     int64_t delta_ticks_est=(int64_t)(m_ticks_per_usec * ((float)delta_usecs));
509    
510     // the measured & estimated rate
511     float rate_meas=((double)delta_ticks_meas/(double)delta_usecs);
512     float rate_est=((float)m_ticks_per_usec);
513    
514     // these make sure we don't update when the measurement is
515     // bad. We know the nominal rate, and it can't be that far
516     // off. The thing is that there is a problem in measuring
517     // both usecs and ticks at the same time (no provision in
518     // the kernel.
519     // We know that there are some tolerances on both
520     // the system clock and the firewire clock such that the
521     // actual difference is rather small. So we discard values
522     // that are too far from the nominal rate.
523     // Otherwise the DLL has to have a very low bandwidth, in
524     // order not to be desturbed too much by these bad measurements
525     // resulting in very slow locking.
526    
527     if (   (rate_meas < 24.576*(1.0+CC_MAX_RATE_ERROR))
528         && (rate_meas > 24.576*(1.0-CC_MAX_RATE_ERROR))) {
529
530 #ifdef DEBUG
531
532         int64_t diff=(int64_t)delta_ticks_est;
533        
534         // calculate the difference in predicted ticks and
535         // measured ticks
536         diff -= delta_ticks_meas;
537        
538        
539         if (diff > 24000L || diff < -24000L) { // approx +/-1 msec error
540             debugOutput(DEBUG_LEVEL_VERBOSE,"Bad pred (%p): diff=%lld, dt_est=%lld, dt_meas=%lld, d=%lldus, err=%fus\n", this,
541                 diff, delta_ticks_est, delta_ticks_meas, delta_usecs, (((float)diff)/24.576)
542                 );
543         } else {
544             debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"Good pred: diff=%lld, dt_est=%lld, dt_meas=%lld, d=%lldus, err=%fus\n",
545                 diff, delta_ticks_est, delta_ticks_meas, delta_usecs, (((float)diff)/24.576)
546                 );
547         }
548 #endif
549         // DLL the error to obtain the rate.
550         // (note: the DLL makes the error=0)
551         // only update the DLL if the rate is within 10% of the expected
552         // rate
553         float err=rate_meas-rate_est;
554        
555         // 2nd order DLL update
556 //         const float w=6.28*0.0001;
557 //         const float b=w*1.45;
558 //         const float c=w*w;
559 //         
560 //         m_ticks_per_usec += b*err + m_ticks_per_usec_dll_err2;
561 //         m_ticks_per_usec_dll_err2 += c * err;
562
563         // first order DLL update
564          m_ticks_per_usec += CC_DLL_COEFF*err;
565    
566         if (   (m_ticks_per_usec > 24.576*(1.0+CC_MAX_RATE_ERROR))
567             || (m_ticks_per_usec < 24.576*(1.0-CC_MAX_RATE_ERROR))) {
568             debugOutput(DEBUG_LEVEL_VERBOSE, "Warning: DLL ticks/usec near clipping (%8.4f)\n",
569                         m_ticks_per_usec);
570         }
571        
572         // update the internal values
573         // note: the next cycletimer point is
574         //       the estimated one, not the measured one!
575         m_cycletimer_ticks += delta_ticks_est;
576         // if we need to wrap, do it
577         if (m_cycletimer_ticks > TICKS_PER_SECOND * 128L) {
578             m_cycletimer_ticks -= TICKS_PER_SECOND * 128L;
579         }
580
581         m_lastmeas_usecs = new_usecs;
582
583         debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"U TS: %10llu -> %10llu, d=%7lldus, dt_est=%7lld,  dt_meas=%7lld, erate=%6.4f, mrate=%6f\n",
584               prev_ticks, m_cycletimer_ticks, delta_usecs,
585               delta_ticks_est, delta_ticks_meas, m_ticks_per_usec, rate_meas
586               );
587
588         // the estimate is good
589         return true;
590     } else {
591         debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"U TS: Not updating, rate out of range (%6.4f)\n",
592               rate_meas
593               );
594         return false;
595
596     }
597 }
598
599 void IsoHandler::initCycleTimer() {
600     freebob_microsecs_t prev_usecs;
601     unsigned int prev_ticks;
602     unsigned int prev_timer;
603    
604     freebob_microsecs_t new_usecs;
605     unsigned int new_ticks;
606     unsigned int new_timer;
607    
608     float rate=0.0;
609    
610     unsigned int try_cnt=0;
611    
612     // make sure that we start with a decent rate,
613     // meaning that we want two successive (usecs,ticks)
614     // points that make sense.
615    
616     while ( (try_cnt++ < CC_INIT_MAX_TRIES) &&
617            (   (rate > 24.576*(1.0+CC_MAX_RATE_ERROR))
618            || (rate < 24.576*(1.0-CC_MAX_RATE_ERROR)))) {
619            
620 #ifdef LIBRAW1394_USE_CTRREAD_API
621         struct raw1394_cycle_timer ctr;
622         int err;
623         err=raw1394_read_cycle_timer(m_handle_util, &ctr);
624         if(err) {
625             debugWarning("raw1394_read_cycle_timer: %s", strerror(err));
626         }
627         prev_usecs=(freebob_microsecs_t)ctr.local_time;
628         prev_timer=ctr.cycle_timer;
629 #else
630         // normally we should be able to use the same handle
631         // because it is not iterated on by any other stuff
632         // but I'm not sure
633         quadlet_t buf=0;
634         raw1394_read(m_handle_util, raw1394_get_local_id(m_handle_util),
635             CSR_REGISTER_BASE | CSR_CYCLE_TIME, 4, &buf);
636         prev_usecs=m_TimeSource->getCurrentTimeAsUsecs();
637         prev_timer= ntohl(buf) & 0xFFFFFFFF;
638 #endif               
639         prev_ticks=CYCLE_TIMER_TO_TICKS(prev_timer);
640        
641         usleep(CC_SLEEP_TIME_AFTER_UPDATE);
642        
643        
644 #ifdef LIBRAW1394_USE_CTRREAD_API
645         err=raw1394_read_cycle_timer(m_handle_util, &ctr);
646         if(err) {
647             debugWarning("raw1394_read_cycle_timer: %s", strerror(err));
648         }
649         new_usecs=(freebob_microsecs_t)ctr.local_time;
650         new_timer=ctr.cycle_timer;
651 #else
652         // normally we should be able to use the same handle
653         // because it is not iterated on by any other stuff
654         // but I'm not sure
655         raw1394_read(m_handle_util, raw1394_get_local_id(m_handle_util),
656             CSR_REGISTER_BASE | CSR_CYCLE_TIME, 4, &buf);
657         new_usecs=m_TimeSource->getCurrentTimeAsUsecs();
658         new_timer= ntohl(buf) & 0xFFFFFFFF;
659 #endif   
660
661         new_ticks=CYCLE_TIMER_TO_TICKS(new_timer);
662        
663         unsigned int delta_ticks;
664        
665         if (new_ticks > prev_ticks) {
666             delta_ticks=new_ticks - prev_ticks;
667         } else { // wraparound
668             delta_ticks=CYCLE_TIMER_UNWRAP_TICKS(new_ticks) - prev_ticks;
669         }
670        
671         int delta_usecs=new_usecs-prev_usecs;
672        
673         // this cannot be 0, because m_TimeSource->getCurrentTimeAsUsecs should
674         // never return the same value (maybe in future terrahz processors?)
675         assert(delta_usecs);
676        
677         rate=((float)delta_ticks/(float)delta_usecs);
678        
679         // update the internal values
680         m_cycletimer_ticks=new_ticks;
681         m_lastmeas_usecs=new_usecs;
682        
683         debugOutput(DEBUG_LEVEL_VERBOSE,"Try %d: rate=%6.4f\n",
684             try_cnt,rate
685             );
686
687     }
688    
689     // this is not fatal, the DLL will eventually correct this
690     if(try_cnt == CC_INIT_MAX_TRIES) {
691         debugWarning("Failed to properly initialize cycle timer...\n");
692     }
693    
694     // initialize this to the nominal value
695     m_ticks_per_usec = 24.576;
696     m_ticks_per_usec_dll_err2 = 0;
697    
698 }
699
700 void IsoHandler::dumpInfo()
701 {
702
703     int channel=-1;
704     if (m_Client) channel=m_Client->getChannel();
705
706     debugOutputShort( DEBUG_LEVEL_NORMAL, "  Handler type    : %s\n",
707             (this->getType()==EHT_Receive ? "Receive" : "Transmit"));
708     debugOutputShort( DEBUG_LEVEL_NORMAL, "  Port, Channel   : %2d, %2d\n",
709             m_port, channel);
710     debugOutputShort( DEBUG_LEVEL_NORMAL, "  Packet count    : %10d (%5d dropped)\n",
711             this->getPacketCount(), this->getDroppedCount());
712            
713     #ifdef DEBUG
714     unsigned int cc=this->getCycleTimerTicks();
715     debugOutputShort( DEBUG_LEVEL_NORMAL, "  Cycle timer     : %10lu (%03us, %04ucycles, %04uticks)\n",
716             cc,TICKS_TO_SECS(cc),TICKS_TO_CYCLES(cc),TICKS_TO_OFFSET(cc));
717              
718 /*  freebob_microsecs_t now=m_TimeSource->getCurrentTimeAsUsecs();
719     cc=mapToCycleTimer(now);
720     freebob_microsecs_t now_mapped=mapToTimeSource(cc);
721    
722     debugOutputShort( DEBUG_LEVEL_NORMAL, "  Mapping test   : now: %14llu, cc: %10lu, mapped now: %14llu\n",
723             now,cc,now_mapped);*/
724     #endif
725     debugOutputShort( DEBUG_LEVEL_NORMAL, "  Ticks/usec      : %8.6f (dll2: %8.6e)\n\n",
726             this->getTicksPerUsec(), m_ticks_per_usec_dll_err2);
727
728 };
729
730 void IsoHandler::setVerboseLevel(int l)
731 {
732     setDebugLevel(l);
733 }
734
735 bool IsoHandler::registerStream(IsoStream *stream)
736 {
737     assert(stream);
738     debugOutput( DEBUG_LEVEL_VERBOSE, "registering stream (%p)\n", stream);
739
740     if (m_Client) {
741             debugFatal( "Generic IsoHandlers can have only one client\n");     
742             return false;
743     }
744
745     m_Client=stream;
746
747     m_Client->setHandler(this);
748
749     return true;
750
751 }
752
753 bool IsoHandler::unregisterStream(IsoStream *stream)
754 {
755     assert(stream);
756     debugOutput( DEBUG_LEVEL_VERBOSE, "unregistering stream (%p)\n", stream);
757
758     if(stream != m_Client) {
759             debugFatal( "no client registered\n");     
760             return false;
761     }
762
763     m_Client->clearHandler();
764    
765     m_Client=0;
766     return true;
767
768 }
769
770 /* The timesource interface */
771 freebob_microsecs_t IsoHandler::getCurrentTime() {
772     unsigned int new_timer;
773    
774     new_timer= getCycleTimerTicks();
775        
776     // this assumes that it never happens that there are more than 2
777     // minutes between calls
778     if (CYCLE_TIMER_GET_SECS(new_timer) < m_TimeSource_LastSecs) {
779         m_TimeSource_NbCycleWraps++;
780     }
781    
782     freebob_microsecs_t ticks=m_TimeSource_NbCycleWraps * 128L * TICKS_PER_SECOND
783             + CYCLE_TIMER_TO_TICKS(new_timer);
784    
785     m_TimeSource_LastSecs=CYCLE_TIMER_GET_SECS(new_timer);
786    
787     debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"Wraps=%4u, LastSecs=%3u, nowSecs=%3u, ticks=%10u\n",
788               m_TimeSource_NbCycleWraps, m_TimeSource_LastSecs,
789               CYCLE_TIMER_GET_SECS(new_timer), ticks
790               );
791              
792     return  ticks;
793 }
794
795 freebob_microsecs_t IsoHandler::unWrapTime(freebob_microsecs_t t) {
796     return CYCLE_TIMER_UNWRAP_TICKS(t);
797 }
798
799 freebob_microsecs_t IsoHandler::wrapTime(freebob_microsecs_t t) {
800     return CYCLE_TIMER_WRAP_TICKS(t);
801 }
802
803 freebob_microsecs_t IsoHandler::getCurrentTimeAsUsecs() {
804     float tmp=getCurrentTime();
805     float tmp2 = tmp * USECS_PER_TICK;
806     freebob_microsecs_t retval=(freebob_microsecs_t)tmp2;
807    
808     debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"tmp=%f, tmp2=%f, retval=%u\n",
809               tmp, tmp2,retval
810               );
811    
812     return retval;
813 }
814
815
816
817 /* Child class implementations */
818
819 IsoRecvHandler::IsoRecvHandler(int port)
820                 : IsoHandler(port)
821 {
822     debugOutput( DEBUG_LEVEL_VERBOSE, "enter...\n");
823 }
824 IsoRecvHandler::IsoRecvHandler(int port, unsigned int buf_packets,
825                                unsigned int max_packet_size, int irq)
826                 : IsoHandler(port, buf_packets,max_packet_size,irq)
827 {
828     debugOutput( DEBUG_LEVEL_VERBOSE, "enter...\n");
829
830 }
831 IsoRecvHandler::~IsoRecvHandler()
832 {
833
834 }
835
836 bool
837 IsoRecvHandler::init() {
838     debugOutput( DEBUG_LEVEL_VERBOSE, "init recv handler %p\n",this);
839
840     if(!(IsoHandler::init())) {
841         return false;
842     }
843     return true;
844
845 }
846
847 enum raw1394_iso_disposition IsoRecvHandler::putPacket(
848                     unsigned char *data, unsigned int length,
849                     unsigned char channel, unsigned char tag, unsigned char sy,
850                     unsigned int cycle, unsigned int dropped) {
851
852     debugOutput( DEBUG_LEVEL_VERY_VERBOSE,
853                  "received packet: length=%d, channel=%d, cycle=%d\n",
854                  length, channel, cycle );
855     m_packetcount++;
856     m_dropped+=dropped;
857
858     if(m_Client) {
859         return m_Client->putPacket(data, length, channel, tag, sy, cycle, dropped);
860     }
861    
862     return RAW1394_ISO_OK;
863 }
864
865 bool IsoRecvHandler::prepare()
866 {
867    
868     // prepare the generic IsoHandler
869     if(!IsoHandler::prepare()) {
870         return false;
871     }
872
873     debugOutput( DEBUG_LEVEL_VERBOSE, "Preparing iso receive handler (%p)\n",this);
874     debugOutput( DEBUG_LEVEL_VERBOSE, " Buffers         : %d \n",m_buf_packets);
875     debugOutput( DEBUG_LEVEL_VERBOSE, " Max Packet size : %d \n",m_max_packet_size);
876     debugOutput( DEBUG_LEVEL_VERBOSE, " Channel         : %d \n",m_Client->getChannel());
877     debugOutput( DEBUG_LEVEL_VERBOSE, " Irq interval    : %d \n",m_irq_interval);
878
879     if(m_irq_interval > 1) {
880         if(raw1394_iso_recv_init(m_handle,   
881                                 iso_receive_handler,
882                                 m_buf_packets,
883                                 m_max_packet_size,
884                                 m_Client->getChannel(),
885                                 RAW1394_DMA_BUFFERFILL,
886                                 m_irq_interval)) {
887             debugFatal("Could not do receive initialisation!\n" );
888             debugFatal("  %s\n",strerror(errno));
889    
890             return false;
891         }
892     } else {
893         if(raw1394_iso_recv_init(m_handle,   
894                                 iso_receive_handler,
895                                 m_buf_packets,
896                                 m_max_packet_size,
897                                 m_Client->getChannel(),
898                                 RAW1394_DMA_PACKET_PER_BUFFER,
899                                 m_irq_interval)) {
900             debugFatal("Could not do receive initialisation!\n" );
901             debugFatal("  %s\n",strerror(errno));
902    
903             return false;
904         }   
905     }
906     return true;
907 }
908
909 bool IsoRecvHandler::start(int cycle)
910 {
911     debugOutput( DEBUG_LEVEL_VERBOSE, "start on cycle %d\n", cycle);
912    
913     // start the generic IsoHandler
914     if(!IsoHandler::start(cycle)) {
915         return false;
916     }
917    
918     if(raw1394_iso_recv_start(m_handle, cycle, -1, 0)) {
919         debugFatal("Could not start receive handler (%s)\n",strerror(errno));
920         return false;
921     }
922     return true;
923 }
924
925 int IsoRecvHandler::handleBusReset(unsigned int generation) {
926     debugOutput( DEBUG_LEVEL_VERBOSE, "handle bus reset...\n");
927    
928     //TODO: implement busreset
929    
930     // pass on the busreset signal
931     if(IsoHandler::handleBusReset(generation)) {
932         return -1;
933     }
934     return 0;
935 }
936
937 /* ----------------- XMIT --------------- */
938
939 IsoXmitHandler::IsoXmitHandler(int port)
940                 : IsoHandler(port), m_prebuffers(0)
941 {
942     debugOutput( DEBUG_LEVEL_VERBOSE, "IsoXmitHandler enter...\n");
943
944 }
945 IsoXmitHandler::IsoXmitHandler(int port, unsigned int buf_packets,
946                                unsigned int max_packet_size, int irq)
947                 : IsoHandler(port, buf_packets, max_packet_size,irq),
948                   m_speed(RAW1394_ISO_SPEED_400), m_prebuffers(0)
949 {
950     debugOutput( DEBUG_LEVEL_VERBOSE, "IsoXmitHandler enter...\n");
951
952 }
953 IsoXmitHandler::IsoXmitHandler(int port, unsigned int buf_packets,
954                                unsigned int max_packet_size, int irq,
955                                enum raw1394_iso_speed speed)
956                 : IsoHandler(port, buf_packets,max_packet_size,irq),
957                   m_speed(speed), m_prebuffers(0)
958 {
959     debugOutput( DEBUG_LEVEL_VERBOSE, "IsoXmitHandler enter...\n");
960
961 }
962
963 IsoXmitHandler::~IsoXmitHandler()
964 {
965     // handle cleanup is done in the IsoHanlder destructor
966 }
967
968 bool
969 IsoXmitHandler::init() {
970
971     debugOutput( DEBUG_LEVEL_VERBOSE, "init xmit handler %p\n",this);
972
973     if(!(IsoHandler::init())) {
974         return false;
975     }
976
977     return true;
978 }
979
980 bool IsoXmitHandler::prepare()
981 {
982     debugOutput( DEBUG_LEVEL_VERBOSE, "Preparing iso transmit handler (%p, client=%p)\n",this,m_Client);
983
984     if(!(IsoHandler::prepare())) {
985         return false;
986     }
987
988     debugOutput( DEBUG_LEVEL_VERBOSE, " Buffers         : %d \n",m_buf_packets);
989     debugOutput( DEBUG_LEVEL_VERBOSE, " Max Packet size : %d \n",m_max_packet_size);
990     debugOutput( DEBUG_LEVEL_VERBOSE, " Channel         : %d \n",m_Client->getChannel());
991     debugOutput( DEBUG_LEVEL_VERBOSE, " Speed           : %d \n",m_speed);
992     debugOutput( DEBUG_LEVEL_VERBOSE, " Irq interval    : %d \n",m_irq_interval);
993
994     if(raw1394_iso_xmit_init(m_handle,
995                              iso_transmit_handler,
996                              m_buf_packets,
997                              m_max_packet_size,
998                              m_Client->getChannel(),
999                              m_speed,
1000                              m_irq_interval)) {
1001         debugFatal("Could not do xmit initialisation!\n" );
1002
1003         return false;
1004     }
1005
1006     return true;
1007 }
1008
1009 bool IsoXmitHandler::start(int cycle)
1010 {
1011     debugOutput( DEBUG_LEVEL_VERBOSE, "start on cycle %d\n", cycle);
1012    
1013     if(!(IsoHandler::start(cycle))) {
1014         return false;
1015     }
1016    
1017     if(raw1394_iso_xmit_start(m_handle, cycle, m_prebuffers)) {
1018         debugFatal("Could not start xmit handler (%s)\n",strerror(errno));
1019         return false;
1020     }
1021     return true;
1022 }
1023
1024 enum raw1394_iso_disposition IsoXmitHandler::getPacket(
1025                     unsigned char *data, unsigned int *length,
1026                     unsigned char *tag, unsigned char *sy,
1027                     int cycle, unsigned int dropped) {
1028
1029     debugOutput( DEBUG_LEVEL_VERY_VERBOSE,
1030                     "sending packet: length=%d, cycle=%d\n",
1031                     *length, cycle );
1032     m_packetcount++;
1033     m_dropped+=dropped;
1034
1035     if(m_Client) {
1036         return m_Client->getPacket(data, length, tag, sy, cycle, dropped, m_max_packet_size);
1037     }
1038
1039     return RAW1394_ISO_OK;
1040 }
1041
1042 int IsoXmitHandler::handleBusReset(unsigned int generation) {
1043     debugOutput( DEBUG_LEVEL_VERBOSE, "bus reset...\n");
1044     //TODO: implement busreset
1045    
1046     // pass on the busreset signal
1047     if(IsoHandler::handleBusReset(generation)) {
1048             return -1;
1049     }
1050    
1051     return 0;
1052 }
1053
1054 }
1055
1056 /* multichannel receive  */
1057 #if 0
1058 IsoRecvHandler::IsoRecvHandler(int port)
1059                 : IsoHandler(port)
1060 {
1061         debugOutput( DEBUG_LEVEL_VERBOSE, "enter...\n");
1062 }
1063 IsoRecvHandler::IsoRecvHandler(int port, unsigned int buf_packets,
1064                                unsigned int max_packet_size, int irq)
1065                 : IsoHandler(port, buf_packets,max_packet_size,irq)
1066 {
1067         debugOutput( DEBUG_LEVEL_VERBOSE, "enter...\n");
1068
1069 }
1070 IsoRecvHandler::~IsoRecvHandler()
1071 {
1072 // Don't call until libraw1394's raw1394_new_handle() function has been
1073 // fixed to correctly initialise the iso_packet_infos field.  Bug is
1074 // confirmed present in libraw1394 1.2.1.  In any case,
1075 // raw1394_destroy_handle() (in the base class destructor) will do any iso
1076 // system shutdown required.
1077         raw1394_iso_shutdown(m_handle);
1078
1079 }
1080
1081 bool
1082 IsoRecvHandler::initialize() {
1083         debugOutput( DEBUG_LEVEL_VERBOSE, "enter...\n");
1084
1085         IsoHandler *base=static_cast<IsoHandler *>(this);
1086
1087         if(!(base->initialize())) {
1088                 return false;
1089         }
1090
1091         raw1394_set_userdata(m_handle, static_cast<void *>(this));
1092
1093         if(raw1394_iso_multichannel_recv_init(m_handle,
1094                                          iso_receive_handler,
1095                                          m_buf_packets,
1096                                          m_max_packet_size,
1097                                          m_irq_interval)) {
1098                 debugFatal("Could not do multichannel receive initialisation!\n" );
1099
1100                 return false;
1101         }
1102
1103         return true;
1104
1105 }
1106
1107 enum raw1394_iso_disposition IsoRecvHandler::putPacket(unsigned char *data, unsigned int length,
1108                               unsigned char channel, unsigned char tag, unsigned char sy,
1109                                   unsigned int cycle, unsigned int dropped) {
1110
1111         debugOutput( DEBUG_LEVEL_VERY_VERBOSE,
1112                      "received packet: length=%d, channel=%d, cycle=%d\n",
1113                      length, channel, cycle );
1114        
1115         return RAW1394_ISO_OK;
1116 }
1117
1118 // an recv handler can have multiple destination IsoStreams
1119 // NOTE: this implementation even allows for already registered
1120 // streams to be registered again.
1121 int IsoRecvHandler::registerStream(IsoRecvStream *stream)
1122 {
1123         assert(stream);
1124         debugOutput( DEBUG_LEVEL_VERBOSE, "enter...\n");
1125
1126         m_Clients.push_back(stream);
1127
1128         listen(stream->getChannel());
1129         return 0;
1130
1131 }
1132
1133 int IsoRecvHandler::unregisterStream(IsoRecvStream *stream)
1134 {
1135         assert(stream);
1136         debugOutput( DEBUG_LEVEL_VERBOSE, "enter...\n");
1137
1138     for ( IsoRecvStreamVectorIterator it = m_Clients.begin();
1139           it != m_Clients.end();
1140           ++it )
1141     {
1142         IsoRecvStream* s = *it;
1143         if ( s == stream ) {
1144                         unListen(s->getChannel());
1145             m_Clients.erase(it);
1146                         return 0;
1147         }
1148     }
1149
1150         return -1; //not found
1151
1152 }
1153
1154 void IsoRecvHandler::listen(int channel) {
1155         int retval;
1156         debugOutput( DEBUG_LEVEL_VERBOSE, "enter...\n");
1157
1158         retval=raw1394_iso_recv_listen_channel(m_handle, channel);
1159
1160 }
1161
1162 void IsoRecvHandler::unListen(int channel) {
1163         int retval;
1164         debugOutput( DEBUG_LEVEL_VERBOSE, "enter...\n");
1165
1166         retval=raw1394_iso_recv_unlisten_channel(m_handle, channel);
1167
1168 }
1169
1170 int IsoRecvHandler::start(int cycle)
1171 {
1172         debugOutput( DEBUG_LEVEL_VERBOSE, "enter...\n");
1173         return raw1394_iso_recv_start(m_handle, cycle, -1, 0);
1174 }
1175 #endif
Note: See TracBrowser for help on using the browser.