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

Revision 1348, 37.5 kB (checked in by ppalmers, 16 years ago)

merge 2.0 branch changes to trunk. svn merge -r 1337:HEAD svn+ssh://ffadosvn@ffado.org/ffado/branches/libffado-2.0

  • Property svn:eol-style set to native
  • Property svn:keywords set to Author Date Id Revision
Line 
1 /*
2  * Copyright (C) 2005-2008 by Daniel Wagner
3  * Copyright (C) 2005-2008 by Pieter Palmers
4  *
5  * This file is part of FFADO
6  * FFADO = Free Firewire (pro-)audio drivers for linux
7  *
8  * FFADO is based upon FreeBoB
9  *
10  * This program is free software: you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation, either version 2 of the License, or
13  * (at your option) version 3 of the License.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
18  * GNU General Public License for more details.
19  *
20  * You should have received a copy of the GNU General Public License
21  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
22  *
23  */
24
25 #include "config.h"
26
27 #include "ieee1394service.h"
28 #include "ARMHandler.h"
29 #include "cycletimer.h"
30 #include "IsoHandlerManager.h"
31 #include "CycleTimerHelper.h"
32
33 #include <libavc1394/avc1394.h>
34 #include <libraw1394/csr.h>
35 #include <libiec61883/iec61883.h>
36
37 #include "libutil/SystemTimeSource.h"
38 #include "libutil/Watchdog.h"
39 #include "libutil/PosixMutex.h"
40
41 #include <errno.h>
42 #include "libutil/ByteSwap.h"
43
44 #include <string.h>
45
46 #include <iostream>
47 #include <iomanip>
48
49 using namespace std;
50
51 IMPL_DEBUG_MODULE( Ieee1394Service, Ieee1394Service, DEBUG_LEVEL_NORMAL );
52
53 Ieee1394Service::Ieee1394Service()
54     : m_handle( 0 )
55     , m_handle_lock( new Util::PosixMutex("SRCVHND") )
56     , m_resetHandle( 0 )
57     , m_util_handle( 0 )
58     , m_port( -1 )
59     , m_RHThread_lock( new Util::PosixMutex("SRVCRH") )
60     , m_threadRunning( false )
61     , m_realtime ( false )
62     , m_base_priority ( 0 )
63     , m_pIsoManager( new IsoHandlerManager( *this ) )
64     , m_pCTRHelper ( new CycleTimerHelper( *this, IEEE1394SERVICE_CYCLETIMER_DLL_UPDATE_INTERVAL_USEC ) )
65     , m_have_new_ctr_read ( false )
66     , m_pWatchdog ( new Util::Watchdog() )
67 {
68     for (unsigned int i=0; i<64; i++) {
69         m_channels[i].channel=-1;
70         m_channels[i].bandwidth=-1;
71         m_channels[i].alloctype=AllocFree;
72         m_channels[i].xmit_node=0xFFFF;
73         m_channels[i].xmit_plug=-1;
74         m_channels[i].recv_node=0xFFFF;
75         m_channels[i].recv_plug=-1;
76     }
77 }
78
79 Ieee1394Service::Ieee1394Service(bool rt, int prio)
80     : m_handle( 0 )
81     , m_handle_lock( new Util::PosixMutex("SRCVHND") )
82     , m_resetHandle( 0 )
83     , m_util_handle( 0 )
84     , m_port( -1 )
85     , m_RHThread_lock( new Util::PosixMutex("SRVCRH") )
86     , m_threadRunning( false )
87     , m_realtime ( rt )
88     , m_base_priority ( prio )
89     , m_pIsoManager( new IsoHandlerManager( *this, rt, prio ) )
90     , m_pCTRHelper ( new CycleTimerHelper( *this, IEEE1394SERVICE_CYCLETIMER_DLL_UPDATE_INTERVAL_USEC,
91                                            rt && IEEE1394SERVICE_CYCLETIMER_HELPER_RUN_REALTIME,
92                                            prio + IEEE1394SERVICE_CYCLETIMER_HELPER_PRIO_INCREASE ) )
93     , m_have_new_ctr_read ( false )
94     , m_pWatchdog ( new Util::Watchdog() )
95 {
96     for (unsigned int i=0; i<64; i++) {
97         m_channels[i].channel=-1;
98         m_channels[i].bandwidth=-1;
99         m_channels[i].alloctype=AllocFree;
100         m_channels[i].xmit_node=0xFFFF;
101         m_channels[i].xmit_plug=-1;
102         m_channels[i].recv_node=0xFFFF;
103         m_channels[i].recv_plug=-1;
104     }
105 }
106
107 Ieee1394Service::~Ieee1394Service()
108 {
109     delete m_pIsoManager;
110     delete m_pCTRHelper;
111     stopRHThread();
112     for ( arm_handler_vec_t::iterator it = m_armHandlers.begin();
113           it != m_armHandlers.end();
114           ++it )
115     {
116         debugOutput(DEBUG_LEVEL_VERBOSE, "Unregistering ARM handler for 0x%016llX\n", (*it)->getStart());
117         int err=raw1394_arm_unregister(m_resetHandle, (*it)->getStart());
118         if (err) {
119             debugError(" Failed to unregister ARM handler for 0x%016llX\n", (*it)->getStart());
120             debugError(" Error: %s\n", strerror(errno));
121         }
122     }
123
124     delete m_pWatchdog;
125     if ( m_handle ) {
126         raw1394_destroy_handle( m_handle );
127     }
128     delete m_handle_lock;
129
130     if ( m_resetHandle ) {
131         raw1394_destroy_handle( m_resetHandle );
132     }
133     delete m_RHThread_lock;
134     if ( m_util_handle ) {
135         raw1394_destroy_handle( m_util_handle );
136     }
137 }
138
139 int
140 Ieee1394Service::detectNbPorts()
141 {
142     raw1394handle_t tmp_handle = raw1394_new_handle();
143     if ( tmp_handle == NULL ) {
144         debugError("Could not get libraw1394 handle.\n");
145         return -1;
146     }
147     struct raw1394_portinfo pinf[IEEE1394SERVICE_MAX_FIREWIRE_PORTS];
148     int nb_detected_ports = raw1394_get_port_info(tmp_handle, pinf, IEEE1394SERVICE_MAX_FIREWIRE_PORTS);
149     raw1394_destroy_handle(tmp_handle);
150
151     if (nb_detected_ports < 0) {
152         debugError("Failed to detect number of ports\n");
153         return -1;
154     }
155     return nb_detected_ports;
156 }
157
158 void
159 Ieee1394Service::doBusReset() {
160     debugOutput(DEBUG_LEVEL_VERBOSE, "Issue bus reset on service %p (port %d).\n", this, getPort());
161     raw1394_reset_bus(m_handle);
162 }
163
164 /**
165  * This function waits until there are no bus resets generated in a sleep_time_ms interval
166  * @param nb_tries number of tries to take
167  * @param sleep_time_ms sleep between tries
168  * @return true if the storm passed
169  */
170 bool
171 Ieee1394Service::waitForBusResetStormToEnd( int nb_tries, int sleep_time_ms ) {
172     unsigned int gen_current;
173     do {
174         gen_current = getGeneration();
175         debugOutput(DEBUG_LEVEL_VERBOSE, "Waiting... (gen: %u)\n", gen_current);
176
177         // wait for a while
178         Util::SystemTimeSource::SleepUsecRelative( sleep_time_ms * 1000);
179     } while (gen_current != getGeneration() && --nb_tries);
180
181     if (!nb_tries) {
182         debugError( "Bus reset storm did not stop on time...\n");
183         return false;
184     }
185     return true;
186 }
187
188 bool
189 Ieee1394Service::initialize( int port )
190 {
191     using namespace std;
192
193     int nb_ports = detectNbPorts();
194     if (port + 1 > nb_ports) {
195         debugFatal("Requested port (%d) out of range (# ports: %d)\n", port, nb_ports);
196     }
197
198     if(!m_pWatchdog) {
199         debugError("No valid RT watchdog found.\n");
200         return false;
201     }
202     if(!m_pWatchdog->start()) {
203         debugError("Could not start RT watchdog.\n");
204         return false;
205     }
206
207     m_handle = raw1394_new_handle_on_port( port );
208     if ( !m_handle ) {
209         if ( !errno ) {
210             debugFatal("libraw1394 not compatible\n");
211         } else {
212             debugFatal("Ieee1394Service::initialize: Could not get 1394 handle: %s\n",
213                 strerror(errno) );
214             debugFatal("Is ieee1394 and raw1394 driver loaded?\n");
215         }
216         return false;
217     }
218
219     m_resetHandle = raw1394_new_handle_on_port( port );
220     if ( !m_resetHandle ) {
221         if ( !errno ) {
222             debugFatal("libraw1394 not compatible\n");
223         } else {
224             debugFatal("Ieee1394Service::initialize: Could not get 1394 handle: %s",
225                 strerror(errno) );
226             debugFatal("Is ieee1394 and raw1394 driver loaded?\n");
227         }
228         return false;
229     }
230
231     m_util_handle = raw1394_new_handle_on_port( port );
232     if ( !m_util_handle ) {
233         if ( !errno ) {
234             debugFatal("libraw1394 not compatible\n");
235         } else {
236             debugFatal("Ieee1394Service::initialize: Could not get 1394 handle: %s",
237                 strerror(errno) );
238             debugFatal("Is ieee1394 and raw1394 driver loaded?\n");
239         }
240         return false;
241     }
242
243     // test the cycle timer read function
244     int err;
245     uint32_t cycle_timer;
246     uint64_t local_time;
247     err=raw1394_read_cycle_timer(m_handle, &cycle_timer, &local_time);
248     if(err) {
249         debugOutput(DEBUG_LEVEL_VERBOSE, "raw1394_read_cycle_timer failed.\n");
250         debugOutput(DEBUG_LEVEL_VERBOSE, " Error descr: %s\n", strerror(err));
251         debugWarning("==================================================================\n");
252         debugWarning(" This system doesn't support the raw1394_read_cycle_timer call.   \n");
253         debugWarning(" Fallback to indirect CTR read method.                            \n");
254         debugWarning(" FFADO should work, but achieving low-latency might be a problem. \n");
255         debugWarning(" Upgrade the kernel to version 2.6.21 or higher to solve this.    \n");
256         debugWarning("==================================================================\n");
257         m_have_new_ctr_read = false;
258     } else {
259         debugOutput(DEBUG_LEVEL_VERBOSE, "This system supports the raw1394_read_cycle_timer call, using it.\n");
260         m_have_new_ctr_read = true;
261     }
262
263     m_port = port;
264
265     // obtain port name
266     raw1394handle_t tmp_handle = raw1394_new_handle();
267     if ( tmp_handle == NULL ) {
268         debugError("Could not get temporaty libraw1394 handle.\n");
269         return false;
270     }
271     struct raw1394_portinfo pinf[IEEE1394SERVICE_MAX_FIREWIRE_PORTS];
272     int nb_detected_ports = raw1394_get_port_info(tmp_handle, pinf, IEEE1394SERVICE_MAX_FIREWIRE_PORTS);
273     raw1394_destroy_handle(tmp_handle);
274
275     if (nb_detected_ports < 0) {
276         debugError("Failed to detect number of ports\n");
277         return false;
278     }
279
280     if(nb_detected_ports && port < IEEE1394SERVICE_MAX_FIREWIRE_PORTS) {
281         m_portName = pinf[port].name;
282     } else {
283         m_portName = "Unknown";
284     }
285     if (m_portName == "") {
286         m_portName = "Unknown";
287     }
288
289     raw1394_set_userdata( m_handle, this );
290     raw1394_set_userdata( m_resetHandle, this );
291     raw1394_set_userdata( m_util_handle, this );
292     raw1394_set_bus_reset_handler( m_resetHandle,
293                                    this->resetHandlerLowLevel );
294
295     m_default_arm_handler = raw1394_set_arm_tag_handler( m_resetHandle,
296                                    this->armHandlerLowLevel );
297
298     if(!m_pCTRHelper) {
299         debugFatal("No CycleTimerHelper available, bad!\n");
300         return false;
301     }
302     m_pCTRHelper->setVerboseLevel(getDebugLevel());
303     if(!m_pCTRHelper->Start()) {
304         debugFatal("Could not start CycleTimerHelper\n");
305         return false;
306     }
307
308     if(!m_pIsoManager) {
309         debugFatal("No IsoHandlerManager available, bad!\n");
310         return false;
311     }
312     m_pIsoManager->setVerboseLevel(getDebugLevel());
313     if(!m_pIsoManager->init()) {
314         debugFatal("Could not initialize IsoHandlerManager\n");
315         return false;
316     }
317
318     startRHThread();
319
320     // make sure that the thread parameters of all our helper threads are OK
321     if(!setThreadParameters(m_realtime, m_base_priority)) {
322         debugFatal("Could not set thread parameters\n");
323         return false;
324     }
325     return true;
326 }
327
328 bool
329 Ieee1394Service::setThreadParameters(bool rt, int priority) {
330     bool result = true;
331     if (priority > THREAD_MAX_RTPRIO) priority = THREAD_MAX_RTPRIO;
332     m_base_priority = priority;
333     m_realtime = rt;
334     if (m_pIsoManager) {
335         debugOutput(DEBUG_LEVEL_VERBOSE, "Switching IsoManager to (rt=%d, prio=%d)\n",
336                                          rt, priority);
337         result &= m_pIsoManager->setThreadParameters(rt, priority);
338     }
339     if (m_pCTRHelper) {
340         debugOutput(DEBUG_LEVEL_VERBOSE, "Switching CycleTimerHelper to (rt=%d, prio=%d)\n",
341                                          rt && IEEE1394SERVICE_CYCLETIMER_HELPER_RUN_REALTIME,
342                                          priority + IEEE1394SERVICE_CYCLETIMER_HELPER_PRIO_INCREASE);
343         result &= m_pCTRHelper->setThreadParameters(rt && IEEE1394SERVICE_CYCLETIMER_HELPER_RUN_REALTIME,
344                                                     priority + IEEE1394SERVICE_CYCLETIMER_HELPER_PRIO_INCREASE);
345     }
346     return result;
347 }
348
349 int
350 Ieee1394Service::getNodeCount()
351 {
352     Util::MutexLockHelper lock(*m_handle_lock);
353     return raw1394_get_nodecount( m_handle );
354 }
355
356 nodeid_t Ieee1394Service::getLocalNodeId() {
357     Util::MutexLockHelper lock(*m_handle_lock);
358     return raw1394_get_local_id(m_handle) & 0x3F;
359 }
360
361 /**
362  * Returns the current value of the cycle timer (in ticks)
363  *
364  * @return the current value of the cycle timer (in ticks)
365  */
366
367 uint32_t
368 Ieee1394Service::getCycleTimerTicks() {
369     return m_pCTRHelper->getCycleTimerTicks();
370 }
371
372 /**
373  * Returns the current value of the cycle timer (as is)
374  *
375  * @return the current value of the cycle timer (as is)
376  */
377 uint32_t
378 Ieee1394Service::getCycleTimer() {
379     return m_pCTRHelper->getCycleTimer();
380 }
381
382 /**
383  * Returns the current value of the cycle timer (in ticks)
384  * for a specific time instant (usecs since epoch)
385  * @return the current value of the cycle timer (in ticks)
386  */
387
388 uint32_t
389 Ieee1394Service::getCycleTimerTicks(uint64_t t) {
390     return m_pCTRHelper->getCycleTimerTicks(t);
391 }
392
393 /**
394  * Returns the current value of the cycle timer (as is)
395  * for a specific time instant (usecs since epoch)
396  * @return the current value of the cycle timer (as is)
397  */
398 uint32_t
399 Ieee1394Service::getCycleTimer(uint64_t t) {
400     return m_pCTRHelper->getCycleTimer(t);
401 }
402
403 uint64_t
404 Ieee1394Service::getSystemTimeForCycleTimerTicks(uint32_t ticks) {
405     return m_pCTRHelper->getSystemTimeForCycleTimerTicks(ticks);
406 }
407
408 uint64_t
409 Ieee1394Service::getSystemTimeForCycleTimer(uint32_t ctr) {
410     return m_pCTRHelper->getSystemTimeForCycleTimer(ctr);
411 }
412
413 bool
414 Ieee1394Service::readCycleTimerReg(uint32_t *cycle_timer, uint64_t *local_time)
415 {
416     if(m_have_new_ctr_read) {
417         int err;
418         err = raw1394_read_cycle_timer(m_util_handle, cycle_timer, local_time);
419         if(err) {
420             debugWarning("raw1394_read_cycle_timer: %s\n", strerror(err));
421             return false;
422         }
423         return true;
424     } else {
425         // do a normal read of the CTR register
426         // the disadvantage is that local_time and cycle time are not
427         // read at the same time instant (scheduling issues)
428         *local_time = getCurrentTimeAsUsecs();
429         if ( raw1394_read( m_util_handle,
430                 getLocalNodeId() | 0xFFC0,
431                 CSR_REGISTER_BASE | CSR_CYCLE_TIME,
432                 sizeof(uint32_t), cycle_timer ) == 0 ) {
433             *cycle_timer = CondSwapFromBus32(*cycle_timer);
434             return true;
435         } else {
436             return false;
437         }
438     }
439 }
440
441 uint64_t
442 Ieee1394Service::getCurrentTimeAsUsecs() {
443     return Util::SystemTimeSource::getCurrentTimeAsUsecs();
444 }
445
446 bool
447 Ieee1394Service::read( fb_nodeid_t nodeId,
448                        fb_nodeaddr_t addr,
449                        size_t length,
450                        fb_quadlet_t* buffer )
451 {
452     Util::MutexLockHelper lock(*m_handle_lock);
453     if (nodeId == INVALID_NODE_ID) {
454         debugWarning("operation on invalid node\n");
455         return false;
456     }
457     if ( raw1394_read( m_handle, nodeId, addr, length*4, buffer ) == 0 ) {
458
459         #ifdef DEBUG
460         debugOutput(DEBUG_LEVEL_VERY_VERBOSE,
461             "read: node 0x%hX, addr = 0x%016llX, length = %u\n",
462             nodeId, addr, length);
463         printBuffer( DEBUG_LEVEL_VERY_VERBOSE, length, buffer );
464         #endif
465
466         return true;
467     } else {
468         #ifdef DEBUG
469         debugOutput(DEBUG_LEVEL_NORMAL,
470                     "raw1394_read failed: node 0x%hX, addr = 0x%016llX, length = %u\n",
471                     nodeId, addr, length);
472         #endif
473         return false;
474     }
475 }
476
477 bool
478 Ieee1394Service::read_quadlet( fb_nodeid_t nodeId,
479                                fb_nodeaddr_t addr,
480                                fb_quadlet_t* buffer )
481 {
482     return read( nodeId,  addr, sizeof( *buffer )/4, buffer );
483 }
484
485 bool
486 Ieee1394Service::read_octlet( fb_nodeid_t nodeId,
487                               fb_nodeaddr_t addr,
488                               fb_octlet_t* buffer )
489 {
490     return read( nodeId, addr, sizeof( *buffer )/4,
491                  reinterpret_cast<fb_quadlet_t*>( buffer ) );
492 }
493
494 bool
495 Ieee1394Service::write( fb_nodeid_t nodeId,
496                         fb_nodeaddr_t addr,
497                         size_t length,
498                         fb_quadlet_t* data )
499 {
500     Util::MutexLockHelper lock(*m_handle_lock);
501     if (nodeId == INVALID_NODE_ID) {
502         debugWarning("operation on invalid node\n");
503         return false;
504     }
505
506     #ifdef DEBUG
507     debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"write: node 0x%hX, addr = 0x%016llX, length = %d\n",
508                 nodeId, addr, length);
509     printBuffer( DEBUG_LEVEL_VERY_VERBOSE, length, data );
510     #endif
511
512     return raw1394_write( m_handle, nodeId, addr, length*4, data ) == 0;
513 }
514
515 bool
516 Ieee1394Service::write_quadlet( fb_nodeid_t nodeId,
517                                 fb_nodeaddr_t addr,
518                                 fb_quadlet_t data )
519 {
520     return write( nodeId, addr, sizeof( data )/4, &data );
521 }
522
523 bool
524 Ieee1394Service::write_octlet( fb_nodeid_t nodeId,
525                                fb_nodeaddr_t addr,
526                                fb_octlet_t data )
527 {
528     return write( nodeId, addr, sizeof( data )/4,
529                   reinterpret_cast<fb_quadlet_t*>( &data ) );
530 }
531
532 bool
533 Ieee1394Service::lockCompareSwap64( fb_nodeid_t nodeId,
534                                     fb_nodeaddr_t addr,
535                                     fb_octlet_t compare_value,
536                                     fb_octlet_t swap_value,
537                                     fb_octlet_t* result )
538 {
539     if (nodeId == INVALID_NODE_ID) {
540         debugWarning("operation on invalid node\n");
541         return false;
542     }
543     #ifdef DEBUG
544     debugOutput(DEBUG_LEVEL_VERBOSE,"lockCompareSwap64: node 0x%X, addr = 0x%016llX\n",
545                 nodeId, addr);
546     debugOutput(DEBUG_LEVEL_VERBOSE,"  if (*(addr)==0x%016llX) *(addr)=0x%016llX\n",
547                 compare_value, swap_value);
548     fb_octlet_t buffer;
549     if(!read_octlet( nodeId, addr,&buffer )) {
550         debugWarning("Could not read register\n");
551     } else {
552         debugOutput(DEBUG_LEVEL_VERBOSE,"before = 0x%016llX\n", buffer);
553     }
554     #endif
555
556     // do endiannes swapping
557     compare_value = CondSwapToBus64(compare_value);
558     swap_value    = CondSwapToBus64(swap_value);
559
560     // do separate locking here (no MutexLockHelper) since
561     // we use read_octlet in the DEBUG code in this function
562     m_handle_lock->Lock();
563     int retval=raw1394_lock64(m_handle, nodeId, addr,
564                               RAW1394_EXTCODE_COMPARE_SWAP,
565                               swap_value, compare_value, result);
566     m_handle_lock->Unlock();
567
568     if(retval) {
569         debugError("raw1394_lock64 failed: %s\n", strerror(errno));
570     }
571
572     #ifdef DEBUG
573     if(!read_octlet( nodeId, addr,&buffer )) {
574         debugWarning("Could not read register\n");
575     } else {
576         debugOutput(DEBUG_LEVEL_VERBOSE,"after = 0x%016llX\n", buffer);
577     }
578     #endif
579
580     *result = CondSwapFromBus64(*result);
581
582     return (retval == 0);
583 }
584
585 fb_quadlet_t*
586 Ieee1394Service::transactionBlock( fb_nodeid_t nodeId,
587                                    fb_quadlet_t* buf,
588                                    int len,
589                                    unsigned int* resp_len )
590 {
591     if (nodeId == INVALID_NODE_ID) {
592         debugWarning("operation on invalid node\n");
593         return false;
594     }
595     // FIXME: this requires transactionBlockClose to unlock
596     m_handle_lock->Lock();
597     for (int i = 0; i < len; ++i) {
598         buf[i] = CondSwapFromBus32( buf[i] );
599     }
600
601     fb_quadlet_t* result =
602         avc1394_transaction_block2( m_handle,
603                                     nodeId,
604                                     buf,
605                                     len,
606                                     resp_len,
607                                     10 );
608
609     for ( unsigned int i = 0; i < *resp_len; ++i ) {
610         result[i] = CondSwapToBus32( result[i] );
611     }
612
613     return result;
614 }
615
616
617 bool
618 Ieee1394Service::transactionBlockClose()
619 {
620     avc1394_transaction_block_close( m_handle );
621     m_handle_lock->Unlock();
622     return true;
623 }
624
625 int
626 Ieee1394Service::getVerboseLevel()
627 {
628     return getDebugLevel();
629 }
630
631 void
632 Ieee1394Service::printBuffer( unsigned int level, size_t length, fb_quadlet_t* buffer ) const
633 {
634
635     for ( unsigned int i=0; i < length; ++i ) {
636         if ( ( i % 4 ) == 0 ) {
637             if ( i > 0 ) {
638                 debugOutputShort(level,"\n");
639             }
640             debugOutputShort(level," %4d: ",i*4);
641         }
642         debugOutputShort(level,"%08X ",buffer[i]);
643     }
644     debugOutputShort(level,"\n");
645 }
646 void
647 Ieee1394Service::printBufferBytes( unsigned int level, size_t length, byte_t* buffer ) const
648 {
649
650     for ( unsigned int i=0; i < length; ++i ) {
651         if ( ( i % 16 ) == 0 ) {
652             if ( i > 0 ) {
653                 debugOutputShort(level,"\n");
654             }
655             debugOutputShort(level," %4d: ",i*16);
656         }
657         debugOutputShort(level,"%02X ",buffer[i]);
658     }
659     debugOutputShort(level,"\n");
660 }
661
662 int
663 Ieee1394Service::resetHandlerLowLevel( raw1394handle_t handle,
664                                        unsigned int generation )
665 {
666     raw1394_update_generation ( handle, generation );
667     Ieee1394Service* instance
668         = (Ieee1394Service*) raw1394_get_userdata( handle );
669     instance->resetHandler( generation );
670
671     return 0;
672 }
673
674 bool
675 Ieee1394Service::resetHandler( unsigned int generation )
676 {
677     quadlet_t buf=0;
678
679     m_handle_lock->Lock();
680     raw1394_update_generation(m_handle, generation);
681     m_handle_lock->Unlock();
682
683     // do a simple read on ourself in order to update the internal structures
684     // this avoids failures after a bus reset
685     read_quadlet( getLocalNodeId() | 0xFFC0,
686                   CSR_REGISTER_BASE | CSR_CYCLE_TIME,
687                   &buf );
688
689     for ( reset_handler_vec_t::iterator it = m_busResetHandlers.begin();
690           it != m_busResetHandlers.end();
691           ++it )
692     {
693         Util::Functor* func = *it;
694         ( *func )();
695     }
696
697     return true;
698 }
699
700 bool Ieee1394Service::registerARMHandler(ARMHandler *h) {
701     debugOutput(DEBUG_LEVEL_VERBOSE, "Registering ARM handler (%p) for 0x%016llX, length %u\n",
702         h, h->getStart(), h->getLength());
703
704     int err=raw1394_arm_register(m_resetHandle, h->getStart(),
705                                  h->getLength(), h->getBuffer(), (octlet_t)h,
706                                  h->getAccessRights(),
707                                  h->getNotificationOptions(),
708                                  h->getClientTransactions());
709     if (err) {
710         debugError("Failed to register ARM handler for 0x%016llX\n", h->getStart());
711         debugError(" Error: %s\n", strerror(errno));
712         return false;
713     }
714
715     m_armHandlers.push_back( h );
716
717     return true;
718 }
719
720 bool Ieee1394Service::unregisterARMHandler( ARMHandler *h ) {
721     debugOutput(DEBUG_LEVEL_VERBOSE, "Unregistering ARM handler (%p) for 0x%016llX\n",
722         h, h->getStart());
723
724     for ( arm_handler_vec_t::iterator it = m_armHandlers.begin();
725           it != m_armHandlers.end();
726           ++it )
727     {
728         if((*it) == h) {
729             int err=raw1394_arm_unregister(m_resetHandle, h->getStart());
730             if (err) {
731                 debugError("Failed to unregister ARM handler (%p)\n", h);
732                 debugError(" Error: %s\n", strerror(errno));
733             } else {
734                 m_armHandlers.erase(it);
735                 return true;
736             }
737         }
738     }
739     debugOutput(DEBUG_LEVEL_VERBOSE, " handler not found!\n");
740
741     return false;
742 }
743 /**
744  * @brief Tries to find a free ARM address range
745  *
746  * @param start  address to start with
747  * @param length length of the block needed (bytes)
748  * @param step   step to use when searching (bytes)
749  * @return The base address that is free, and 0xFFFFFFFFFFFFFFFF when failed
750  */
751 nodeaddr_t Ieee1394Service::findFreeARMBlock( nodeaddr_t start, size_t length, size_t step ) {
752     debugOutput(DEBUG_LEVEL_VERBOSE, "Finding free ARM block of %d bytes, from 0x%016llX in steps of %d bytes\n",
753         length, start, step);
754
755     int cnt=0;
756     const int maxcnt=10;
757     int err=1;
758     while(err && cnt++ < maxcnt) {
759         // try to register
760         err=raw1394_arm_register(m_resetHandle, start, length, 0, 0, 0, 0, 0);
761
762         if (err) {
763             debugOutput(DEBUG_LEVEL_VERBOSE, " -> cannot use 0x%016llX\n", start);
764             debugError("    Error: %s\n", strerror(errno));
765             start += step;
766         } else {
767             debugOutput(DEBUG_LEVEL_VERBOSE, " -> use 0x%016llX\n", start);
768             err=raw1394_arm_unregister(m_resetHandle, start);
769             if (err) {
770                 debugOutput(DEBUG_LEVEL_VERBOSE, " error unregistering test handler\n");
771                 debugError("    Error: %s\n", strerror(errno));
772                 return 0xFFFFFFFFFFFFFFFFLLU;
773             }
774             return start;
775         }
776     }
777     debugOutput(DEBUG_LEVEL_VERBOSE, " Could not find free block in %d tries\n",cnt);
778     return 0xFFFFFFFFFFFFFFFFLLU;
779 }
780
781 int
782 Ieee1394Service::armHandlerLowLevel(raw1394handle_t handle,
783                      unsigned long arm_tag,
784                      byte_t request_type, unsigned int requested_length,
785                      void *data)
786 {
787     Ieee1394Service* instance
788         = (Ieee1394Service*) raw1394_get_userdata( handle );
789     instance->armHandler( arm_tag, request_type, requested_length, data );
790
791     return 0;
792 }
793
794 bool
795 Ieee1394Service::armHandler(  unsigned long arm_tag,
796                      byte_t request_type, unsigned int requested_length,
797                      void *data)
798 {
799     for ( arm_handler_vec_t::iterator it = m_armHandlers.begin();
800           it != m_armHandlers.end();
801           ++it )
802     {
803         if((*it) == (ARMHandler *)arm_tag) {
804             struct raw1394_arm_request_response *arm_req_resp;
805             arm_req_resp  = (struct raw1394_arm_request_response *) data;
806             raw1394_arm_request_t arm_req=arm_req_resp->request;
807             raw1394_arm_response_t arm_resp=arm_req_resp->response;
808
809             debugOutput(DEBUG_LEVEL_VERBOSE,"ARM handler for address 0x%016llX called\n",
810                 (*it)->getStart());
811             debugOutput(DEBUG_LEVEL_VERBOSE," request type   : 0x%02X\n",request_type);
812             debugOutput(DEBUG_LEVEL_VERBOSE," request length : %04d\n",requested_length);
813
814             switch(request_type) {
815                 case RAW1394_ARM_READ:
816                     (*it)->handleRead(arm_req);
817                     *arm_resp=*((*it)->getResponse());
818                     break;
819                 case RAW1394_ARM_WRITE:
820                     (*it)->handleWrite(arm_req);
821                     *arm_resp=*((*it)->getResponse());
822                     break;
823                 case RAW1394_ARM_LOCK:
824                     (*it)->handleLock(arm_req);
825                     *arm_resp=*((*it)->getResponse());
826                     break;
827                 default:
828                     debugWarning("Unknown request type received, ignoring...\n");
829             }
830
831             return true;
832         }
833     }
834
835     debugOutput(DEBUG_LEVEL_VERBOSE,"default ARM handler called\n");
836
837     m_default_arm_handler(m_resetHandle, arm_tag, request_type, requested_length, data );
838     return true;
839 }
840
841 bool
842 Ieee1394Service::startRHThread()
843 {
844     int i;
845
846     if ( m_threadRunning ) {
847         return true;
848     }
849     m_RHThread_lock->Lock();
850     i = pthread_create( &m_thread, 0, rHThread, this );
851     m_RHThread_lock->Unlock();
852     if (i) {
853         debugFatal("Could not start ieee1394 service thread\n");
854         return false;
855     }
856     m_threadRunning = true;
857
858     return true;
859 }
860
861 void
862 Ieee1394Service::stopRHThread()
863 {
864     if ( m_threadRunning ) {
865         // wait for the thread to finish it's work
866         m_RHThread_lock->Lock();
867         pthread_cancel (m_thread);
868         pthread_join (m_thread, 0);
869         m_RHThread_lock->Unlock();
870         m_threadRunning = false;
871     }
872 }
873
874 void*
875 Ieee1394Service::rHThread( void* arg )
876 {
877     Ieee1394Service* pIeee1394Service = (Ieee1394Service*) arg;
878
879     while (true) {
880         // protect ourselves from dying
881         {
882             // use a scoped lock such that it is unlocked
883             // even if we are cancelled while running
884             // FIXME: check if this is true!
885 //             Util::MutexLockHelper lock(*(pIeee1394Service->m_RHThread_lock));
886             raw1394_loop_iterate (pIeee1394Service->m_resetHandle);
887         }
888         pthread_testcancel ();
889     }
890
891     return 0;
892 }
893
894 bool
895 Ieee1394Service::addBusResetHandler( Util::Functor* functor )
896 {
897     debugOutput(DEBUG_LEVEL_VERBOSE, "Adding busreset handler (%p)\n", functor);
898     m_busResetHandlers.push_back( functor );
899     return true;
900 }
901
902 bool
903 Ieee1394Service::remBusResetHandler( Util::Functor* functor )
904 {
905     debugOutput(DEBUG_LEVEL_VERBOSE, "Removing busreset handler (%p)\n", functor);
906
907     for ( reset_handler_vec_t::iterator it = m_busResetHandlers.begin();
908           it != m_busResetHandlers.end();
909           ++it )
910     {
911         if ( *it == functor ) {
912             debugOutput(DEBUG_LEVEL_VERBOSE, " found\n");
913             m_busResetHandlers.erase( it );
914             return true;
915         }
916     }
917     debugOutput(DEBUG_LEVEL_VERBOSE, " not found\n");
918     return false;
919 }
920
921 /**
922  * Allocates an iso channel for use by the interface in a similar way to
923  * libiec61883.  Returns -1 on error (due to there being no free channels)
924  * or an allocated channel number.
925  *
926  * Does not perform anything other than registering the channel and the
927  * bandwidth at the IRM
928  *
929  * Also allocates the necessary bandwidth (in ISO allocation units).
930  *
931  * FIXME: As in libiec61883, channel 63 is not requested; this is either a
932  * bug or it's omitted since that's the channel preferred by video devices.
933  *
934  * @param bandwidth the bandwidth to allocate for this channel
935  * @return the channel number
936  */
937 signed int Ieee1394Service::allocateIsoChannelGeneric(unsigned int bandwidth) {
938     debugOutput(DEBUG_LEVEL_VERBOSE, "Allocating ISO channel using generic method...\n" );
939
940     Util::MutexLockHelper lock(*m_handle_lock);
941     struct ChannelInfo cinfo;
942
943     int c = -1;
944     for (c = 0; c < 63; c++) {
945         if (raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_ALLOC) == 0)
946             break;
947     }
948     if (c < 63) {
949         if (raw1394_bandwidth_modify(m_handle, bandwidth, RAW1394_MODIFY_ALLOC) < 0) {
950             debugFatal("Could not allocate bandwidth of %d\n", bandwidth);
951
952             raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_FREE);
953             return -1;
954         } else {
955             cinfo.channel=c;
956             cinfo.bandwidth=bandwidth;
957             cinfo.alloctype=AllocGeneric;
958
959             cinfo.xmit_node=-1;
960             cinfo.xmit_plug=-1;
961             cinfo.recv_node=-1;
962             cinfo.recv_plug=-1;
963
964             if (registerIsoChannel(c, cinfo)) {
965                 return c;
966             } else {
967                 raw1394_bandwidth_modify(m_handle, bandwidth, RAW1394_MODIFY_FREE);
968                 raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_FREE);
969                 return -1;
970             }
971         }
972     }
973     return -1;
974 }
975
976 /**
977  * Allocates an iso channel for use by the interface in a similar way to
978  * libiec61883.  Returns -1 on error (due to there being no free channels)
979  * or an allocated channel number.
980  *
981  * Uses IEC61883 Connection Management Procedure to establish the connection.
982  *
983  * Also allocates the necessary bandwidth (in ISO allocation units).
984  *
985  * @param xmit_node  node id of the transmitter
986  * @param xmit_plug  the output plug to use. If -1, find the first online plug, and
987  * upon return, contains the plug number used.
988  * @param recv_node  node id of the receiver
989  * @param recv_plug the input plug to use. If -1, find the first online plug, and
990  * upon return, contains the plug number used.
991  *
992  * @return the channel number
993  */
994
995 signed int Ieee1394Service::allocateIsoChannelCMP(
996     nodeid_t xmit_node, int xmit_plug,
997     nodeid_t recv_node, int recv_plug
998     ) {
999
1000     if (xmit_node == INVALID_NODE_ID) {
1001         debugWarning("operation on invalid node (XMIT)\n");
1002         return -1;
1003     }
1004     if (recv_node == INVALID_NODE_ID) {
1005         debugWarning("operation on invalid node (RECV)\n");
1006         return -1;
1007     }
1008
1009     debugOutput(DEBUG_LEVEL_VERBOSE, "Allocating ISO channel using IEC61883 CMP...\n" );
1010     Util::MutexLockHelper lock(*m_handle_lock);
1011
1012     struct ChannelInfo cinfo;
1013
1014     int c = -1;
1015     int bandwidth=1;
1016     #if IEEE1394SERVICE_SKIP_IEC61883_BANDWIDTH_ALLOCATION
1017     bandwidth=0;
1018     #endif
1019
1020     // do connection management: make connection
1021     c = iec61883_cmp_connect(
1022         m_handle,
1023         xmit_node | 0xffc0,
1024         &xmit_plug,
1025         recv_node | 0xffc0,
1026         &recv_plug,
1027         &bandwidth);
1028
1029     if((c<0) || (c>63)) {
1030         debugError("Could not do CMP from %04X:%02d to %04X:%02d\n",
1031             xmit_node, xmit_plug, recv_node, recv_plug
1032             );
1033         return -1;
1034     }
1035
1036     cinfo.channel=c;
1037     cinfo.bandwidth=bandwidth;
1038     cinfo.alloctype=AllocCMP;
1039
1040     cinfo.xmit_node=xmit_node;
1041     cinfo.xmit_plug=xmit_plug;
1042     cinfo.recv_node=recv_node;
1043     cinfo.recv_plug=recv_plug;
1044
1045     if (registerIsoChannel(c, cinfo)) {
1046         return c;
1047     }
1048
1049     return -1;
1050 }
1051
1052 /**
1053  * Deallocates an iso channel.  Silently ignores a request to deallocate
1054  * a negative channel number.
1055  *
1056  * Figures out the method that was used to allocate the channel (generic, cmp, ...)
1057  * and uses the appropriate method to deallocate. Also frees the bandwidth
1058  * that was reserved along with this channel.
1059  *
1060  * @param c channel number
1061  * @return true if successful
1062  */
1063 bool Ieee1394Service::freeIsoChannel(signed int c) {
1064     debugOutput(DEBUG_LEVEL_VERBOSE, "Freeing ISO channel %d...\n", c );
1065     Util::MutexLockHelper lock(*m_handle_lock);
1066
1067     if (c < 0 || c > 63) {
1068         debugWarning("Invalid channel number: %d\n", c);
1069         return false;
1070     }
1071
1072     switch (m_channels[c].alloctype) {
1073         default:
1074             debugError(" BUG: invalid allocation type!\n");
1075             return false;
1076
1077         case AllocFree:
1078             debugWarning(" Channel %d not registered\n", c);
1079             return false;
1080
1081         case AllocGeneric:
1082             debugOutput(DEBUG_LEVEL_VERBOSE, " allocated using generic routine...\n" );
1083             debugOutput(DEBUG_LEVEL_VERBOSE, " freeing %d bandwidth units...\n", m_channels[c].bandwidth );
1084             if (raw1394_bandwidth_modify(m_handle, m_channels[c].bandwidth, RAW1394_MODIFY_FREE) !=0) {
1085                 debugWarning("Failed to deallocate bandwidth\n");
1086             }
1087             debugOutput(DEBUG_LEVEL_VERBOSE, " freeing channel %d...\n", m_channels[c].channel );
1088             if (raw1394_channel_modify (m_handle, m_channels[c].channel, RAW1394_MODIFY_FREE) != 0) {
1089                 debugWarning("Failed to free channel\n");
1090             }
1091             if (!unregisterIsoChannel(c))
1092                 return false;
1093             return true;
1094
1095         case AllocCMP:
1096             debugOutput(DEBUG_LEVEL_VERBOSE, " allocated using IEC61883 CMP...\n" );
1097             debugOutput(DEBUG_LEVEL_VERBOSE, " performing IEC61883 CMP disconnect...\n" );
1098             if(iec61883_cmp_disconnect(
1099                     m_handle,
1100                     m_channels[c].xmit_node | 0xffc0,
1101                     m_channels[c].xmit_plug,
1102                     m_channels[c].recv_node | 0xffc0,
1103                     m_channels[c].recv_plug,
1104                     m_channels[c].channel,
1105                     m_channels[c].bandwidth) != 0) {
1106                 debugWarning("Could not do CMP disconnect for channel %d!\n",c);
1107             }
1108             if (!unregisterIsoChannel(c))
1109                 return false;
1110             return true;
1111     }
1112
1113     // unreachable
1114     debugError("BUG: unreachable code reached!\n");
1115
1116     return false;
1117 }
1118
1119 /**
1120  * Registers a channel as managed by this ieee1394service
1121  * @param c channel number
1122  * @param cinfo channel info struct
1123  * @return true if successful
1124  */
1125 bool Ieee1394Service::registerIsoChannel(unsigned int c, struct ChannelInfo cinfo) {
1126     if (c < 63) {
1127         if (m_channels[c].alloctype != AllocFree) {
1128             debugWarning("Channel %d already registered with bandwidth %d\n",
1129                 m_channels[c].channel, m_channels[c].bandwidth);
1130         }
1131
1132         memcpy(&m_channels[c], &cinfo, sizeof(struct ChannelInfo));
1133
1134     } else return false;
1135     return true;
1136 }
1137
1138 /**
1139  * unegisters a channel from this ieee1394service
1140  * @param c channel number
1141  * @return true if successful
1142  */
1143 bool Ieee1394Service::unregisterIsoChannel(unsigned int c) {
1144     if (c < 63) {
1145         if (m_channels[c].alloctype == AllocFree) {
1146             debugWarning("Channel %d not registered\n", c);
1147             return false;
1148         }
1149
1150         m_channels[c].channel=-1;
1151         m_channels[c].bandwidth=-1;
1152         m_channels[c].alloctype=AllocFree;
1153         m_channels[c].xmit_node=0xFFFF;
1154         m_channels[c].xmit_plug=-1;
1155         m_channels[c].recv_node=0xFFFF;
1156         m_channels[c].recv_plug=-1;
1157
1158     } else return false;
1159     return true;
1160 }
1161
1162 /**
1163  * Returns the current value of the `bandwidth available' register on
1164  * the IRM, or -1 on error.
1165  * @return
1166  */
1167 signed int Ieee1394Service::getAvailableBandwidth() {
1168     quadlet_t buffer;
1169     Util::MutexLockHelper lock(*m_handle_lock);
1170     signed int result = raw1394_read (m_handle, raw1394_get_irm_id (m_handle),
1171         CSR_REGISTER_BASE + CSR_BANDWIDTH_AVAILABLE,
1172         sizeof (quadlet_t), &buffer);
1173
1174     if (result < 0)
1175         return -1;
1176     return CondSwapFromBus32(buffer);
1177 }
1178
1179 void
1180 Ieee1394Service::setVerboseLevel(int l)
1181 {
1182     if (m_pIsoManager) m_pIsoManager->setVerboseLevel(l);
1183     if (m_pCTRHelper) m_pCTRHelper->setVerboseLevel(l);
1184     if (m_pWatchdog) m_pWatchdog->setVerboseLevel(l);
1185     setDebugLevel(l);
1186     debugOutput( DEBUG_LEVEL_VERBOSE, "Setting verbose level to %d...\n", l );
1187 }
1188
1189 void
1190 Ieee1394Service::show()
1191 {
1192     #ifdef DEBUG
1193     uint32_t cycle_timer;
1194     uint64_t local_time;
1195     if(!readCycleTimerReg(&cycle_timer, &local_time)) {
1196         debugWarning("Could not read cycle timer register\n");
1197    
1198     }
1199     uint64_t ctr = CYCLE_TIMER_TO_TICKS( cycle_timer );
1200
1201     debugOutput( DEBUG_LEVEL_VERBOSE, "Port:  %d\n", getPort() );
1202     debugOutput( DEBUG_LEVEL_VERBOSE, " Name: %s\n", getPortName().c_str() );
1203     debugOutput( DEBUG_LEVEL_VERBOSE, " CycleTimerHelper: %p, IsoManager: %p, WatchDog: %p\n",
1204                  m_pCTRHelper, m_pIsoManager, m_pWatchdog );
1205     debugOutput( DEBUG_LEVEL_VERBOSE, " Time: %011llu (%03us %04ucy %04uticks)\n",
1206                 ctr,
1207                 (unsigned int)TICKS_TO_SECS( ctr ),
1208                 (unsigned int)TICKS_TO_CYCLES( ctr ),
1209                 (unsigned int)TICKS_TO_OFFSET( ctr ) );
1210     debugOutputShort( DEBUG_LEVEL_NORMAL, "Iso handler info:\n");
1211     #endif
1212     if (m_pIsoManager) m_pIsoManager->dumpInfo();
1213 }
Note: See TracBrowser for help on using the browser.