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

Revision 2164, 55.4 kB (checked in by jwoithe, 12 years ago)

A more robust solution to ticket #354. Rather than relying on an ad hoc delay, use a condition variable to ensure that PosixThread::ThreadHandler?() acquires m_lock before anything else. It is particularly important that PosixThread::Kill() and PosixThread::Stop() don't lock m_lock before PosixThread::ThreadHandler?(); otherwise a deadlock will result.

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