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

Revision 1161, 35.2 kB (checked in by ppalmers, 16 years ago)

add busreset functionality

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