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

Revision 2167, 57.2 kB (checked in by jwoithe, 12 years ago)

Possibly address ticket #242. Libraw1394 2.1.0 will introduce a new raw1394_read_cycle_timer_and_clock() which permits a choice of the clock to use. If this function is available, use it in preference to raw1394_read_cycle_timer(). By using CLOCK_MONOTONIC_RAW we should avoid timing glitches induced by NTP.
This patch exploits weak linkage to permit ffado to compile and run against all versions libraw1394 regardless of which is present on the runtime system. Correct behaviour with pre-2.1.0 libraw1394 has been verified. Testing with libraw1394 2.1.0 (when it's released) will be needed to confirm that the logic of this patch works as expected.

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