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

Revision 2192, 57.6 kB (checked in by jwoithe, 12 years ago)

Demote SPLIT_TIMEOUT failure warnings to verbose debug messages. As summarised by Stefan Richter: The SPLIT_TIMEOUT manipulation is obsolete since kernel 2.6.39 inclusive. It does not hurt to attempt it, but the user should not be alerted of an alleged error if it fails.

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