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

Revision 394, 35.8 kB (checked in by pieterpalmers, 16 years ago)

- fixed SYT timestamp to ticks conversion

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