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

Revision 390, 35.3 kB (checked in by pieterpalmers, 16 years ago)

* working version of SYT based AMDTP receive and transmit.

Still has to be tuned to work with low buffer sizes.

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