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

Revision 2162, 55.7 kB (checked in by jwoithe, 9 years ago)

Possibly address ticket #354. It seems that some FFADO threads can take a while to start: so long in fact that short operations (like 'ffado-test BusReset?') can be completed before all threads have completed their startup sequence. This in turn can create problems during shutdown (or any other operation which assumes that all threads are operational) . The obvious way to deal with this is with the use of a small delay to give the threads a chance to start, so we will try that and see what happens.

  • 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 "cycletimer.h"
29 #include "IsoHandlerManager.h"
30 #include "CycleTimerHelper.h"
31
32 #include <unistd.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 #include "libutil/PosixThread.h"
40 #include "libutil/Configuration.h"
41
42 #include <errno.h>
43 #include "libutil/ByteSwap.h"
44
45 #include <string.h>
46
47 #include <iostream>
48 #include <iomanip>
49
50 using namespace std;
51
52 IMPL_DEBUG_MODULE( Ieee1394Service, Ieee1394Service, DEBUG_LEVEL_NORMAL );
53
54 Ieee1394Service::Ieee1394Service()
55     : m_configuration( NULL )
56     , m_resetHelper( NULL )
57     , m_armHelperNormal( NULL )
58     , m_armHelperRealtime( NULL )
59     , m_handle( 0 )
60     , m_handle_lock( new Util::PosixMutex("SRVCHND") )
61     , m_util_handle( 0 )
62     , m_port( -1 )
63     , m_realtime ( false )
64     , m_base_priority ( 0 )
65     , m_pIsoManager( new IsoHandlerManager( *this ) )
66     , m_pCTRHelper ( new CycleTimerHelper( *this, IEEE1394SERVICE_CYCLETIMER_DLL_UPDATE_INTERVAL_USEC ) )
67     , m_have_new_ctr_read ( false )
68     , m_filterFCPResponse ( false )
69     , m_pWatchdog ( new Util::Watchdog() )
70 {
71     for (unsigned int i=0; i<64; i++) {
72         m_channels[i].channel=-1;
73         m_channels[i].bandwidth=-1;
74         m_channels[i].alloctype=AllocFree;
75         m_channels[i].xmit_node=0xFFFF;
76         m_channels[i].xmit_plug=-1;
77         m_channels[i].recv_node=0xFFFF;
78         m_channels[i].recv_plug=-1;
79     }
80 }
81
82 Ieee1394Service::Ieee1394Service(bool rt, int prio)
83     : m_configuration( NULL )
84     , m_resetHelper( NULL )
85     , m_armHelperNormal( NULL )
86     , m_armHelperRealtime( NULL )
87     , m_handle( 0 )
88     , m_handle_lock( new Util::PosixMutex("SRVCHND") )
89     , m_util_handle( 0 )
90     , m_port( -1 )
91     , m_realtime ( rt )
92     , m_base_priority ( prio )
93     , m_pIsoManager( new IsoHandlerManager( *this, rt, prio ) )
94     , m_pCTRHelper ( new CycleTimerHelper( *this, IEEE1394SERVICE_CYCLETIMER_DLL_UPDATE_INTERVAL_USEC,
95                                            rt && IEEE1394SERVICE_CYCLETIMER_HELPER_RUN_REALTIME,
96                                            IEEE1394SERVICE_CYCLETIMER_HELPER_PRIO ) )
97     , m_have_new_ctr_read ( false )
98     , m_filterFCPResponse ( false )
99     , m_pWatchdog ( new Util::Watchdog() )
100 {
101     for (unsigned int i=0; i<64; i++) {
102         m_channels[i].channel=-1;
103         m_channels[i].bandwidth=-1;
104         m_channels[i].alloctype=AllocFree;
105         m_channels[i].xmit_node=0xFFFF;
106         m_channels[i].xmit_plug=-1;
107         m_channels[i].recv_node=0xFFFF;
108         m_channels[i].recv_plug=-1;
109     }
110 }
111
112 Ieee1394Service::~Ieee1394Service()
113 {
114     delete m_pIsoManager;
115     delete m_pCTRHelper;
116
117     m_resetHelper->Stop();
118     m_armHelperNormal->Stop();
119     m_armHelperRealtime->Stop();
120
121     for ( arm_handler_vec_t::iterator it = m_armHandlers.begin();
122           it != m_armHandlers.end();
123           ++it )
124     {
125         debugOutput(DEBUG_LEVEL_VERBOSE, "Unregistering ARM handler for 0x%016"PRIX64"\n", (*it)->getStart());
126         if(m_armHelperNormal) {
127             int err = raw1394_arm_unregister(m_armHelperNormal->get1394Handle(), (*it)->getStart());
128             if (err) {
129                 debugError(" Failed to unregister ARM handler for 0x%016"PRIX64"\n", (*it)->getStart());
130                 debugError(" Error: %s\n", strerror(errno));
131             }
132         } else {
133             debugWarning("ARM handler registered without valid ARM helper thread\n");
134         }
135     }
136
137     delete m_pWatchdog;
138     if ( m_handle ) {
139         raw1394_destroy_handle( m_handle );
140     }
141     delete m_handle_lock;
142
143     if(m_resetHelper) delete m_resetHelper;
144     if(m_armHelperNormal) delete m_armHelperNormal;
145     if(m_armHelperRealtime) delete m_armHelperRealtime;
146
147     if ( m_util_handle ) {
148         raw1394_destroy_handle( m_util_handle );
149     }
150 }
151
152 bool
153 Ieee1394Service::useConfiguration(Util::Configuration *c)
154 {
155     m_configuration = c;
156     return configurationUpdated();
157 }
158
159 bool
160 Ieee1394Service::configurationUpdated()
161 {
162     if(m_configuration) {
163        
164     }
165     return true;
166 }
167
168 #define DEVICEFAILTEXT "Could not get libraw1394 handle.\n\
169 This usually means:\n\
170  a) The device-node /dev/raw1394 doesn't exists because you don't have a\n\
171     (recognized) firewire controller.\n \
172  b) The modules needed aren't loaded. This is not in the scope of ffado but of\n\
173     your distribution, so if you have a firewire controller that should be\n\
174     supported and the modules aren't loaded, file a bug with your distributions\n\
175     bug tracker.\n \
176  c) You don't have permissions to access /dev/raw1394. 'ls -l /dev/raw1394'\n\
177     shows the device-node with its permissions, make sure you belong to the\n\
178     right group and the group is allowed to access the device.\n"
179
180 int
181 Ieee1394Service::detectNbPorts()
182 {
183     raw1394handle_t tmp_handle = raw1394_new_handle();
184     if ( tmp_handle == NULL ) {
185         debugError(DEVICEFAILTEXT);
186         return -1;
187     }
188     struct raw1394_portinfo pinf[IEEE1394SERVICE_MAX_FIREWIRE_PORTS];
189     int nb_detected_ports = raw1394_get_port_info(tmp_handle, pinf, IEEE1394SERVICE_MAX_FIREWIRE_PORTS);
190     raw1394_destroy_handle(tmp_handle);
191
192     if (nb_detected_ports < 0) {
193         debugError("Failed to detect number of ports\n");
194         return -1;
195     }
196     return nb_detected_ports;
197 }
198
199 void
200 Ieee1394Service::doBusReset() {
201     debugOutput(DEBUG_LEVEL_VERBOSE, "Issue bus reset on service %p (port %d).\n", this, getPort());
202     raw1394_reset_bus(m_handle);
203 }
204
205 /**
206  * This function waits until there are no bus resets generated in a sleep_time_ms interval
207  * @param nb_tries number of tries to take
208  * @param sleep_time_ms sleep between tries
209  * @return true if the storm passed
210  */
211 bool
212 Ieee1394Service::waitForBusResetStormToEnd( int nb_tries, int sleep_time_ms ) {
213     unsigned int gen_current;
214     do {
215         gen_current = getGeneration();
216         debugOutput(DEBUG_LEVEL_VERBOSE, "Waiting... (gen: %u)\n", gen_current);
217
218         // wait for a while
219         Util::SystemTimeSource::SleepUsecRelative( sleep_time_ms * 1000);
220     } while (gen_current != getGeneration() && --nb_tries);
221
222     debugOutput(DEBUG_LEVEL_VERBOSE, "Bus reset storm over at gen: %u\n", gen_current);
223
224     if (!nb_tries) {
225         debugError( "Bus reset storm did not stop on time...\n");
226         return false;
227     }
228     return true;
229 }
230
231 bool
232 Ieee1394Service::initialize( int port )
233 {
234     using namespace std;
235
236     int nb_ports = detectNbPorts();
237     if (port + 1 > nb_ports) {
238         debugFatal("Requested port (%d) out of range (# ports: %d)\n", port, nb_ports);
239     }
240     m_port = port;
241
242     if(!m_pWatchdog) {
243         debugError("No valid RT watchdog found.\n");
244         return false;
245     }
246     if(!m_pWatchdog->start()) {
247         debugError("Could not start RT watchdog.\n");
248         return false;
249     }
250
251     m_handle = raw1394_new_handle_on_port( port );
252     if ( !m_handle ) {
253         if ( !errno ) {
254             debugFatal("libraw1394 not compatible\n");
255         } else {
256             debugFatal("Ieee1394Service::initialize: Could not get 1394 handle: %s\n",
257                 strerror(errno) );
258             debugFatal("Is ieee1394 and raw1394 driver loaded?\n");
259         }
260         return false;
261     }
262
263     // helper threads for all sorts of ASYNC events
264     // note: m_port has to be set!
265     m_resetHelper = new HelperThread(*this, "BUSRST");
266     if ( !m_resetHelper ) {
267         debugFatal("Could not allocate busreset handler helper\n");
268         return false;
269     }
270     m_armHelperNormal = new HelperThread(*this, "ARMSTD");
271     if ( !m_armHelperNormal ) {
272         debugFatal("Could not allocate standard ARM handler helper\n");
273         return false;
274     }
275     m_armHelperRealtime = new HelperThread(*this, "ARMRT", m_realtime, m_base_priority);
276     if ( !m_armHelperRealtime ) {
277         debugFatal("Could not allocate realtime ARM handler helper\n");
278         return false;
279     }
280
281     // start helper threads
282     if(!m_resetHelper->Start()) {
283         debugFatal("Could not start busreset helper thread\n");
284         return false;
285     }
286     if(!m_armHelperNormal->Start()) {
287         debugFatal("Could not start standard ARM helper thread\n");
288         return false;
289     }
290     if(!m_armHelperRealtime->Start()) {
291         debugFatal("Could not start realtime ARM helper thread\n");
292         return false;
293     }
294
295     // attach the reset and ARM handlers
296     // NOTE: the handlers have to be started first, or there is no 1394handle
297     raw1394_set_bus_reset_handler( m_resetHelper->get1394Handle(),
298                                    this->resetHandlerLowLevel );
299
300     m_default_arm_handler = raw1394_set_arm_tag_handler( m_armHelperNormal->get1394Handle(),
301                                    this->armHandlerLowLevel );
302
303     // utility handle (used to read the CTR register)
304     m_util_handle = raw1394_new_handle_on_port( port );
305     if ( !m_util_handle ) {
306         if ( !errno ) {
307             debugFatal("libraw1394 not compatible\n");
308         } else {
309             debugFatal("Ieee1394Service::initialize: Could not get 1394 handle: %s",
310                 strerror(errno) );
311             debugFatal("Is ieee1394 and raw1394 driver loaded?\n");
312         }
313         return false;
314     }
315
316     // test the cycle timer read function
317     int err;
318     uint32_t cycle_timer;
319     uint64_t local_time;
320     err = raw1394_read_cycle_timer(m_util_handle, &cycle_timer, &local_time);
321     if(err) {
322         debugOutput(DEBUG_LEVEL_VERBOSE, "raw1394_read_cycle_timer failed.\n");
323         debugOutput(DEBUG_LEVEL_VERBOSE, " Error descr: %s\n", strerror(err));
324         debugWarning("==================================================================\n");
325         debugWarning(" This system doesn't support the raw1394_read_cycle_timer call.   \n");
326         debugWarning(" Fallback to indirect CTR read method.                            \n");
327         debugWarning(" FFADO should work, but achieving low-latency might be a problem. \n");
328         debugWarning(" Upgrade the kernel to version 2.6.21 or higher to solve this.    \n");
329         debugWarning("==================================================================\n");
330         m_have_new_ctr_read = false;
331     } else {
332         debugOutput(DEBUG_LEVEL_VERBOSE, "This system supports the raw1394_read_cycle_timer call, using it.\n");
333         m_have_new_ctr_read = true;
334     }
335
336     // obtain port name
337     raw1394handle_t tmp_handle = raw1394_new_handle();
338     if ( tmp_handle == NULL ) {
339         debugError("Could not get temporary libraw1394 handle.\n");
340         return false;
341     }
342     struct raw1394_portinfo pinf[IEEE1394SERVICE_MAX_FIREWIRE_PORTS];
343     int nb_detected_ports = raw1394_get_port_info(tmp_handle, pinf, IEEE1394SERVICE_MAX_FIREWIRE_PORTS);
344     raw1394_destroy_handle(tmp_handle);
345
346     if (nb_detected_ports < 0) {
347         debugError("Failed to detect number of ports\n");
348         return false;
349     }
350
351     if(nb_detected_ports && port < IEEE1394SERVICE_MAX_FIREWIRE_PORTS) {
352         m_portName = pinf[port].name;
353     } else {
354         m_portName = "Unknown";
355     }
356     if (m_portName == "") {
357         m_portName = "Unknown";
358     }
359
360     // set userdata
361     raw1394_set_userdata( m_handle, this );
362     raw1394_set_userdata( m_util_handle, this );
363
364     // increase the split-transaction timeout if required (e.g. for bebob's)
365     int split_timeout = IEEE1394SERVICE_MIN_SPLIT_TIMEOUT_USECS;
366     if(m_configuration) {
367         m_configuration->getValueForSetting("ieee1394.min_split_timeout_usecs", split_timeout);
368     }
369
370     // set SPLIT_TIMEOUT to one second to cope with DM1x00 devices that
371     // send responses regardless of the timeout
372     int timeout = getSplitTimeoutUsecs(getLocalNodeId());
373     debugOutput(DEBUG_LEVEL_VERBOSE, "Minimum SPLIT_TIMEOUT: %d. Current: %d\n", split_timeout, timeout);
374     if (timeout < split_timeout) {
375         if(!setSplitTimeoutUsecs(getLocalNodeId(), split_timeout+124)) {
376             debugWarning("Could not set SPLIT_TIMEOUT to min requested (%d)\n", split_timeout);
377         }
378         timeout = getSplitTimeoutUsecs(getLocalNodeId());
379         if (timeout < split_timeout) {
380             debugWarning("Set SPLIT_TIMEOUT to min requested (%d) did not succeed\n", split_timeout);
381         }
382     }
383
384     // init helpers
385     if(!m_pCTRHelper) {
386         debugFatal("No CycleTimerHelper available, bad!\n");
387         return false;
388     }
389     m_pCTRHelper->setVerboseLevel(getDebugLevel());
390     if(!m_pCTRHelper->Start()) {
391         debugFatal("Could not start CycleTimerHelper\n");
392         return false;
393     }
394
395     if(!m_pIsoManager) {
396         debugFatal("No IsoHandlerManager available, bad!\n");
397         return false;
398     }
399     m_pIsoManager->setVerboseLevel(getDebugLevel());
400
401     if(!m_pIsoManager->init()) {
402         debugFatal("Could not initialize IsoHandlerManager\n");
403         return false;
404     }
405
406     // Give time for all threads to start.  Otherwise it can happen that
407     // some FFADO threads are ready to run before others.  If the former
408     // then go ahead and use the latter on the assumption that they're
409     // running, race conditions can occur.  The delay here has been
410     // determined experimentally.
411     usleep(500);
412
413     // make sure that the thread parameters of all our helper threads are OK
414     if(!setThreadParameters(m_realtime, m_base_priority)) {
415         debugFatal("Could not set thread parameters\n");
416         return false;
417     }
418     return true;
419 }
420
421 bool
422 Ieee1394Service::setThreadParameters(bool rt, int priority) {
423     bool result = true;
424     if (priority > THREAD_MAX_RTPRIO) priority = THREAD_MAX_RTPRIO;
425     if (priority < THREAD_MIN_RTPRIO) priority = THREAD_MIN_RTPRIO;
426     m_base_priority = priority;
427     m_realtime = rt;
428     if (m_pIsoManager) {
429         debugOutput(DEBUG_LEVEL_VERBOSE, "Switching IsoManager to (rt=%d, prio=%d)\n",
430                                          rt, priority);
431         result &= m_pIsoManager->setThreadParameters(rt, priority);
432     } //else debugError("Bogus isomanager\n");
433     if (m_pCTRHelper) {
434         debugOutput(DEBUG_LEVEL_VERBOSE, "Switching CycleTimerHelper to (rt=%d, prio=%d)\n",
435                                          rt && IEEE1394SERVICE_CYCLETIMER_HELPER_RUN_REALTIME,
436                                          IEEE1394SERVICE_CYCLETIMER_HELPER_PRIO);
437         result &= m_pCTRHelper->setThreadParameters(rt && IEEE1394SERVICE_CYCLETIMER_HELPER_RUN_REALTIME,
438                                                     IEEE1394SERVICE_CYCLETIMER_HELPER_PRIO);
439     } //else debugError("Bogus CTR helper\n");
440     if(m_armHelperRealtime) {
441         m_armHelperRealtime->setThreadParameters(rt, priority);
442     } //else debugError("Bogus RT ARM helper\n");
443     return result;
444 }
445
446 int
447 Ieee1394Service::getNodeCount()
448 {
449     Util::MutexLockHelper lock(*m_handle_lock);
450     return raw1394_get_nodecount( m_handle );
451 }
452
453 nodeid_t Ieee1394Service::getLocalNodeId() {
454     Util::MutexLockHelper lock(*m_handle_lock);
455     return raw1394_get_local_id(m_handle) & 0x3F;
456 }
457
458 /**
459  * Returns the current value of the cycle timer (in ticks)
460  *
461  * @return the current value of the cycle timer (in ticks)
462  */
463
464 uint32_t
465 Ieee1394Service::getCycleTimerTicks() {
466     return m_pCTRHelper->getCycleTimerTicks();
467 }
468
469 /**
470  * Returns the current value of the cycle timer (as is)
471  *
472  * @return the current value of the cycle timer (as is)
473  */
474 uint32_t
475 Ieee1394Service::getCycleTimer() {
476     return m_pCTRHelper->getCycleTimer();
477 }
478
479 /**
480  * Returns the current value of the cycle timer (in ticks)
481  * for a specific time instant (usecs since epoch)
482  * @return the current value of the cycle timer (in ticks)
483  */
484
485 uint32_t
486 Ieee1394Service::getCycleTimerTicks(uint64_t t) {
487     return m_pCTRHelper->getCycleTimerTicks(t);
488 }
489
490 /**
491  * Returns the current value of the cycle timer (as is)
492  * for a specific time instant (usecs since epoch)
493  * @return the current value of the cycle timer (as is)
494  */
495 uint32_t
496 Ieee1394Service::getCycleTimer(uint64_t t) {
497     return m_pCTRHelper->getCycleTimer(t);
498 }
499
500 uint64_t
501 Ieee1394Service::getSystemTimeForCycleTimerTicks(uint32_t ticks) {
502     return m_pCTRHelper->getSystemTimeForCycleTimerTicks(ticks);
503 }
504
505 uint64_t
506 Ieee1394Service::getSystemTimeForCycleTimer(uint32_t ctr) {
507     return m_pCTRHelper->getSystemTimeForCycleTimer(ctr);
508 }
509
510 bool
511 Ieee1394Service::readCycleTimerReg(uint32_t *cycle_timer, uint64_t *local_time)
512 {
513     if(m_have_new_ctr_read) {
514         int err;
515         err = raw1394_read_cycle_timer(m_util_handle, cycle_timer, local_time);
516         if(err) {
517             debugWarning("raw1394_read_cycle_timer: %s\n", strerror(err));
518             return false;
519         }
520         return true;
521     } else {
522         // do a normal read of the CTR register
523         // the disadvantage is that local_time and cycle time are not
524         // read at the same time instant (scheduling issues)
525         *local_time = getCurrentTimeAsUsecs();
526         if ( raw1394_read( m_util_handle,
527                 getLocalNodeId() | 0xFFC0,
528                 CSR_REGISTER_BASE | CSR_CYCLE_TIME,
529                 sizeof(uint32_t), cycle_timer ) == 0 ) {
530             *cycle_timer = CondSwapFromBus32(*cycle_timer);
531             return true;
532         } else {
533             return false;
534         }
535     }
536 }
537
538 uint64_t
539 Ieee1394Service::getCurrentTimeAsUsecs() {
540     return Util::SystemTimeSource::getCurrentTimeAsUsecs();
541 }
542
543 bool
544 Ieee1394Service::read( fb_nodeid_t nodeId,
545                        fb_nodeaddr_t addr,
546                        size_t length,
547                        fb_quadlet_t* buffer )
548 {
549     Util::MutexLockHelper lock(*m_handle_lock);
550     return readNoLock(nodeId, addr, length, buffer);
551 }
552
553 bool
554 Ieee1394Service::readNoLock( fb_nodeid_t nodeId,
555                              fb_nodeaddr_t addr,
556                              size_t length,
557                              fb_quadlet_t* buffer )
558 {
559     if (nodeId == INVALID_NODE_ID) {
560         debugWarning("operation on invalid node\n");
561         return false;
562     }
563     if ( raw1394_read( m_handle, nodeId, addr, length*4, buffer ) == 0 ) {
564
565         #ifdef DEBUG
566         debugOutput(DEBUG_LEVEL_VERY_VERBOSE,
567             "read: node 0x%hX, addr = 0x%016"PRIX64", length = %zd\n",
568             nodeId, addr, length);
569         printBuffer( DEBUG_LEVEL_VERY_VERBOSE, length, buffer );
570         #endif
571
572         return true;
573     } else {
574         #ifdef DEBUG
575         debugOutput(DEBUG_LEVEL_VERBOSE,
576                     "raw1394_read failed: node 0x%hX, addr = 0x%016"PRIX64", length = %zd\n",
577                     nodeId, addr, length);
578         #endif
579         return false;
580     }
581 }
582
583 bool
584 Ieee1394Service::read_quadlet( fb_nodeid_t nodeId,
585                                fb_nodeaddr_t addr,
586                                fb_quadlet_t* buffer )
587 {
588     return read( nodeId,  addr, sizeof( *buffer )/4, buffer );
589 }
590
591 bool
592 Ieee1394Service::read_octlet( fb_nodeid_t nodeId,
593                               fb_nodeaddr_t addr,
594                               fb_octlet_t* buffer )
595 {
596     return read( nodeId, addr, sizeof( *buffer )/4,
597                  reinterpret_cast<fb_quadlet_t*>( buffer ) );
598 }
599
600 bool
601 Ieee1394Service::write( fb_nodeid_t nodeId,
602                         fb_nodeaddr_t addr,
603                         size_t length,
604                         fb_quadlet_t* data )
605 {
606     Util::MutexLockHelper lock(*m_handle_lock);
607     return writeNoLock(nodeId, addr, length, data);
608 }
609
610 bool
611 Ieee1394Service::writeNoLock( fb_nodeid_t nodeId,
612                               fb_nodeaddr_t addr,
613                               size_t length,
614                               fb_quadlet_t* data )
615 {
616     if (nodeId == INVALID_NODE_ID) {
617         debugWarning("operation on invalid node\n");
618         return false;
619     }
620
621     #ifdef DEBUG
622     debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"write: node 0x%hX, addr = 0x%016"PRIX64", length = %zd\n",
623                 nodeId, addr, length);
624     printBuffer( DEBUG_LEVEL_VERY_VERBOSE, length, data );
625     #endif
626
627     return raw1394_write( m_handle, nodeId, addr, length*4, data ) == 0;
628 }
629
630 bool
631 Ieee1394Service::write_quadlet( fb_nodeid_t nodeId,
632                                 fb_nodeaddr_t addr,
633                                 fb_quadlet_t data )
634 {
635     return write( nodeId, addr, sizeof( data )/4, &data );
636 }
637
638 bool
639 Ieee1394Service::write_octlet( fb_nodeid_t nodeId,
640                                fb_nodeaddr_t addr,
641                                fb_octlet_t data )
642 {
643     return write( nodeId, addr, sizeof( data )/4,
644                   reinterpret_cast<fb_quadlet_t*>( &data ) );
645 }
646
647 bool
648 Ieee1394Service::lockCompareSwap64( fb_nodeid_t nodeId,
649                                     fb_nodeaddr_t addr,
650                                     fb_octlet_t compare_value,
651                                     fb_octlet_t swap_value,
652                                     fb_octlet_t* result )
653 {
654     if (nodeId == INVALID_NODE_ID) {
655         debugWarning("operation on invalid node\n");
656         return false;
657     }
658     #ifdef DEBUG
659     debugOutput(DEBUG_LEVEL_VERBOSE,"lockCompareSwap64: node 0x%X, addr = 0x%016"PRIX64"\n",
660                 nodeId, addr);
661     debugOutput(DEBUG_LEVEL_VERBOSE,"  if (*(addr)==0x%016"PRIX64") *(addr)=0x%016"PRIX64"\n",
662                 compare_value, swap_value);
663     fb_octlet_t buffer;
664     if(!read_octlet( nodeId, addr,&buffer )) {
665         debugWarning("Could not read register\n");
666     } else {
667         debugOutput(DEBUG_LEVEL_VERBOSE,"before = 0x%016"PRIX64"\n", buffer);
668     }
669     #endif
670
671     // do endiannes swapping
672     compare_value = CondSwapToBus64(compare_value);
673     swap_value    = CondSwapToBus64(swap_value);
674
675     // do separate locking here (no MutexLockHelper) since
676     // we use read_octlet in the DEBUG code in this function
677     m_handle_lock->Lock();
678     int retval=raw1394_lock64(m_handle, nodeId, addr,
679                               RAW1394_EXTCODE_COMPARE_SWAP,
680                               swap_value, compare_value, result);
681     m_handle_lock->Unlock();
682
683     if(retval) {
684         debugError("raw1394_lock64 failed: %s\n", strerror(errno));
685     }
686
687     #ifdef DEBUG
688     if(!read_octlet( nodeId, addr,&buffer )) {
689         debugWarning("Could not read register\n");
690     } else {
691         debugOutput(DEBUG_LEVEL_VERBOSE,"after = 0x%016"PRIX64"\n", buffer);
692     }
693     #endif
694
695     *result = CondSwapFromBus64(*result);
696
697     return (retval == 0);
698 }
699
700 fb_quadlet_t*
701 Ieee1394Service::transactionBlock( fb_nodeid_t nodeId,
702                                    fb_quadlet_t* buf,
703                                    int len,
704                                    unsigned int* resp_len )
705 {
706     // FIXME: simplify semantics
707     if (nodeId == INVALID_NODE_ID) {
708         debugWarning("operation on invalid node\n");
709         return false;
710     }
711     // NOTE: this expects a call to transactionBlockClose to unlock
712     m_handle_lock->Lock();
713
714     // clear the request & response memory
715     memset(&m_fcp_block, 0, sizeof(m_fcp_block));
716
717     // make a local copy of the request
718     if(len < MAX_FCP_BLOCK_SIZE_QUADS) {
719         memcpy(m_fcp_block.request, buf, len*sizeof(quadlet_t));
720         m_fcp_block.request_length = len;
721     } else {
722         debugWarning("Truncating FCP request\n");
723         memcpy(m_fcp_block.request, buf, MAX_FCP_BLOCK_SIZE_BYTES);
724         m_fcp_block.request_length = MAX_FCP_BLOCK_SIZE_QUADS;
725     }
726     m_fcp_block.target_nodeid = 0xffc0 | nodeId;
727
728     bool success = doFcpTransaction();
729     if(success) {
730         *resp_len = m_fcp_block.response_length;
731         return m_fcp_block.response;
732     } else {
733         debugWarning("FCP transaction failed\n");
734         *resp_len = 0;
735         return NULL;
736     }
737 }
738
739 bool
740 Ieee1394Service::transactionBlockClose()
741 {
742     m_handle_lock->Unlock();
743     return true;
744 }
745
746 // FCP code
747 bool
748 Ieee1394Service::doFcpTransaction()
749 {
750     for(int i=0; i < IEEE1394SERVICE_FCP_MAX_TRIES; i++) {
751         if(doFcpTransactionTry()) {
752             return true;
753         } else {
754             debugOutput(DEBUG_LEVEL_VERBOSE, "FCP transaction try %d failed\n", i);
755             Util::SystemTimeSource::SleepUsecRelative( IEEE1394SERVICE_FCP_SLEEP_BETWEEN_FAILURES_USECS);
756         }
757     }
758     debugError("FCP transaction didn't succeed in %d tries\n", IEEE1394SERVICE_FCP_MAX_TRIES);
759     return false;
760 }
761
762 #define FCP_COMMAND_ADDR   0xFFFFF0000B00ULL
763 #define FCP_RESPONSE_ADDR  0xFFFFF0000D00ULL
764
765 /* AV/C FCP response codes */
766 #define FCP_RESPONSE_NOT_IMPLEMENTED 0x08000000
767 #define FCP_RESPONSE_ACCEPTED 0x09000000
768 #define FCP_RESPONSE_REJECTED 0x0A000000
769 #define FCP_RESPONSE_IN_TRANSITION 0x0B000000
770 #define FCP_RESPONSE_IMPLEMENTED 0x0C000000
771 #define FCP_RESPONSE_STABLE 0x0C000000
772 #define FCP_RESPONSE_CHANGED 0x0D000000
773 #define FCP_RESPONSE_INTERIM 0x0F000000
774
775 /* AV/C FCP mask macros */
776 #define FCP_MASK_START(x) ((x) & 0xF0000000)
777 #define FCP_MASK_CTYPE(x) ((x) & 0x0F000000)
778 #define FCP_MASK_RESPONSE(x) ((x) & 0x0F000000)
779 #define FCP_MASK_SUBUNIT(x) ((x) & 0x00FF0000)
780 #define FCP_MASK_SUBUNIT_TYPE(x) ((x) & 0x00F80000)
781 #define FCP_MASK_SUBUNIT_ID(x) ((x) & 0x00070000)
782 #define FCP_MASK_OPCODE(x) ((x) & 0x0000FF00)
783 #define FCP_MASK_SUBUNIT_AND_OPCODE(x) ((x) & 0x00FFFF00)
784 #define FCP_MASK_OPERAND0(x) ((x) & 0x000000FF)
785 #define FCP_MASK_OPERAND(x, n) ((x) & (0xFF000000 >> ((((n)-1)%4)*8)))
786 #define FCP_MASK_RESPONSE_OPERAND(x, n) ((x) & (0xFF000000 >> (((n)%4)*8)))
787
788 bool
789 Ieee1394Service::doFcpTransactionTry()
790 {
791     // NOTE that access to this is protected by the m_handle lock
792     int err;
793     bool retval = true;
794     uint64_t timeout;
795
796     // prepare an fcp response handler
797     raw1394_set_fcp_handler(m_handle, _avc_fcp_handler);
798
799     // start listening for FCP requests
800     // this fails if some other program is listening for a FCP response
801     err = raw1394_start_fcp_listen(m_handle);
802     if(err) {
803         debugOutput(DEBUG_LEVEL_VERBOSE, "could not start FCP listen (err=%d, errno=%d)\n", err, errno);
804         retval = false;
805         goto out;
806     }
807
808     m_fcp_block.status = eFS_Waiting;
809
810     #ifdef DEBUG
811     debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"fcp request: node 0x%hX, length = %d bytes\n",
812                 m_fcp_block.target_nodeid, m_fcp_block.request_length*4);
813     printBuffer(DEBUG_LEVEL_VERY_VERBOSE, m_fcp_block.request_length, m_fcp_block.request );
814     #endif
815
816     // write the FCP request
817     if(!writeNoLock( m_fcp_block.target_nodeid, FCP_COMMAND_ADDR,
818                      m_fcp_block.request_length, m_fcp_block.request)) {
819         debugOutput(DEBUG_LEVEL_VERBOSE, "write of FCP request failed\n");
820         retval = false;
821         goto out;
822     }
823
824     // wait for the response to arrive
825     struct pollfd raw1394_poll;
826     raw1394_poll.fd = raw1394_get_fd(m_handle);
827     raw1394_poll.events = POLLIN;
828
829     timeout = Util::SystemTimeSource::getCurrentTimeAsUsecs() +
830               IEEE1394SERVICE_FCP_RESPONSE_TIMEOUT_USEC;
831
832     while(m_fcp_block.status == eFS_Waiting
833           && Util::SystemTimeSource::getCurrentTimeAsUsecs() < timeout) {
834         if(poll( &raw1394_poll, 1, IEEE1394SERVICE_FCP_POLL_TIMEOUT_MSEC) > 0) {
835             if (raw1394_poll.revents & POLLIN) {
836                 raw1394_loop_iterate(m_handle);
837             }
838         }
839     }
840
841     // check the request and figure out what happened
842     if(m_fcp_block.status == eFS_Waiting) {
843         debugOutput(DEBUG_LEVEL_VERBOSE, "FCP response timed out\n");
844         retval = false;
845         goto out;
846     }
847     if(m_fcp_block.status == eFS_Error) {
848         debugError("FCP request/response error\n");
849         retval = false;
850         goto out;
851     }
852
853 out:
854     // stop listening for FCP responses
855     err = raw1394_stop_fcp_listen(m_handle);
856     if(err) {
857         debugOutput(DEBUG_LEVEL_VERBOSE, "could not stop FCP listen (err=%d, errno=%d)\n", err, errno);
858         retval = false;
859     }
860
861     m_fcp_block.status = eFS_Empty;
862     return retval;
863 }
864
865 int
866 Ieee1394Service::_avc_fcp_handler(raw1394handle_t handle, nodeid_t nodeid,
867                                   int response, size_t length,
868                                   unsigned char *data)
869 {
870     Ieee1394Service *service = static_cast<Ieee1394Service *>(raw1394_get_userdata(handle));
871     if(service) {
872         return service->handleFcpResponse(nodeid, response, length, data);
873     } else return -1;
874 }
875
876 int
877 Ieee1394Service::handleFcpResponse(nodeid_t nodeid,
878                                    int response, size_t length,
879                                    unsigned char *data)
880 {
881     static struct sFcpBlock fcp_block_last;
882
883     fb_quadlet_t *data_quads = (fb_quadlet_t *)data;
884     #ifdef DEBUG
885     debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"fcp response: node 0x%hX, response = %d, length = %zd bytes\n",
886                 nodeid, response, length);
887     printBuffer(DEBUG_LEVEL_VERY_VERBOSE, (length+3)/4, data_quads );
888     #endif
889
890     if (response && length > 3) {
891         if(length > MAX_FCP_BLOCK_SIZE_BYTES) {
892             length = MAX_FCP_BLOCK_SIZE_BYTES;
893             debugWarning("Truncated FCP response\n");
894         }
895
896         // is it an actual response or is it INTERIM?
897         quadlet_t first_quadlet = CondSwapFromBus32(data_quads[0]);
898         if(FCP_MASK_RESPONSE(first_quadlet) == FCP_RESPONSE_INTERIM) {
899             debugOutput(DEBUG_LEVEL_VERBOSE, "INTERIM\n");
900         } else {
901             // it's an actual response, check if it matches our request
902             if(nodeid != m_fcp_block.target_nodeid) {
903                 debugOutput(DEBUG_LEVEL_VERBOSE, "FCP response node id's don't match! (%x, %x)\n",
904                                                  m_fcp_block.target_nodeid, nodeid);
905             } else if (first_quadlet == 0) {
906                 debugWarning("Bogus FCP response\n");
907                 printBuffer(DEBUG_LEVEL_WARNING, (length+3)/4, data_quads );
908 #ifdef DEBUG
909             } else if(FCP_MASK_RESPONSE(first_quadlet) < 0x08000000) {
910                 debugWarning("Bogus AV/C FCP response code\n");
911                 printBuffer(DEBUG_LEVEL_WARNING, (length+3)/4, data_quads );
912 #endif
913             } else if(FCP_MASK_SUBUNIT_AND_OPCODE(first_quadlet)
914                       != FCP_MASK_SUBUNIT_AND_OPCODE(CondSwapFromBus32(m_fcp_block.request[0]))) {
915                 debugOutput(DEBUG_LEVEL_VERBOSE, "FCP response not for this request: %08X != %08X\n",
916                              FCP_MASK_SUBUNIT_AND_OPCODE(first_quadlet),
917                              FCP_MASK_SUBUNIT_AND_OPCODE(CondSwapFromBus32(m_fcp_block.request[0])));
918             } else if(m_filterFCPResponse && (memcmp(fcp_block_last.response, data, length) == 0)) {
919                 // This is workaround for the Edirol FA-101. The device tends to send more than
920                 // one responde to one request. This seems to happen when discovering
921                 // function blocks and looks very likely there is a race condition in the
922                 // device. The workaround here compares the just arrived FCP responde
923                 // to the last one. If it is the same as the previously one then we
924                 // just ignore it. The downside of this approach is, we cannot issue
925                 // the same FCP twice.
926                 debugWarning("Received duplicate FCP response. Ignore it\n");
927             } else {
928                 m_fcp_block.response_length = (length + sizeof(quadlet_t) - 1) / sizeof(quadlet_t);
929                 memcpy(m_fcp_block.response, data, length);
930                 if (m_filterFCPResponse) {
931                     memcpy(fcp_block_last.response, data, length);
932                 }
933                 m_fcp_block.status = eFS_Responded;
934             }
935        }
936     }
937     return 0;
938 }
939
940 bool
941 Ieee1394Service::setSplitTimeoutUsecs(fb_nodeid_t nodeId, unsigned int timeout)
942 {
943     Util::MutexLockHelper lock(*m_handle_lock);
944     debugOutput(DEBUG_LEVEL_VERBOSE, "setting SPLIT_TIMEOUT on node 0x%X to %uusecs...\n", nodeId, timeout);
945     unsigned int secs = timeout / 1000000;
946     unsigned int usecs = timeout % 1000000;
947
948     quadlet_t split_timeout_hi = CondSwapToBus32(secs & 7);
949     quadlet_t split_timeout_low = CondSwapToBus32(((usecs / 125) & 0x1FFF) << 19);
950
951     // write the CSR registers
952     if(!writeNoLock( 0xffc0 | nodeId, CSR_REGISTER_BASE + CSR_SPLIT_TIMEOUT_HI, 1,
953                   &split_timeout_hi)) {
954         debugOutput(DEBUG_LEVEL_VERBOSE, "write of CSR_SPLIT_TIMEOUT_HI failed\n");
955         return false;
956     }
957     if(!writeNoLock( 0xffc0 | nodeId, CSR_REGISTER_BASE + CSR_SPLIT_TIMEOUT_LO, 1,
958                   &split_timeout_low)) {
959         debugOutput(DEBUG_LEVEL_VERBOSE, "write of CSR_SPLIT_TIMEOUT_LO failed\n");
960         return false;
961     }
962     return true;
963 }
964
965 int
966 Ieee1394Service::getSplitTimeoutUsecs(fb_nodeid_t nodeId)
967 {
968     Util::MutexLockHelper lock(*m_handle_lock);
969
970     // Keep Valgrind quiet by including explicit assignment
971     quadlet_t split_timeout_hi = 0;
972     quadlet_t split_timeout_low = 0;
973
974     debugOutput(DEBUG_LEVEL_VERBOSE, "reading SPLIT_TIMEOUT on node 0x%X...\n", nodeId);
975
976     if(!readNoLock( 0xffc0 | nodeId, CSR_REGISTER_BASE + CSR_SPLIT_TIMEOUT_HI, 1,
977                   &split_timeout_hi)) {
978         debugOutput(DEBUG_LEVEL_VERBOSE, "read of CSR_SPLIT_TIMEOUT_HI failed\n");
979         return 0;
980     }
981     debugOutput(DEBUG_LEVEL_VERBOSE, " READ HI: 0x%08X\n", split_timeout_hi);
982
983     if(!readNoLock( 0xffc0 | nodeId, CSR_REGISTER_BASE + CSR_SPLIT_TIMEOUT_LO, 1,
984                   &split_timeout_low)) {
985         debugOutput(DEBUG_LEVEL_VERBOSE, "read of CSR_SPLIT_TIMEOUT_LO failed\n");
986         return 0;
987     }
988     debugOutput(DEBUG_LEVEL_VERBOSE, " READ LO: 0x%08X\n", split_timeout_low);
989
990     split_timeout_hi = CondSwapFromBus32(split_timeout_hi);
991     split_timeout_low = CondSwapFromBus32(split_timeout_low);
992
993     return (split_timeout_hi & 7) * 1000000 + (split_timeout_low >> 19) * 125;
994 }
995
996 void
997 Ieee1394Service::setFCPResponseFiltering(bool enable)
998 {
999     m_filterFCPResponse = enable;
1000 }
1001
1002 int
1003 Ieee1394Service::getVerboseLevel()
1004 {
1005     return getDebugLevel();
1006 }
1007
1008 void
1009 Ieee1394Service::printBuffer( unsigned int level, size_t length, fb_quadlet_t* buffer ) const
1010 {
1011
1012     for ( unsigned int i=0; i < length; ++i ) {
1013         if ( ( i % 4 ) == 0 ) {
1014             if ( i > 0 ) {
1015                 debugOutputShort(level,"\n");
1016             }
1017             debugOutputShort(level," %4d: ",i*4);
1018         }
1019         debugOutputShort(level,"%08X ",buffer[i]);
1020     }
1021     debugOutputShort(level,"\n");
1022 }
1023 void
1024 Ieee1394Service::printBufferBytes( unsigned int level, size_t length, byte_t* buffer ) const
1025 {
1026
1027     for ( unsigned int i=0; i < length; ++i ) {
1028         if ( ( i % 16 ) == 0 ) {
1029             if ( i > 0 ) {
1030                 debugOutputShort(level,"\n");
1031             }
1032             debugOutputShort(level," %4d: ",i*16);
1033         }
1034         debugOutputShort(level,"%02X ",buffer[i]);
1035     }
1036     debugOutputShort(level,"\n");
1037 }
1038
1039 int
1040 Ieee1394Service::resetHandlerLowLevel( raw1394handle_t handle,
1041                                        unsigned int generation )
1042 {
1043     raw1394_update_generation ( handle, generation );
1044
1045     Ieee1394Service::HelperThread *thread = reinterpret_cast<Ieee1394Service::HelperThread *>(raw1394_get_userdata( handle ));
1046     if(thread == NULL) {
1047         debugFatal("Bogus 1394 handle private data\n");
1048         return -1;
1049     }
1050
1051     Ieee1394Service& service = thread->get1394Service();
1052     service.resetHandler( generation );
1053
1054     return 0;
1055 }
1056
1057 bool
1058 Ieee1394Service::resetHandler( unsigned int generation )
1059 {
1060     quadlet_t buf=0;
1061
1062     m_handle_lock->Lock();
1063     raw1394_update_generation(m_handle, generation);
1064     m_handle_lock->Unlock();
1065
1066     // do a simple read on ourself in order to update the internal structures
1067     // this avoids failures after a bus reset
1068     read_quadlet( getLocalNodeId() | 0xFFC0,
1069                   CSR_REGISTER_BASE | CSR_CYCLE_TIME,
1070                   &buf );
1071
1072     for ( reset_handler_vec_t::iterator it = m_busResetHandlers.begin();
1073           it != m_busResetHandlers.end();
1074           ++it )
1075     {
1076         Util::Functor* func = *it;
1077         ( *func )();
1078     }
1079
1080     return true;
1081 }
1082
1083 bool Ieee1394Service::registerARMHandler(ARMHandler *h) {
1084     debugOutput(DEBUG_LEVEL_VERBOSE,
1085                 "Registering ARM handler (%p) for 0x%016"PRIX64", length %zu\n",
1086                 h, h->getStart(), h->getLength());
1087
1088     // FIXME: note that this will result in the ARM handlers not running in a realtime context
1089     int err = raw1394_arm_register(m_armHelperNormal->get1394Handle(), h->getStart(),
1090                                    h->getLength(), h->getBuffer(), (octlet_t)h,
1091                                    h->getAccessRights(),
1092                                    h->getNotificationOptions(),
1093                                    h->getClientTransactions());
1094     if (err) {
1095         debugError("Failed to register ARM handler for 0x%016"PRIX64"\n", h->getStart());
1096         debugError(" Error: %s\n", strerror(errno));
1097         return false;
1098     }
1099     m_armHandlers.push_back( h );
1100     return true;
1101 }
1102
1103 bool Ieee1394Service::unregisterARMHandler( ARMHandler *h ) {
1104     debugOutput(DEBUG_LEVEL_VERBOSE, "Unregistering ARM handler (%p) for 0x%016"PRIX64"\n",
1105         h, h->getStart());
1106
1107     for ( arm_handler_vec_t::iterator it = m_armHandlers.begin();
1108           it != m_armHandlers.end();
1109           ++it )
1110     {
1111         if((*it) == h) {
1112             int err = raw1394_arm_unregister(m_armHelperNormal->get1394Handle(), h->getStart());
1113             if (err) {
1114                 debugError("Failed to unregister ARM handler (%p)\n", h);
1115                 debugError(" Error: %s\n", strerror(errno));
1116             } else {
1117                 m_armHandlers.erase(it);
1118                 return true;
1119             }
1120         }
1121     }
1122     debugOutput(DEBUG_LEVEL_VERBOSE, " handler not found!\n");
1123
1124     return false;
1125 }
1126 /**
1127  * @brief Tries to find a free ARM address range
1128  *
1129  * @param start  address to start with
1130  * @param length length of the block needed (bytes)
1131  * @param step   step to use when searching (bytes)
1132  * @return The base address that is free, and 0xFFFFFFFFFFFFFFFF when failed
1133  */
1134 nodeaddr_t Ieee1394Service::findFreeARMBlock( nodeaddr_t start, size_t length, size_t step ) {
1135     debugOutput(DEBUG_LEVEL_VERBOSE,
1136                 "Finding free ARM block of %zd bytes, from 0x%016"PRIX64" in steps of %zd bytes\n",
1137                 length, start, step);
1138
1139     int cnt=0;
1140     const int maxcnt=10;
1141     int err=1;
1142     Util::MutexLockHelper lock(*m_handle_lock);
1143     while(err && cnt++ < maxcnt) {
1144         // try to register
1145         err = raw1394_arm_register(m_handle, start, length, 0, 0, 0, 0, 0);
1146
1147         if (err) {
1148             debugOutput(DEBUG_LEVEL_VERBOSE, " -> cannot use 0x%016"PRIX64"\n", start);
1149             debugError("    Error: %s\n", strerror(errno));
1150             start += step;
1151         } else {
1152             debugOutput(DEBUG_LEVEL_VERBOSE, " -> use 0x%016"PRIX64"\n", start);
1153             err = raw1394_arm_unregister(m_handle, start);
1154             if (err) {
1155                 debugOutput(DEBUG_LEVEL_VERBOSE, " error unregistering test handler\n");
1156                 debugError("    Error: %s\n", strerror(errno));
1157                 return 0xFFFFFFFFFFFFFFFFLLU;
1158             }
1159             return start;
1160         }
1161     }
1162     debugOutput(DEBUG_LEVEL_VERBOSE, " Could not find free block in %d tries\n",cnt);
1163     return 0xFFFFFFFFFFFFFFFFLLU;
1164 }
1165
1166 int
1167 Ieee1394Service::armHandlerLowLevel(raw1394handle_t handle,
1168                                     unsigned long arm_tag,
1169                                     byte_t request_type, unsigned int requested_length,
1170                                     void *data)
1171 {
1172     Ieee1394Service::HelperThread *thread = reinterpret_cast<Ieee1394Service::HelperThread *>(raw1394_get_userdata( handle ));
1173     if(thread == NULL) {
1174         debugFatal("Bogus 1394 handle private data\n");
1175         return -1;
1176     }
1177
1178     Ieee1394Service& service = thread->get1394Service();
1179     if(service.armHandler( arm_tag, request_type, requested_length, data )) {
1180         return 0;
1181     } else {
1182         return -1;
1183     }
1184 }
1185
1186 bool
1187 Ieee1394Service::armHandler(  unsigned long arm_tag,
1188                      byte_t request_type, unsigned int requested_length,
1189                      void *data)
1190 {
1191     for ( arm_handler_vec_t::iterator it = m_armHandlers.begin();
1192           it != m_armHandlers.end();
1193           ++it )
1194     {
1195         if((*it) == (ARMHandler *)arm_tag) {
1196             struct raw1394_arm_request_response *arm_req_resp;
1197             arm_req_resp = (struct raw1394_arm_request_response *) data;
1198             raw1394_arm_request_t arm_req = arm_req_resp->request;
1199             raw1394_arm_response_t arm_resp = arm_req_resp->response;
1200
1201             debugOutput(DEBUG_LEVEL_VERBOSE,"ARM handler for address 0x%016"PRIX64" called\n",
1202                 (*it)->getStart());
1203             debugOutput(DEBUG_LEVEL_VERBOSE," request type   : 0x%02X\n", request_type);
1204             debugOutput(DEBUG_LEVEL_VERBOSE," request length : %04d\n", requested_length);
1205
1206             switch(request_type) {
1207                 case RAW1394_ARM_READ:
1208                     (*it)->handleRead(arm_req);
1209                     *arm_resp = *((*it)->getResponse());
1210                     break;
1211                 case RAW1394_ARM_WRITE:
1212                     (*it)->handleWrite(arm_req);
1213                     *arm_resp = *((*it)->getResponse());
1214                     break;
1215                 case RAW1394_ARM_LOCK:
1216                     (*it)->handleLock(arm_req);
1217                     *arm_resp = *((*it)->getResponse());
1218                     break;
1219                 default:
1220                     debugWarning("Unknown request type received, ignoring...\n");
1221             }
1222             return true;
1223         }
1224     }
1225
1226     debugOutput(DEBUG_LEVEL_VERBOSE,"default ARM handler called\n");
1227
1228     m_default_arm_handler(m_armHelperNormal->get1394Handle(), arm_tag, request_type, requested_length, data );
1229     return true;
1230 }
1231
1232 bool
1233 Ieee1394Service::addBusResetHandler( Util::Functor* functor )
1234 {
1235     debugOutput(DEBUG_LEVEL_VERBOSE, "Adding busreset handler (%p)\n", functor);
1236     m_busResetHandlers.push_back( functor );
1237     return true;
1238 }
1239
1240 bool
1241 Ieee1394Service::remBusResetHandler( Util::Functor* functor )
1242 {
1243     debugOutput(DEBUG_LEVEL_VERBOSE, "Removing busreset handler (%p)\n", functor);
1244
1245     for ( reset_handler_vec_t::iterator it = m_busResetHandlers.begin();
1246           it != m_busResetHandlers.end();
1247           ++it )
1248     {
1249         if ( *it == functor ) {
1250             debugOutput(DEBUG_LEVEL_VERBOSE, " found\n");
1251             m_busResetHandlers.erase( it );
1252             return true;
1253         }
1254     }
1255     debugOutput(DEBUG_LEVEL_VERBOSE, " not found\n");
1256     return false;
1257 }
1258
1259 /**
1260  * Allocates an iso channel for use by the interface in a similar way to
1261  * libiec61883.  Returns -1 on error (due to there being no free channels)
1262  * or an allocated channel number.
1263  *
1264  * Does not perform anything other than registering the channel and the
1265  * bandwidth at the IRM
1266  *
1267  * Also allocates the necessary bandwidth (in ISO allocation units).
1268  *
1269  * FIXME: As in libiec61883, channel 63 is not requested; this is either a
1270  * bug or it's omitted since that's the channel preferred by video devices.
1271  *
1272  * @param bandwidth the bandwidth to allocate for this channel
1273  * @return the channel number
1274  */
1275 signed int Ieee1394Service::allocateIsoChannelGeneric(unsigned int bandwidth) {
1276     debugOutput(DEBUG_LEVEL_VERBOSE, "Allocating ISO channel using generic method...\n" );
1277
1278     Util::MutexLockHelper lock(*m_handle_lock);
1279     struct ChannelInfo cinfo;
1280
1281     int c = -1;
1282     for (c = 0; c < 63; c++) {
1283         if (raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_ALLOC) == 0)
1284             break;
1285     }
1286     if (c < 63) {
1287         if (raw1394_bandwidth_modify(m_handle, bandwidth, RAW1394_MODIFY_ALLOC) < 0) {
1288             debugFatal("Could not allocate bandwidth of %d\n", bandwidth);
1289
1290             raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_FREE);
1291             return -1;
1292         } else {
1293             cinfo.channel=c;
1294             cinfo.bandwidth=bandwidth;
1295             cinfo.alloctype=AllocGeneric;
1296
1297             cinfo.xmit_node=-1;
1298             cinfo.xmit_plug=-1;
1299             cinfo.recv_node=-1;
1300             cinfo.recv_plug=-1;
1301
1302             if (registerIsoChannel(c, cinfo)) {
1303                 return c;
1304             } else {
1305                 raw1394_bandwidth_modify(m_handle, bandwidth, RAW1394_MODIFY_FREE);
1306                 raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_FREE);
1307                 return -1;
1308             }
1309         }
1310     }
1311     return -1;
1312 }
1313
1314 /**
1315  * Allocates a specific fixed iso channel for use by the interface.  Returns
1316  * -1 on error (due to the requested channel not being free) or the fixed iso
1317  * channel number.
1318  *
1319  * Does not perform anything other than registering the channel and the
1320  * bandwidth at the IRM
1321  *
1322  * Also allocates the necessary bandwidth (in ISO allocation units).
1323  *
1324  * FIXME: As in libiec61883, channel 63 is not requested; this is either a
1325  * bug or it's omitted since that's the channel preferred by video devices.
1326  *
1327  * @chan the channel number being requested
1328  * @param bandwidth the bandwidth to allocate for this channel
1329  * @return the channel number
1330  */
1331 signed int Ieee1394Service::allocateFixedIsoChannelGeneric(
1332     unsigned int chan, unsigned int bandwidth
1333     ) {
1334     debugOutput(DEBUG_LEVEL_VERBOSE, "Allocating ISO channel %d using generic method...\n", chan );
1335
1336     Util::MutexLockHelper lock(*m_handle_lock);
1337     struct ChannelInfo cinfo;
1338
1339     if (raw1394_channel_modify (m_handle, chan, RAW1394_MODIFY_ALLOC) == 0) {
1340         if (raw1394_bandwidth_modify(m_handle, bandwidth, RAW1394_MODIFY_ALLOC) < 0) {
1341             debugFatal("Could not allocate bandwidth of %d\n", bandwidth);
1342
1343             raw1394_channel_modify (m_handle, chan, RAW1394_MODIFY_FREE);
1344             return -1;
1345         } else {
1346             cinfo.channel=chan;
1347             cinfo.bandwidth=bandwidth;
1348             cinfo.alloctype=AllocGeneric;
1349
1350             cinfo.xmit_node=-1;
1351             cinfo.xmit_plug=-1;
1352             cinfo.recv_node=-1;
1353             cinfo.recv_plug=-1;
1354
1355             if (registerIsoChannel(chan, cinfo)) {
1356                 return chan;
1357             } else {
1358                 raw1394_bandwidth_modify(m_handle, bandwidth, RAW1394_MODIFY_FREE);
1359                 raw1394_channel_modify (m_handle, chan, RAW1394_MODIFY_FREE);
1360                 return -1;
1361             }
1362         }
1363     }
1364     return -1;
1365 }
1366
1367 /**
1368  * Allocates an iso channel for use by the interface in a similar way to
1369  * libiec61883.  Returns -1 on error (due to there being no free channels)
1370  * or an allocated channel number.
1371  *
1372  * Uses IEC61883 Connection Management Procedure to establish the connection.
1373  *
1374  * Also allocates the necessary bandwidth (in ISO allocation units).
1375  *
1376  * @param xmit_node  node id of the transmitter
1377  * @param xmit_plug  the output plug to use. If -1, find the first online plug, and
1378  * upon return, contains the plug number used.
1379  * @param recv_node  node id of the receiver
1380  * @param recv_plug the input plug to use. If -1, find the first online plug, and
1381  * upon return, contains the plug number used.
1382  *
1383  * @return the channel number
1384  */
1385
1386 signed int Ieee1394Service::allocateIsoChannelCMP(
1387     nodeid_t xmit_node, int xmit_plug,
1388     nodeid_t recv_node, int recv_plug
1389     ) {
1390
1391     if (xmit_node == INVALID_NODE_ID) {
1392         debugWarning("operation on invalid node (XMIT)\n");
1393         return -1;
1394     }
1395     if (recv_node == INVALID_NODE_ID) {
1396         debugWarning("operation on invalid node (RECV)\n");
1397         return -1;
1398     }
1399
1400     debugOutput(DEBUG_LEVEL_VERBOSE, "Allocating ISO channel using IEC61883 CMP...\n" );
1401     Util::MutexLockHelper lock(*m_handle_lock);
1402
1403     struct ChannelInfo cinfo;
1404
1405     int c = -1;
1406     int bandwidth=1;
1407     #if IEEE1394SERVICE_SKIP_IEC61883_BANDWIDTH_ALLOCATION
1408     bandwidth=0;
1409     #endif
1410
1411     // do connection management: make connection
1412     c = iec61883_cmp_connect(
1413         m_handle,
1414         xmit_node | 0xffc0,
1415         &xmit_plug,
1416         recv_node | 0xffc0,
1417         &recv_plug,
1418         &bandwidth);
1419
1420     if((c<0) || (c>63)) {
1421         debugError("Could not do CMP from %04X:%02d to %04X:%02d\n",
1422             xmit_node, xmit_plug, recv_node, recv_plug
1423             );
1424         return -1;
1425     }
1426
1427     cinfo.channel=c;
1428     cinfo.bandwidth=bandwidth;
1429     cinfo.alloctype=AllocCMP;
1430
1431     cinfo.xmit_node=xmit_node;
1432     cinfo.xmit_plug=xmit_plug;
1433     cinfo.recv_node=recv_node;
1434     cinfo.recv_plug=recv_plug;
1435
1436     if (registerIsoChannel(c, cinfo)) {
1437         return c;
1438     }
1439
1440     return -1;
1441 }
1442
1443 /**
1444  * Deallocates an iso channel.  Silently ignores a request to deallocate
1445  * a negative channel number.
1446  *
1447  * Figures out the method that was used to allocate the channel (generic, cmp, ...)
1448  * and uses the appropriate method to deallocate. Also frees the bandwidth
1449  * that was reserved along with this channel.
1450  *
1451  * @param c channel number
1452  * @return true if successful
1453  */
1454 bool Ieee1394Service::freeIsoChannel(signed int c) {
1455     debugOutput(DEBUG_LEVEL_VERBOSE, "Freeing ISO channel %d...\n", c );
1456     Util::MutexLockHelper lock(*m_handle_lock);
1457
1458     if (c < 0 || c > 63) {
1459         debugWarning("Invalid channel number: %d\n", c);
1460         return false;
1461     }
1462
1463     switch (m_channels[c].alloctype) {
1464         default:
1465             debugError(" BUG: invalid allocation type!\n");
1466             return false;
1467
1468         case AllocFree:
1469             debugWarning(" Channel %d not registered\n", c);
1470             return false;
1471
1472         case AllocGeneric:
1473             debugOutput(DEBUG_LEVEL_VERBOSE, " allocated using generic routine...\n" );
1474             debugOutput(DEBUG_LEVEL_VERBOSE, " freeing %d bandwidth units...\n", m_channels[c].bandwidth );
1475             if (raw1394_bandwidth_modify(m_handle, m_channels[c].bandwidth, RAW1394_MODIFY_FREE) !=0) {
1476                 debugWarning("Failed to deallocate bandwidth\n");
1477             }
1478             debugOutput(DEBUG_LEVEL_VERBOSE, " freeing channel %d...\n", m_channels[c].channel );
1479             if (raw1394_channel_modify (m_handle, m_channels[c].channel, RAW1394_MODIFY_FREE) != 0) {
1480                 debugWarning("Failed to free channel\n");
1481             }
1482             if (!unregisterIsoChannel(c))
1483                 return false;
1484             return true;
1485
1486         case AllocCMP:
1487             debugOutput(DEBUG_LEVEL_VERBOSE, " allocated using IEC61883 CMP...\n" );
1488             debugOutput(DEBUG_LEVEL_VERBOSE, " performing IEC61883 CMP disconnect...\n" );
1489             if(iec61883_cmp_disconnect(
1490                     m_handle,
1491                     m_channels[c].xmit_node | 0xffc0,
1492                     m_channels[c].xmit_plug,
1493                     m_channels[c].recv_node | 0xffc0,
1494                     m_channels[c].recv_plug,
1495                     m_channels[c].channel,
1496                     m_channels[c].bandwidth) != 0) {
1497                 debugWarning("Could not do CMP disconnect for channel %d!\n",c);
1498             }
1499             if (!unregisterIsoChannel(c))
1500                 return false;
1501             return true;
1502     }
1503
1504     // unreachable
1505     debugError("BUG: unreachable code reached!\n");
1506
1507     return false;
1508 }
1509
1510 /**
1511  * Registers a channel as managed by this ieee1394service
1512  * @param c channel number
1513  * @param cinfo channel info struct
1514  * @return true if successful
1515  */
1516 bool Ieee1394Service::registerIsoChannel(unsigned int c, struct ChannelInfo cinfo) {
1517     if (c < 63) {
1518         if (m_channels[c].alloctype != AllocFree) {
1519             debugWarning("Channel %d already registered with bandwidth %d\n",
1520                 m_channels[c].channel, m_channels[c].bandwidth);
1521         }
1522
1523         memcpy(&m_channels[c], &cinfo, sizeof(struct ChannelInfo));
1524
1525     } else return false;
1526     return true;
1527 }
1528
1529 /**
1530  * unegisters a channel from this ieee1394service
1531  * @param c channel number
1532  * @return true if successful
1533  */
1534 bool Ieee1394Service::unregisterIsoChannel(unsigned int c) {
1535     if (c < 63) {
1536         if (m_channels[c].alloctype == AllocFree) {
1537             debugWarning("Channel %d not registered\n", c);
1538             return false;
1539         }
1540
1541         m_channels[c].channel=-1;
1542         m_channels[c].bandwidth=-1;
1543         m_channels[c].alloctype=AllocFree;
1544         m_channels[c].xmit_node=0xFFFF;
1545         m_channels[c].xmit_plug=-1;
1546         m_channels[c].recv_node=0xFFFF;
1547         m_channels[c].recv_plug=-1;
1548
1549     } else return false;
1550     return true;
1551 }
1552
1553 /**
1554  * Returns the current value of the `bandwidth available' register on
1555  * the IRM, or -1 on error.
1556  * @return
1557  */
1558 signed int Ieee1394Service::getAvailableBandwidth() {
1559     quadlet_t buffer;
1560     Util::MutexLockHelper lock(*m_handle_lock);
1561     signed int result = raw1394_read (m_handle, raw1394_get_irm_id (m_handle),
1562         CSR_REGISTER_BASE + CSR_BANDWIDTH_AVAILABLE,
1563         sizeof (quadlet_t), &buffer);
1564
1565     if (result < 0)
1566         return -1;
1567     return CondSwapFromBus32(buffer);
1568 }
1569
1570 void
1571 Ieee1394Service::setVerboseLevel(int l)
1572 {
1573     if (m_pIsoManager) m_pIsoManager->setVerboseLevel(l);
1574     if (m_pCTRHelper) m_pCTRHelper->setVerboseLevel(l);
1575     if (m_pWatchdog) m_pWatchdog->setVerboseLevel(l);
1576     setDebugLevel(l);
1577     debugOutput( DEBUG_LEVEL_VERBOSE, "Setting verbose level to %d...\n", l );
1578 }
1579
1580 void
1581 Ieee1394Service::show()
1582 {
1583     #ifdef DEBUG
1584     uint32_t cycle_timer;
1585     uint64_t local_time;
1586     if(!readCycleTimerReg(&cycle_timer, &local_time)) {
1587         debugWarning("Could not read cycle timer register\n");
1588    
1589     }
1590     uint64_t ctr = CYCLE_TIMER_TO_TICKS( cycle_timer );
1591
1592     debugOutput( DEBUG_LEVEL_VERBOSE, "Port:  %d\n", getPort() );
1593     debugOutput( DEBUG_LEVEL_VERBOSE, " Name: %s\n", getPortName().c_str() );
1594     debugOutput( DEBUG_LEVEL_VERBOSE, " CycleTimerHelper: %p, IsoManager: %p, WatchDog: %p\n",
1595                  m_pCTRHelper, m_pIsoManager, m_pWatchdog );
1596     debugOutput( DEBUG_LEVEL_VERBOSE, " Time: %011"PRIu64" (%03us %04ucy %04uticks)\n",
1597                 ctr,
1598                 (unsigned int)TICKS_TO_SECS( ctr ),
1599                 (unsigned int)TICKS_TO_CYCLES( ctr ),
1600                 (unsigned int)TICKS_TO_OFFSET( ctr ) );
1601     debugOutputShort( DEBUG_LEVEL_NORMAL, "Iso handler info:\n");
1602     #endif
1603     if (m_pIsoManager) m_pIsoManager->dumpInfo();
1604 }
1605
1606 // the helper thread class
1607 Ieee1394Service::HelperThread::HelperThread(Ieee1394Service &parent, std::string name)
1608 : m_parent( parent )
1609 , m_name( name )
1610 , m_handle( NULL )
1611 , m_thread( *(new Util::PosixThread(this, name, false, 0, PTHREAD_CANCEL_DEFERRED)) )
1612 , m_iterate( false )
1613 , m_debugModule(parent.m_debugModule)
1614 {
1615     m_handle = raw1394_new_handle_on_port( parent.m_port );
1616     if(!m_handle) {
1617         debugError("Could not allocate handle\n");
1618         // FIXME: better error handling required
1619     }
1620     raw1394_set_userdata( m_handle, this );
1621 }
1622
1623 Ieee1394Service::HelperThread::HelperThread(Ieee1394Service &parent, std::string name, bool rt, int prio)
1624 : m_parent( parent )
1625 , m_name( name )
1626 , m_handle( NULL )
1627 , m_thread( *(new Util::PosixThread(this, name, rt, prio, PTHREAD_CANCEL_DEFERRED)) )
1628 , m_iterate( false )
1629 , m_debugModule(parent.m_debugModule)
1630 {
1631     m_handle = raw1394_new_handle_on_port( parent.m_port );
1632     if(!m_handle) {
1633         debugError("Could not allocate handle\n");
1634         // FIXME: better error handling required
1635     }
1636     raw1394_set_userdata( m_handle, this );
1637 }
1638
1639 Ieee1394Service::HelperThread::~HelperThread()
1640 {
1641     m_thread.Stop();
1642     delete &m_thread;
1643     if(m_handle) {
1644         raw1394_destroy_handle(m_handle);
1645     }
1646 }
1647
1648 bool
1649 Ieee1394Service::HelperThread::Init()
1650 {
1651     m_iterate = true;
1652     return true;
1653 }
1654
1655 bool
1656 Ieee1394Service::HelperThread::Execute()
1657 {
1658     if(m_iterate) {
1659         int err;
1660         err = raw1394_loop_iterate (m_handle);
1661         if(err < 0) {
1662             debugError("Failed to iterate handler\n");
1663             return false;
1664         } else {
1665             return true;
1666         }
1667     } else {
1668         Util::SystemTimeSource::SleepUsecRelative(1000);
1669         return true;
1670     }
1671 }
1672
1673 void
1674 Ieee1394Service::HelperThread::setThreadParameters(bool rt, int priority)
1675 {
1676     debugOutput( DEBUG_LEVEL_VERBOSE, "(%p) switch to: (rt=%d, prio=%d)...\n", this, rt, priority);
1677     if (priority > THREAD_MAX_RTPRIO) priority = THREAD_MAX_RTPRIO; // cap the priority
1678     if (rt) {
1679         m_thread.AcquireRealTime(priority);
1680     } else {
1681         m_thread.DropRealTime();
1682     }
1683 }
1684
1685 bool
1686 Ieee1394Service::HelperThread::Start()
1687 {
1688     return m_thread.Start() == 0;
1689 }
1690
1691 bool
1692 Ieee1394Service::HelperThread::Stop()
1693 {
1694     // request to stop iterating
1695     m_iterate = false;
1696     // poke the handler such that the iterate() returns
1697     raw1394_wake_up(m_handle);
1698     // stop the thread
1699     return m_thread.Stop() == 0;
1700 }
Note: See TracBrowser for help on using the browser.