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

Revision 1763, 55.3 kB (checked in by ppalmers, 12 years ago)

Merged revisions 1536,1541,1544-1546,1549,1554-1562,1571,1579-1581,1618,1632,1634-1635,1661,1677-1679,1703-1704,1715,1720-1723,1743-1745,1755 via svnmerge from
svn+ssh://ffadosvn@ffado.org/ffado/branches/libffado-2.0

Also fix remaining format string warnings.

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