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

Revision 2176, 57.7 kB (checked in by jwoithe, 8 years ago)

When summarising the capability of the JACK package found on the system, spell out 'Jack Audio Connection Kit' in full in an attempt to avoid confusion with the CD ripping program 'jack' amongst new users.

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