root/branches/libffado-2.0/src/libieee1394/ieee1394service.cpp

Revision 1385, 42.6 kB (checked in by ppalmers, 12 years ago)

Implement a mechanism to disable the samplerate and clock source controls while the device is streaming in order to avoid changes that could mess up jack. The saffire pro controls that cause a device reset to
happen are also disabled while streaming is active.

  • 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 #include "ieee1394service.h"
27 #include "cycletimer.h"
28 #include "IsoHandlerManager.h"
29 #include "CycleTimerHelper.h"
30
31 #include <libraw1394/csr.h>
32 #include <libiec61883/iec61883.h>
33
34 #include "libutil/SystemTimeSource.h"
35 #include "libutil/Watchdog.h"
36 #include "libutil/PosixMutex.h"
37 #include "libutil/Configuration.h"
38
39 #include <errno.h>
40 #include "libutil/ByteSwap.h"
41
42 #include <string.h>
43
44 #include <iostream>
45 #include <iomanip>
46
47 using namespace std;
48
49 IMPL_DEBUG_MODULE( Ieee1394Service, Ieee1394Service, DEBUG_LEVEL_NORMAL );
50
51 Ieee1394Service::Ieee1394Service()
52     : m_configuration( NULL )
53     , m_handle( 0 )
54     , m_handle_lock( new Util::PosixMutex("SRCVHND") )
55     , m_resetHandle( 0 )
56     , m_util_handle( 0 )
57     , m_port( -1 )
58     , m_RHThread_lock( new Util::PosixMutex("SRVCRH") )
59     , m_threadRunning( false )
60     , m_realtime ( false )
61     , m_base_priority ( 0 )
62     , m_pIsoManager( new IsoHandlerManager( *this ) )
63     , m_pCTRHelper ( new CycleTimerHelper( *this, IEEE1394SERVICE_CYCLETIMER_DLL_UPDATE_INTERVAL_USEC ) )
64     , m_have_new_ctr_read ( false )
65     , m_pWatchdog ( new Util::Watchdog() )
66 {
67     for (unsigned int i=0; i<64; i++) {
68         m_channels[i].channel=-1;
69         m_channels[i].bandwidth=-1;
70         m_channels[i].alloctype=AllocFree;
71         m_channels[i].xmit_node=0xFFFF;
72         m_channels[i].xmit_plug=-1;
73         m_channels[i].recv_node=0xFFFF;
74         m_channels[i].recv_plug=-1;
75     }
76 }
77
78 Ieee1394Service::Ieee1394Service(bool rt, int prio)
79     : m_configuration( NULL )
80     , m_handle( 0 )
81     , m_handle_lock( new Util::PosixMutex("SRCVHND") )
82     , m_resetHandle( 0 )
83     , m_util_handle( 0 )
84     , m_port( -1 )
85     , m_RHThread_lock( new Util::PosixMutex("SRVCRH") )
86     , m_threadRunning( false )
87     , m_realtime ( rt )
88     , m_base_priority ( prio )
89     , m_pIsoManager( new IsoHandlerManager( *this, rt, prio ) )
90     , m_pCTRHelper ( new CycleTimerHelper( *this, IEEE1394SERVICE_CYCLETIMER_DLL_UPDATE_INTERVAL_USEC,
91                                            rt && IEEE1394SERVICE_CYCLETIMER_HELPER_RUN_REALTIME,
92                                            prio + IEEE1394SERVICE_CYCLETIMER_HELPER_PRIO_INCREASE ) )
93     , m_have_new_ctr_read ( false )
94     , m_pWatchdog ( new Util::Watchdog() )
95 {
96     for (unsigned int i=0; i<64; i++) {
97         m_channels[i].channel=-1;
98         m_channels[i].bandwidth=-1;
99         m_channels[i].alloctype=AllocFree;
100         m_channels[i].xmit_node=0xFFFF;
101         m_channels[i].xmit_plug=-1;
102         m_channels[i].recv_node=0xFFFF;
103         m_channels[i].recv_plug=-1;
104     }
105 }
106
107 Ieee1394Service::~Ieee1394Service()
108 {
109     delete m_pIsoManager;
110     delete m_pCTRHelper;
111     stopRHThread();
112
113     delete m_pWatchdog;
114     if ( m_handle ) {
115         raw1394_destroy_handle( m_handle );
116     }
117     delete m_handle_lock;
118
119     if ( m_resetHandle ) {
120         raw1394_destroy_handle( m_resetHandle );
121     }
122     delete m_RHThread_lock;
123     if ( m_util_handle ) {
124         raw1394_destroy_handle( m_util_handle );
125     }
126 }
127
128 bool
129 Ieee1394Service::useConfiguration(Util::Configuration *c)
130 {
131     m_configuration = c;
132     return configurationUpdated();
133 }
134
135 bool
136 Ieee1394Service::configurationUpdated()
137 {
138     if(m_configuration) {
139        
140     }
141     return true;
142 }
143
144 int
145 Ieee1394Service::detectNbPorts()
146 {
147     raw1394handle_t tmp_handle = raw1394_new_handle();
148     if ( tmp_handle == NULL ) {
149         debugError("Could not get libraw1394 handle.\n");
150         return -1;
151     }
152     struct raw1394_portinfo pinf[IEEE1394SERVICE_MAX_FIREWIRE_PORTS];
153     int nb_detected_ports = raw1394_get_port_info(tmp_handle, pinf, IEEE1394SERVICE_MAX_FIREWIRE_PORTS);
154     raw1394_destroy_handle(tmp_handle);
155
156     if (nb_detected_ports < 0) {
157         debugError("Failed to detect number of ports\n");
158         return -1;
159     }
160     return nb_detected_ports;
161 }
162
163 void
164 Ieee1394Service::doBusReset() {
165     debugOutput(DEBUG_LEVEL_VERBOSE, "Issue bus reset on service %p (port %d).\n", this, getPort());
166     raw1394_reset_bus(m_handle);
167 }
168
169 /**
170  * This function waits until there are no bus resets generated in a sleep_time_ms interval
171  * @param nb_tries number of tries to take
172  * @param sleep_time_ms sleep between tries
173  * @return true if the storm passed
174  */
175 bool
176 Ieee1394Service::waitForBusResetStormToEnd( int nb_tries, int sleep_time_ms ) {
177     unsigned int gen_current;
178     do {
179         gen_current = getGeneration();
180         debugOutput(DEBUG_LEVEL_VERBOSE, "Waiting... (gen: %u)\n", gen_current);
181
182         // wait for a while
183         Util::SystemTimeSource::SleepUsecRelative( sleep_time_ms * 1000);
184     } while (gen_current != getGeneration() && --nb_tries);
185
186     if (!nb_tries) {
187         debugError( "Bus reset storm did not stop on time...\n");
188         return false;
189     }
190     return true;
191 }
192
193 bool
194 Ieee1394Service::initialize( int port )
195 {
196     using namespace std;
197
198     int nb_ports = detectNbPorts();
199     if (port + 1 > nb_ports) {
200         debugFatal("Requested port (%d) out of range (# ports: %d)\n", port, nb_ports);
201     }
202
203     if(!m_pWatchdog) {
204         debugError("No valid RT watchdog found.\n");
205         return false;
206     }
207     if(!m_pWatchdog->start()) {
208         debugError("Could not start RT watchdog.\n");
209         return false;
210     }
211
212     m_handle = raw1394_new_handle_on_port( port );
213     if ( !m_handle ) {
214         if ( !errno ) {
215             debugFatal("libraw1394 not compatible\n");
216         } else {
217             debugFatal("Ieee1394Service::initialize: Could not get 1394 handle: %s\n",
218                 strerror(errno) );
219             debugFatal("Is ieee1394 and raw1394 driver loaded?\n");
220         }
221         return false;
222     }
223
224     m_resetHandle = raw1394_new_handle_on_port( port );
225     if ( !m_resetHandle ) {
226         if ( !errno ) {
227             debugFatal("libraw1394 not compatible\n");
228         } else {
229             debugFatal("Ieee1394Service::initialize: Could not get 1394 handle: %s",
230                 strerror(errno) );
231             debugFatal("Is ieee1394 and raw1394 driver loaded?\n");
232         }
233         return false;
234     }
235
236     m_util_handle = raw1394_new_handle_on_port( port );
237     if ( !m_util_handle ) {
238         if ( !errno ) {
239             debugFatal("libraw1394 not compatible\n");
240         } else {
241             debugFatal("Ieee1394Service::initialize: Could not get 1394 handle: %s",
242                 strerror(errno) );
243             debugFatal("Is ieee1394 and raw1394 driver loaded?\n");
244         }
245         return false;
246     }
247
248     // test the cycle timer read function
249     int err;
250     uint32_t cycle_timer;
251     uint64_t local_time;
252     err=raw1394_read_cycle_timer(m_handle, &cycle_timer, &local_time);
253     if(err) {
254         debugOutput(DEBUG_LEVEL_VERBOSE, "raw1394_read_cycle_timer failed.\n");
255         debugOutput(DEBUG_LEVEL_VERBOSE, " Error descr: %s\n", strerror(err));
256         debugWarning("==================================================================\n");
257         debugWarning(" This system doesn't support the raw1394_read_cycle_timer call.   \n");
258         debugWarning(" Fallback to indirect CTR read method.                            \n");
259         debugWarning(" FFADO should work, but achieving low-latency might be a problem. \n");
260         debugWarning(" Upgrade the kernel to version 2.6.21 or higher to solve this.    \n");
261         debugWarning("==================================================================\n");
262         m_have_new_ctr_read = false;
263     } else {
264         debugOutput(DEBUG_LEVEL_VERBOSE, "This system supports the raw1394_read_cycle_timer call, using it.\n");
265         m_have_new_ctr_read = true;
266     }
267
268     m_port = port;
269
270     // obtain port name
271     raw1394handle_t tmp_handle = raw1394_new_handle();
272     if ( tmp_handle == NULL ) {
273         debugError("Could not get temporary libraw1394 handle.\n");
274         return false;
275     }
276     struct raw1394_portinfo pinf[IEEE1394SERVICE_MAX_FIREWIRE_PORTS];
277     int nb_detected_ports = raw1394_get_port_info(tmp_handle, pinf, IEEE1394SERVICE_MAX_FIREWIRE_PORTS);
278     raw1394_destroy_handle(tmp_handle);
279
280     if (nb_detected_ports < 0) {
281         debugError("Failed to detect number of ports\n");
282         return false;
283     }
284
285     if(nb_detected_ports && port < IEEE1394SERVICE_MAX_FIREWIRE_PORTS) {
286         m_portName = pinf[port].name;
287     } else {
288         m_portName = "Unknown";
289     }
290     if (m_portName == "") {
291         m_portName = "Unknown";
292     }
293
294     // set userdata
295     raw1394_set_userdata( m_handle, this );
296     raw1394_set_userdata( m_resetHandle, this );
297     raw1394_set_userdata( m_util_handle, this );
298     raw1394_set_bus_reset_handler( m_resetHandle,
299                                    this->resetHandlerLowLevel );
300
301     int split_timeout = IEEE1394SERVICE_MIN_SPLIT_TIMEOUT_USECS;
302     if(m_configuration) {
303         m_configuration->getValueForSetting("ieee1394.min_split_timeout_usecs", split_timeout);
304     }
305
306     // set SPLIT_TIMEOUT to one second to cope with DM1x00 devices that
307     // send responses regardless of the timeout
308     int timeout = getSplitTimeoutUsecs(getLocalNodeId());
309     debugOutput(DEBUG_LEVEL_VERBOSE, "Minimum SPLIT_TIMEOUT: %d. Current: %d\n", split_timeout, timeout);
310     if (timeout < split_timeout) {
311         if(!setSplitTimeoutUsecs(getLocalNodeId(), split_timeout+124)) {
312             debugWarning("Could not set SPLIT_TIMEOUT to min requested (%d)\n", split_timeout);
313         }
314         timeout = getSplitTimeoutUsecs(getLocalNodeId());
315         if (timeout < split_timeout) {
316             debugWarning("Set SPLIT_TIMEOUT to min requested (%d) did not succeed\n", split_timeout);
317         }
318     }
319
320     // init helpers
321     if(!m_pCTRHelper) {
322         debugFatal("No CycleTimerHelper available, bad!\n");
323         return false;
324     }
325     m_pCTRHelper->setVerboseLevel(getDebugLevel());
326     if(!m_pCTRHelper->Start()) {
327         debugFatal("Could not start CycleTimerHelper\n");
328         return false;
329     }
330
331     if(!m_pIsoManager) {
332         debugFatal("No IsoHandlerManager available, bad!\n");
333         return false;
334     }
335     m_pIsoManager->setVerboseLevel(getDebugLevel());
336
337     if(!m_pIsoManager->init()) {
338         debugFatal("Could not initialize IsoHandlerManager\n");
339         return false;
340     }
341
342     startRHThread();
343
344     // make sure that the thread parameters of all our helper threads are OK
345     if(!setThreadParameters(m_realtime, m_base_priority)) {
346         debugFatal("Could not set thread parameters\n");
347         return false;
348     }
349     return true;
350 }
351
352 bool
353 Ieee1394Service::setThreadParameters(bool rt, int priority) {
354     bool result = true;
355     if (priority > THREAD_MAX_RTPRIO) priority = THREAD_MAX_RTPRIO;
356     m_base_priority = priority;
357     m_realtime = rt;
358     if (m_pIsoManager) {
359         debugOutput(DEBUG_LEVEL_VERBOSE, "Switching IsoManager to (rt=%d, prio=%d)\n",
360                                          rt, priority);
361         result &= m_pIsoManager->setThreadParameters(rt, priority);
362     }
363     if (m_pCTRHelper) {
364         debugOutput(DEBUG_LEVEL_VERBOSE, "Switching CycleTimerHelper to (rt=%d, prio=%d)\n",
365                                          rt && IEEE1394SERVICE_CYCLETIMER_HELPER_RUN_REALTIME,
366                                          priority + IEEE1394SERVICE_CYCLETIMER_HELPER_PRIO_INCREASE);
367         result &= m_pCTRHelper->setThreadParameters(rt && IEEE1394SERVICE_CYCLETIMER_HELPER_RUN_REALTIME,
368                                                     priority + IEEE1394SERVICE_CYCLETIMER_HELPER_PRIO_INCREASE);
369     }
370     return result;
371 }
372
373 int
374 Ieee1394Service::getNodeCount()
375 {
376     Util::MutexLockHelper lock(*m_handle_lock);
377     return raw1394_get_nodecount( m_handle );
378 }
379
380 nodeid_t Ieee1394Service::getLocalNodeId() {
381     Util::MutexLockHelper lock(*m_handle_lock);
382     return raw1394_get_local_id(m_handle) & 0x3F;
383 }
384
385 /**
386  * Returns the current value of the cycle timer (in ticks)
387  *
388  * @return the current value of the cycle timer (in ticks)
389  */
390
391 uint32_t
392 Ieee1394Service::getCycleTimerTicks() {
393     return m_pCTRHelper->getCycleTimerTicks();
394 }
395
396 /**
397  * Returns the current value of the cycle timer (as is)
398  *
399  * @return the current value of the cycle timer (as is)
400  */
401 uint32_t
402 Ieee1394Service::getCycleTimer() {
403     return m_pCTRHelper->getCycleTimer();
404 }
405
406 /**
407  * Returns the current value of the cycle timer (in ticks)
408  * for a specific time instant (usecs since epoch)
409  * @return the current value of the cycle timer (in ticks)
410  */
411
412 uint32_t
413 Ieee1394Service::getCycleTimerTicks(uint64_t t) {
414     return m_pCTRHelper->getCycleTimerTicks(t);
415 }
416
417 /**
418  * Returns the current value of the cycle timer (as is)
419  * for a specific time instant (usecs since epoch)
420  * @return the current value of the cycle timer (as is)
421  */
422 uint32_t
423 Ieee1394Service::getCycleTimer(uint64_t t) {
424     return m_pCTRHelper->getCycleTimer(t);
425 }
426
427 uint64_t
428 Ieee1394Service::getSystemTimeForCycleTimerTicks(uint32_t ticks) {
429     return m_pCTRHelper->getSystemTimeForCycleTimerTicks(ticks);
430 }
431
432 uint64_t
433 Ieee1394Service::getSystemTimeForCycleTimer(uint32_t ctr) {
434     return m_pCTRHelper->getSystemTimeForCycleTimer(ctr);
435 }
436
437 bool
438 Ieee1394Service::readCycleTimerReg(uint32_t *cycle_timer, uint64_t *local_time)
439 {
440     if(m_have_new_ctr_read) {
441         int err;
442         err = raw1394_read_cycle_timer(m_util_handle, cycle_timer, local_time);
443         if(err) {
444             debugWarning("raw1394_read_cycle_timer: %s\n", strerror(err));
445             return false;
446         }
447         return true;
448     } else {
449         // do a normal read of the CTR register
450         // the disadvantage is that local_time and cycle time are not
451         // read at the same time instant (scheduling issues)
452         *local_time = getCurrentTimeAsUsecs();
453         if ( raw1394_read( m_util_handle,
454                 getLocalNodeId() | 0xFFC0,
455                 CSR_REGISTER_BASE | CSR_CYCLE_TIME,
456                 sizeof(uint32_t), cycle_timer ) == 0 ) {
457             *cycle_timer = CondSwapFromBus32(*cycle_timer);
458             return true;
459         } else {
460             return false;
461         }
462     }
463 }
464
465 uint64_t
466 Ieee1394Service::getCurrentTimeAsUsecs() {
467     return Util::SystemTimeSource::getCurrentTimeAsUsecs();
468 }
469
470 bool
471 Ieee1394Service::read( fb_nodeid_t nodeId,
472                        fb_nodeaddr_t addr,
473                        size_t length,
474                        fb_quadlet_t* buffer )
475 {
476     Util::MutexLockHelper lock(*m_handle_lock);
477     return readNoLock(nodeId, addr, length, buffer);
478 }
479
480 bool
481 Ieee1394Service::readNoLock( fb_nodeid_t nodeId,
482                              fb_nodeaddr_t addr,
483                              size_t length,
484                              fb_quadlet_t* buffer )
485 {
486     if (nodeId == INVALID_NODE_ID) {
487         debugWarning("operation on invalid node\n");
488         return false;
489     }
490     if ( raw1394_read( m_handle, nodeId, addr, length*4, buffer ) == 0 ) {
491
492         #ifdef DEBUG
493         debugOutput(DEBUG_LEVEL_VERY_VERBOSE,
494             "read: node 0x%hX, addr = 0x%016llX, length = %u\n",
495             nodeId, addr, length);
496         printBuffer( DEBUG_LEVEL_VERY_VERBOSE, length, buffer );
497         #endif
498
499         return true;
500     } else {
501         #ifdef DEBUG
502         debugOutput(DEBUG_LEVEL_NORMAL,
503                     "raw1394_read failed: node 0x%hX, addr = 0x%016llX, length = %u\n",
504                     nodeId, addr, length);
505         #endif
506         return false;
507     }
508 }
509
510 bool
511 Ieee1394Service::read_quadlet( fb_nodeid_t nodeId,
512                                fb_nodeaddr_t addr,
513                                fb_quadlet_t* buffer )
514 {
515     return read( nodeId,  addr, sizeof( *buffer )/4, buffer );
516 }
517
518 bool
519 Ieee1394Service::read_octlet( fb_nodeid_t nodeId,
520                               fb_nodeaddr_t addr,
521                               fb_octlet_t* buffer )
522 {
523     return read( nodeId, addr, sizeof( *buffer )/4,
524                  reinterpret_cast<fb_quadlet_t*>( buffer ) );
525 }
526
527 bool
528 Ieee1394Service::write( fb_nodeid_t nodeId,
529                         fb_nodeaddr_t addr,
530                         size_t length,
531                         fb_quadlet_t* data )
532 {
533     Util::MutexLockHelper lock(*m_handle_lock);
534     return writeNoLock(nodeId, addr, length, data);
535 }
536
537 bool
538 Ieee1394Service::writeNoLock( fb_nodeid_t nodeId,
539                               fb_nodeaddr_t addr,
540                               size_t length,
541                               fb_quadlet_t* data )
542 {
543     if (nodeId == INVALID_NODE_ID) {
544         debugWarning("operation on invalid node\n");
545         return false;
546     }
547
548     #ifdef DEBUG
549     debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"write: node 0x%hX, addr = 0x%016llX, length = %d\n",
550                 nodeId, addr, length);
551     printBuffer( DEBUG_LEVEL_VERY_VERBOSE, length, data );
552     #endif
553
554     return raw1394_write( m_handle, nodeId, addr, length*4, data ) == 0;
555 }
556
557 bool
558 Ieee1394Service::write_quadlet( fb_nodeid_t nodeId,
559                                 fb_nodeaddr_t addr,
560                                 fb_quadlet_t data )
561 {
562     return write( nodeId, addr, sizeof( data )/4, &data );
563 }
564
565 bool
566 Ieee1394Service::write_octlet( fb_nodeid_t nodeId,
567                                fb_nodeaddr_t addr,
568                                fb_octlet_t data )
569 {
570     return write( nodeId, addr, sizeof( data )/4,
571                   reinterpret_cast<fb_quadlet_t*>( &data ) );
572 }
573
574 bool
575 Ieee1394Service::lockCompareSwap64( fb_nodeid_t nodeId,
576                                     fb_nodeaddr_t addr,
577                                     fb_octlet_t compare_value,
578                                     fb_octlet_t swap_value,
579                                     fb_octlet_t* result )
580 {
581     if (nodeId == INVALID_NODE_ID) {
582         debugWarning("operation on invalid node\n");
583         return false;
584     }
585     #ifdef DEBUG
586     debugOutput(DEBUG_LEVEL_VERBOSE,"lockCompareSwap64: node 0x%X, addr = 0x%016llX\n",
587                 nodeId, addr);
588     debugOutput(DEBUG_LEVEL_VERBOSE,"  if (*(addr)==0x%016llX) *(addr)=0x%016llX\n",
589                 compare_value, swap_value);
590     fb_octlet_t buffer;
591     if(!read_octlet( nodeId, addr,&buffer )) {
592         debugWarning("Could not read register\n");
593     } else {
594         debugOutput(DEBUG_LEVEL_VERBOSE,"before = 0x%016llX\n", buffer);
595     }
596     #endif
597
598     // do endiannes swapping
599     compare_value = CondSwapToBus64(compare_value);
600     swap_value    = CondSwapToBus64(swap_value);
601
602     // do separate locking here (no MutexLockHelper) since
603     // we use read_octlet in the DEBUG code in this function
604     m_handle_lock->Lock();
605     int retval=raw1394_lock64(m_handle, nodeId, addr,
606                               RAW1394_EXTCODE_COMPARE_SWAP,
607                               swap_value, compare_value, result);
608     m_handle_lock->Unlock();
609
610     if(retval) {
611         debugError("raw1394_lock64 failed: %s\n", strerror(errno));
612     }
613
614     #ifdef DEBUG
615     if(!read_octlet( nodeId, addr,&buffer )) {
616         debugWarning("Could not read register\n");
617     } else {
618         debugOutput(DEBUG_LEVEL_VERBOSE,"after = 0x%016llX\n", buffer);
619     }
620     #endif
621
622     *result = CondSwapFromBus64(*result);
623
624     return (retval == 0);
625 }
626
627 fb_quadlet_t*
628 Ieee1394Service::transactionBlock( fb_nodeid_t nodeId,
629                                    fb_quadlet_t* buf,
630                                    int len,
631                                    unsigned int* resp_len )
632 {
633     // FIXME: simplify semantics
634     if (nodeId == INVALID_NODE_ID) {
635         debugWarning("operation on invalid node\n");
636         return false;
637     }
638     // NOTE: this expects a call to transactionBlockClose to unlock
639     m_handle_lock->Lock();
640
641     // clear the request & response memory
642     memset(&m_fcp_block, 0, sizeof(m_fcp_block));
643
644     // make a local copy of the request
645     if(len < MAX_FCP_BLOCK_SIZE_QUADS) {
646         memcpy(m_fcp_block.request, buf, len*sizeof(quadlet_t));
647         m_fcp_block.request_length = len;
648     } else {
649         debugWarning("Truncating FCP request\n");
650         memcpy(m_fcp_block.request, buf, MAX_FCP_BLOCK_SIZE_BYTES);
651         m_fcp_block.request_length = MAX_FCP_BLOCK_SIZE_QUADS;
652     }
653     m_fcp_block.target_nodeid = 0xffc0 | nodeId;
654
655     bool success = doFcpTransaction();
656     if(success) {
657         *resp_len = m_fcp_block.response_length;
658         return m_fcp_block.response;
659     } else {
660         debugWarning("FCP transaction failed\n");
661         *resp_len = 0;
662         return NULL;
663     }
664 }
665
666 bool
667 Ieee1394Service::transactionBlockClose()
668 {
669     m_handle_lock->Unlock();
670     return true;
671 }
672
673 // FCP code
674 bool
675 Ieee1394Service::doFcpTransaction()
676 {
677     for(int i=0; i < IEEE1394SERVICE_FCP_MAX_TRIES; i++) {
678         if(doFcpTransactionTry()) {
679             return true;
680         } else {
681             debugOutput(DEBUG_LEVEL_VERBOSE, "FCP transaction try %d failed\n", i);
682             Util::SystemTimeSource::SleepUsecRelative( IEEE1394SERVICE_FCP_SLEEP_BETWEEN_FAILURES_USECS);
683         }
684     }
685     debugError("FCP transaction didn't succeed in %d tries\n", IEEE1394SERVICE_FCP_MAX_TRIES);
686     return false;
687 }
688
689 #define FCP_COMMAND_ADDR   0xFFFFF0000B00ULL
690 #define FCP_RESPONSE_ADDR  0xFFFFF0000D00ULL
691
692 /* AV/C FCP response codes */
693 #define FCP_RESPONSE_NOT_IMPLEMENTED 0x08000000
694 #define FCP_RESPONSE_ACCEPTED 0x09000000
695 #define FCP_RESPONSE_REJECTED 0x0A000000
696 #define FCP_RESPONSE_IN_TRANSITION 0x0B000000
697 #define FCP_RESPONSE_IMPLEMENTED 0x0C000000
698 #define FCP_RESPONSE_STABLE 0x0C000000
699 #define FCP_RESPONSE_CHANGED 0x0D000000
700 #define FCP_RESPONSE_INTERIM 0x0F000000
701
702 /* AV/C FCP mask macros */
703 #define FCP_MASK_START(x) ((x) & 0xF0000000)
704 #define FCP_MASK_CTYPE(x) ((x) & 0x0F000000)
705 #define FCP_MASK_RESPONSE(x) ((x) & 0x0F000000)
706 #define FCP_MASK_SUBUNIT(x) ((x) & 0x00FF0000)
707 #define FCP_MASK_SUBUNIT_TYPE(x) ((x) & 0x00F80000)
708 #define FCP_MASK_SUBUNIT_ID(x) ((x) & 0x00070000)
709 #define FCP_MASK_OPCODE(x) ((x) & 0x0000FF00)
710 #define FCP_MASK_SUBUNIT_AND_OPCODE(x) ((x) & 0x00FFFF00)
711 #define FCP_MASK_OPERAND0(x) ((x) & 0x000000FF)
712 #define FCP_MASK_OPERAND(x, n) ((x) & (0xFF000000 >> ((((n)-1)%4)*8)))
713 #define FCP_MASK_RESPONSE_OPERAND(x, n) ((x) & (0xFF000000 >> (((n)%4)*8)))
714
715 bool
716 Ieee1394Service::doFcpTransactionTry()
717 {
718     // NOTE that access to this is protected by the m_handle lock
719     int err;
720     bool retval = true;
721     uint64_t timeout;
722
723     // prepare an fcp response handler
724     raw1394_set_fcp_handler(m_handle, _avc_fcp_handler);
725
726     // start listening for FCP requests
727     // this fails if some other program is listening for a FCP response
728     err = raw1394_start_fcp_listen(m_handle);
729     if(err) {
730         debugOutput(DEBUG_LEVEL_VERBOSE, "could not start FCP listen (err=%d, errno=%d)\n", err, errno);
731         retval = false;
732         goto out;
733     }
734
735     m_fcp_block.status = eFS_Waiting;
736
737     #ifdef DEBUG
738     debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"fcp request: node 0x%hX, length = %d bytes\n",
739                 m_fcp_block.target_nodeid, m_fcp_block.request_length*4);
740     printBuffer(DEBUG_LEVEL_VERY_VERBOSE, m_fcp_block.request_length, m_fcp_block.request );
741     #endif
742
743     // write the FCP request
744     if(!writeNoLock( m_fcp_block.target_nodeid, FCP_COMMAND_ADDR,
745                      m_fcp_block.request_length, m_fcp_block.request)) {
746         debugOutput(DEBUG_LEVEL_VERBOSE, "write of FCP request failed\n");
747         retval = false;
748         goto out;
749     }
750
751     // wait for the response to arrive
752     struct pollfd raw1394_poll;
753     raw1394_poll.fd = raw1394_get_fd(m_handle);
754     raw1394_poll.events = POLLIN;
755
756     timeout = Util::SystemTimeSource::getCurrentTimeAsUsecs() +
757               IEEE1394SERVICE_FCP_RESPONSE_TIMEOUT_USEC;
758
759     while(m_fcp_block.status == eFS_Waiting
760           && Util::SystemTimeSource::getCurrentTimeAsUsecs() < timeout) {
761         if(poll( &raw1394_poll, 1, IEEE1394SERVICE_FCP_POLL_TIMEOUT_MSEC) > 0) {
762             if (raw1394_poll.revents & POLLIN) {
763                 raw1394_loop_iterate(m_handle);
764             }
765         }
766     }
767
768     // stop listening for FCP responses
769     err = raw1394_stop_fcp_listen(m_handle);
770     if(err) {
771         debugOutput(DEBUG_LEVEL_VERBOSE, "could not stop FCP listen (err=%d, errno=%d)\n", err, errno);
772         retval = false;
773         goto out;
774     }
775
776     // check the request and figure out what happened
777     if(m_fcp_block.status == eFS_Waiting) {
778         debugOutput(DEBUG_LEVEL_VERBOSE, "FCP response timed out\n");
779         retval = false;
780         goto out;
781     }
782     if(m_fcp_block.status == eFS_Error) {
783         debugError("FCP request/response error\n");
784         retval = false;
785         goto out;
786     }
787
788 out:
789     m_fcp_block.status = eFS_Empty;
790     return retval;
791 }
792
793 int
794 Ieee1394Service::_avc_fcp_handler(raw1394handle_t handle, nodeid_t nodeid,
795                                   int response, size_t length,
796                                   unsigned char *data)
797 {
798     Ieee1394Service *service = static_cast<Ieee1394Service *>(raw1394_get_userdata(handle));
799     if(service) {
800         return service->handleFcpResponse(nodeid, response, length, data);
801     } else return -1;
802 }
803
804 int
805 Ieee1394Service::handleFcpResponse(nodeid_t nodeid,
806                                    int response, size_t length,
807                                    unsigned char *data)
808 {
809     fb_quadlet_t *data_quads = (fb_quadlet_t *)data;
810     #ifdef DEBUG
811     debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"fcp response: node 0x%hX, response = %d, length = %d bytes\n",
812                 nodeid, response, length);
813     printBuffer(DEBUG_LEVEL_VERY_VERBOSE, (length+3)/4, data_quads );
814     #endif
815
816     if (response && length > 3) {
817         if(length > MAX_FCP_BLOCK_SIZE_BYTES) {
818             length = MAX_FCP_BLOCK_SIZE_BYTES;
819             debugWarning("Truncated FCP response\n");
820         }
821
822         // is it an actual response or is it INTERIM?
823         quadlet_t first_quadlet = CondSwapFromBus32(data_quads[0]);
824         if(FCP_MASK_RESPONSE(first_quadlet) == FCP_RESPONSE_INTERIM) {
825             debugOutput(DEBUG_LEVEL_VERBOSE, "INTERIM\n");
826         } else {
827             // it's an actual response, check if it matches our request
828             if(nodeid != m_fcp_block.target_nodeid) {
829                 debugOutput(DEBUG_LEVEL_VERBOSE, "FCP response node id's don't match! (%x, %x)\n",
830                                                  m_fcp_block.target_nodeid, nodeid);
831             } else if (first_quadlet == 0) {
832                 debugWarning("Bogus FCP response\n");
833                 printBuffer(DEBUG_LEVEL_WARNING, (length+3)/4, data_quads );
834 #ifdef DEBUG
835             } else if(FCP_MASK_RESPONSE(first_quadlet) < 0x08000000) {
836                 debugWarning("Bogus AV/C FCP response code\n");
837                 printBuffer(DEBUG_LEVEL_WARNING, (length+3)/4, data_quads );
838 #endif
839             } else if(FCP_MASK_SUBUNIT_AND_OPCODE(first_quadlet)
840                       != FCP_MASK_SUBUNIT_AND_OPCODE(CondSwapFromBus32(m_fcp_block.request[0]))) {
841                 debugOutput(DEBUG_LEVEL_VERBOSE, "FCP response not for this request: %08lX != %08lX\n",
842                              FCP_MASK_SUBUNIT_AND_OPCODE(first_quadlet),
843                              FCP_MASK_SUBUNIT_AND_OPCODE(CondSwapFromBus32(m_fcp_block.request[0])));
844             } else {
845                 m_fcp_block.response_length = (length + sizeof(quadlet_t) - 1) / sizeof(quadlet_t);
846                 memcpy(m_fcp_block.response, data, length);
847                 m_fcp_block.status = eFS_Responded;
848             }
849        }
850     }
851     return 0;
852 }
853
854 bool
855 Ieee1394Service::setSplitTimeoutUsecs(fb_nodeid_t nodeId, unsigned int timeout)
856 {
857     Util::MutexLockHelper lock(*m_handle_lock);
858     debugOutput(DEBUG_LEVEL_VERBOSE, "setting SPLIT_TIMEOUT on node 0x%X to %uusecs...\n", nodeId, timeout);
859     unsigned int secs = timeout / 1000000;
860     unsigned int usecs = timeout % 1000000;
861
862     quadlet_t split_timeout_hi = CondSwapToBus32(secs & 7);
863     quadlet_t split_timeout_low = CondSwapToBus32(((usecs / 125) & 0x1FFF) << 19);
864
865     // write the CSR registers
866     if(!writeNoLock( 0xffc0 | nodeId, CSR_REGISTER_BASE + CSR_SPLIT_TIMEOUT_HI, 1,
867                   &split_timeout_hi)) {
868         debugOutput(DEBUG_LEVEL_VERBOSE, "write of CSR_SPLIT_TIMEOUT_HI failed\n");
869         return false;
870     }
871     if(!writeNoLock( 0xffc0 | nodeId, CSR_REGISTER_BASE + CSR_SPLIT_TIMEOUT_LO, 1,
872                   &split_timeout_low)) {
873         debugOutput(DEBUG_LEVEL_VERBOSE, "write of CSR_SPLIT_TIMEOUT_LO failed\n");
874         return false;
875     }
876     return true;
877 }
878
879 int
880 Ieee1394Service::getSplitTimeoutUsecs(fb_nodeid_t nodeId)
881 {
882     Util::MutexLockHelper lock(*m_handle_lock);
883     quadlet_t split_timeout_hi;
884     quadlet_t split_timeout_low;
885
886     debugOutput(DEBUG_LEVEL_VERBOSE, "reading SPLIT_TIMEOUT on node 0x%X...\n", nodeId);
887
888     if(!readNoLock( 0xffc0 | nodeId, CSR_REGISTER_BASE + CSR_SPLIT_TIMEOUT_HI, 1,
889                   &split_timeout_hi)) {
890         debugOutput(DEBUG_LEVEL_VERBOSE, "read of CSR_SPLIT_TIMEOUT_HI failed\n");
891         return 0;
892     }
893     debugOutput(DEBUG_LEVEL_VERBOSE, " READ HI: 0x%08lX\n", split_timeout_hi);
894
895     if(!readNoLock( 0xffc0 | nodeId, CSR_REGISTER_BASE + CSR_SPLIT_TIMEOUT_LO, 1,
896                   &split_timeout_low)) {
897         debugOutput(DEBUG_LEVEL_VERBOSE, "read of CSR_SPLIT_TIMEOUT_LO failed\n");
898         return 0;
899     }
900     debugOutput(DEBUG_LEVEL_VERBOSE, " READ LO: 0x%08lX\n", split_timeout_low);
901
902     split_timeout_hi = CondSwapFromBus32(split_timeout_hi);
903     split_timeout_low = CondSwapFromBus32(split_timeout_low);
904
905     return (split_timeout_hi & 7) * 1000000 + (split_timeout_low >> 19) * 125;
906 }
907
908 int
909 Ieee1394Service::getVerboseLevel()
910 {
911     return getDebugLevel();
912 }
913
914 void
915 Ieee1394Service::printBuffer( unsigned int level, size_t length, fb_quadlet_t* buffer ) const
916 {
917
918     for ( unsigned int i=0; i < length; ++i ) {
919         if ( ( i % 4 ) == 0 ) {
920             if ( i > 0 ) {
921                 debugOutputShort(level,"\n");
922             }
923             debugOutputShort(level," %4d: ",i*4);
924         }
925         debugOutputShort(level,"%08X ",buffer[i]);
926     }
927     debugOutputShort(level,"\n");
928 }
929 void
930 Ieee1394Service::printBufferBytes( unsigned int level, size_t length, byte_t* buffer ) const
931 {
932
933     for ( unsigned int i=0; i < length; ++i ) {
934         if ( ( i % 16 ) == 0 ) {
935             if ( i > 0 ) {
936                 debugOutputShort(level,"\n");
937             }
938             debugOutputShort(level," %4d: ",i*16);
939         }
940         debugOutputShort(level,"%02X ",buffer[i]);
941     }
942     debugOutputShort(level,"\n");
943 }
944
945 int
946 Ieee1394Service::resetHandlerLowLevel( raw1394handle_t handle,
947                                        unsigned int generation )
948 {
949     raw1394_update_generation ( handle, generation );
950     Ieee1394Service* instance
951         = (Ieee1394Service*) raw1394_get_userdata( handle );
952     instance->resetHandler( generation );
953
954     return 0;
955 }
956
957 bool
958 Ieee1394Service::resetHandler( unsigned int generation )
959 {
960     quadlet_t buf=0;
961
962     m_handle_lock->Lock();
963     raw1394_update_generation(m_handle, generation);
964     m_handle_lock->Unlock();
965
966     // do a simple read on ourself in order to update the internal structures
967     // this avoids failures after a bus reset
968     read_quadlet( getLocalNodeId() | 0xFFC0,
969                   CSR_REGISTER_BASE | CSR_CYCLE_TIME,
970                   &buf );
971
972     for ( reset_handler_vec_t::iterator it = m_busResetHandlers.begin();
973           it != m_busResetHandlers.end();
974           ++it )
975     {
976         Util::Functor* func = *it;
977         ( *func )();
978     }
979
980     return true;
981 }
982
983 bool
984 Ieee1394Service::startRHThread()
985 {
986     int i;
987
988     if ( m_threadRunning ) {
989         return true;
990     }
991     m_RHThread_lock->Lock();
992     i = pthread_create( &m_thread, 0, rHThread, this );
993     m_RHThread_lock->Unlock();
994     if (i) {
995         debugFatal("Could not start ieee1394 service thread\n");
996         return false;
997     }
998     m_threadRunning = true;
999
1000     return true;
1001 }
1002
1003 void
1004 Ieee1394Service::stopRHThread()
1005 {
1006     if ( m_threadRunning ) {
1007         // wait for the thread to finish it's work
1008         m_RHThread_lock->Lock();
1009         pthread_cancel (m_thread);
1010         pthread_join (m_thread, 0);
1011         m_RHThread_lock->Unlock();
1012         m_threadRunning = false;
1013     }
1014 }
1015
1016 void*
1017 Ieee1394Service::rHThread( void* arg )
1018 {
1019     Ieee1394Service* pIeee1394Service = (Ieee1394Service*) arg;
1020
1021     while (true) {
1022         // protect ourselves from dying
1023         {
1024             // use a scoped lock such that it is unlocked
1025             // even if we are cancelled while running
1026             // FIXME: check if this is true!
1027 //             Util::MutexLockHelper lock(*(pIeee1394Service->m_RHThread_lock));
1028             raw1394_loop_iterate (pIeee1394Service->m_resetHandle);
1029         }
1030         pthread_testcancel ();
1031     }
1032
1033     return 0;
1034 }
1035
1036 bool
1037 Ieee1394Service::addBusResetHandler( Util::Functor* functor )
1038 {
1039     debugOutput(DEBUG_LEVEL_VERBOSE, "Adding busreset handler (%p)\n", functor);
1040     m_busResetHandlers.push_back( functor );
1041     return true;
1042 }
1043
1044 bool
1045 Ieee1394Service::remBusResetHandler( Util::Functor* functor )
1046 {
1047     debugOutput(DEBUG_LEVEL_VERBOSE, "Removing busreset handler (%p)\n", functor);
1048
1049     for ( reset_handler_vec_t::iterator it = m_busResetHandlers.begin();
1050           it != m_busResetHandlers.end();
1051           ++it )
1052     {
1053         if ( *it == functor ) {
1054             debugOutput(DEBUG_LEVEL_VERBOSE, " found\n");
1055             m_busResetHandlers.erase( it );
1056             return true;
1057         }
1058     }
1059     debugOutput(DEBUG_LEVEL_VERBOSE, " not found\n");
1060     return false;
1061 }
1062
1063 /**
1064  * Allocates an iso channel for use by the interface in a similar way to
1065  * libiec61883.  Returns -1 on error (due to there being no free channels)
1066  * or an allocated channel number.
1067  *
1068  * Does not perform anything other than registering the channel and the
1069  * bandwidth at the IRM
1070  *
1071  * Also allocates the necessary bandwidth (in ISO allocation units).
1072  *
1073  * FIXME: As in libiec61883, channel 63 is not requested; this is either a
1074  * bug or it's omitted since that's the channel preferred by video devices.
1075  *
1076  * @param bandwidth the bandwidth to allocate for this channel
1077  * @return the channel number
1078  */
1079 signed int Ieee1394Service::allocateIsoChannelGeneric(unsigned int bandwidth) {
1080     debugOutput(DEBUG_LEVEL_VERBOSE, "Allocating ISO channel using generic method...\n" );
1081
1082     Util::MutexLockHelper lock(*m_handle_lock);
1083     struct ChannelInfo cinfo;
1084
1085     int c = -1;
1086     for (c = 0; c < 63; c++) {
1087         if (raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_ALLOC) == 0)
1088             break;
1089     }
1090     if (c < 63) {
1091         if (raw1394_bandwidth_modify(m_handle, bandwidth, RAW1394_MODIFY_ALLOC) < 0) {
1092             debugFatal("Could not allocate bandwidth of %d\n", bandwidth);
1093
1094             raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_FREE);
1095             return -1;
1096         } else {
1097             cinfo.channel=c;
1098             cinfo.bandwidth=bandwidth;
1099             cinfo.alloctype=AllocGeneric;
1100
1101             cinfo.xmit_node=-1;
1102             cinfo.xmit_plug=-1;
1103             cinfo.recv_node=-1;
1104             cinfo.recv_plug=-1;
1105
1106             if (registerIsoChannel(c, cinfo)) {
1107                 return c;
1108             } else {
1109                 raw1394_bandwidth_modify(m_handle, bandwidth, RAW1394_MODIFY_FREE);
1110                 raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_FREE);
1111                 return -1;
1112             }
1113         }
1114     }
1115     return -1;
1116 }
1117
1118 /**
1119  * Allocates an iso channel for use by the interface in a similar way to
1120  * libiec61883.  Returns -1 on error (due to there being no free channels)
1121  * or an allocated channel number.
1122  *
1123  * Uses IEC61883 Connection Management Procedure to establish the connection.
1124  *
1125  * Also allocates the necessary bandwidth (in ISO allocation units).
1126  *
1127  * @param xmit_node  node id of the transmitter
1128  * @param xmit_plug  the output plug to use. If -1, find the first online plug, and
1129  * upon return, contains the plug number used.
1130  * @param recv_node  node id of the receiver
1131  * @param recv_plug the input plug to use. If -1, find the first online plug, and
1132  * upon return, contains the plug number used.
1133  *
1134  * @return the channel number
1135  */
1136
1137 signed int Ieee1394Service::allocateIsoChannelCMP(
1138     nodeid_t xmit_node, int xmit_plug,
1139     nodeid_t recv_node, int recv_plug
1140     ) {
1141
1142     if (xmit_node == INVALID_NODE_ID) {
1143         debugWarning("operation on invalid node (XMIT)\n");
1144         return -1;
1145     }
1146     if (recv_node == INVALID_NODE_ID) {
1147         debugWarning("operation on invalid node (RECV)\n");
1148         return -1;
1149     }
1150
1151     debugOutput(DEBUG_LEVEL_VERBOSE, "Allocating ISO channel using IEC61883 CMP...\n" );
1152     Util::MutexLockHelper lock(*m_handle_lock);
1153
1154     struct ChannelInfo cinfo;
1155
1156     int c = -1;
1157     int bandwidth=1;
1158     #if IEEE1394SERVICE_SKIP_IEC61883_BANDWIDTH_ALLOCATION
1159     bandwidth=0;
1160     #endif
1161
1162     // do connection management: make connection
1163     c = iec61883_cmp_connect(
1164         m_handle,
1165         xmit_node | 0xffc0,
1166         &xmit_plug,
1167         recv_node | 0xffc0,
1168         &recv_plug,
1169         &bandwidth);
1170
1171     if((c<0) || (c>63)) {
1172         debugError("Could not do CMP from %04X:%02d to %04X:%02d\n",
1173             xmit_node, xmit_plug, recv_node, recv_plug
1174             );
1175         return -1;
1176     }
1177
1178     cinfo.channel=c;
1179     cinfo.bandwidth=bandwidth;
1180     cinfo.alloctype=AllocCMP;
1181
1182     cinfo.xmit_node=xmit_node;
1183     cinfo.xmit_plug=xmit_plug;
1184     cinfo.recv_node=recv_node;
1185     cinfo.recv_plug=recv_plug;
1186
1187     if (registerIsoChannel(c, cinfo)) {
1188         return c;
1189     }
1190
1191     return -1;
1192 }
1193
1194 /**
1195  * Deallocates an iso channel.  Silently ignores a request to deallocate
1196  * a negative channel number.
1197  *
1198  * Figures out the method that was used to allocate the channel (generic, cmp, ...)
1199  * and uses the appropriate method to deallocate. Also frees the bandwidth
1200  * that was reserved along with this channel.
1201  *
1202  * @param c channel number
1203  * @return true if successful
1204  */
1205 bool Ieee1394Service::freeIsoChannel(signed int c) {
1206     debugOutput(DEBUG_LEVEL_VERBOSE, "Freeing ISO channel %d...\n", c );
1207     Util::MutexLockHelper lock(*m_handle_lock);
1208
1209     if (c < 0 || c > 63) {
1210         debugWarning("Invalid channel number: %d\n", c);
1211         return false;
1212     }
1213
1214     switch (m_channels[c].alloctype) {
1215         default:
1216             debugError(" BUG: invalid allocation type!\n");
1217             return false;
1218
1219         case AllocFree:
1220             debugWarning(" Channel %d not registered\n", c);
1221             return false;
1222
1223         case AllocGeneric:
1224             debugOutput(DEBUG_LEVEL_VERBOSE, " allocated using generic routine...\n" );
1225             debugOutput(DEBUG_LEVEL_VERBOSE, " freeing %d bandwidth units...\n", m_channels[c].bandwidth );
1226             if (raw1394_bandwidth_modify(m_handle, m_channels[c].bandwidth, RAW1394_MODIFY_FREE) !=0) {
1227                 debugWarning("Failed to deallocate bandwidth\n");
1228             }
1229             debugOutput(DEBUG_LEVEL_VERBOSE, " freeing channel %d...\n", m_channels[c].channel );
1230             if (raw1394_channel_modify (m_handle, m_channels[c].channel, RAW1394_MODIFY_FREE) != 0) {
1231                 debugWarning("Failed to free channel\n");
1232             }
1233             if (!unregisterIsoChannel(c))
1234                 return false;
1235             return true;
1236
1237         case AllocCMP:
1238             debugOutput(DEBUG_LEVEL_VERBOSE, " allocated using IEC61883 CMP...\n" );
1239             debugOutput(DEBUG_LEVEL_VERBOSE, " performing IEC61883 CMP disconnect...\n" );
1240             if(iec61883_cmp_disconnect(
1241                     m_handle,
1242                     m_channels[c].xmit_node | 0xffc0,
1243                     m_channels[c].xmit_plug,
1244                     m_channels[c].recv_node | 0xffc0,
1245                     m_channels[c].recv_plug,
1246                     m_channels[c].channel,
1247                     m_channels[c].bandwidth) != 0) {
1248                 debugWarning("Could not do CMP disconnect for channel %d!\n",c);
1249             }
1250             if (!unregisterIsoChannel(c))
1251                 return false;
1252             return true;
1253     }
1254
1255     // unreachable
1256     debugError("BUG: unreachable code reached!\n");
1257
1258     return false;
1259 }
1260
1261 /**
1262  * Registers a channel as managed by this ieee1394service
1263  * @param c channel number
1264  * @param cinfo channel info struct
1265  * @return true if successful
1266  */
1267 bool Ieee1394Service::registerIsoChannel(unsigned int c, struct ChannelInfo cinfo) {
1268     if (c < 63) {
1269         if (m_channels[c].alloctype != AllocFree) {
1270             debugWarning("Channel %d already registered with bandwidth %d\n",
1271                 m_channels[c].channel, m_channels[c].bandwidth);
1272         }
1273
1274         memcpy(&m_channels[c], &cinfo, sizeof(struct ChannelInfo));
1275
1276     } else return false;
1277     return true;
1278 }
1279
1280 /**
1281  * unegisters a channel from this ieee1394service
1282  * @param c channel number
1283  * @return true if successful
1284  */
1285 bool Ieee1394Service::unregisterIsoChannel(unsigned int c) {
1286     if (c < 63) {
1287         if (m_channels[c].alloctype == AllocFree) {
1288             debugWarning("Channel %d not registered\n", c);
1289             return false;
1290         }
1291
1292         m_channels[c].channel=-1;
1293         m_channels[c].bandwidth=-1;
1294         m_channels[c].alloctype=AllocFree;
1295         m_channels[c].xmit_node=0xFFFF;
1296         m_channels[c].xmit_plug=-1;
1297         m_channels[c].recv_node=0xFFFF;
1298         m_channels[c].recv_plug=-1;
1299
1300     } else return false;
1301     return true;
1302 }
1303
1304 /**
1305  * Returns the current value of the `bandwidth available' register on
1306  * the IRM, or -1 on error.
1307  * @return
1308  */
1309 signed int Ieee1394Service::getAvailableBandwidth() {
1310     quadlet_t buffer;
1311     Util::MutexLockHelper lock(*m_handle_lock);
1312     signed int result = raw1394_read (m_handle, raw1394_get_irm_id (m_handle),
1313         CSR_REGISTER_BASE + CSR_BANDWIDTH_AVAILABLE,
1314         sizeof (quadlet_t), &buffer);
1315
1316     if (result < 0)
1317         return -1;
1318     return CondSwapFromBus32(buffer);
1319 }
1320
1321 void
1322 Ieee1394Service::setVerboseLevel(int l)
1323 {
1324     if (m_pIsoManager) m_pIsoManager->setVerboseLevel(l);
1325     if (m_pCTRHelper) m_pCTRHelper->setVerboseLevel(l);
1326     if (m_pWatchdog) m_pWatchdog->setVerboseLevel(l);
1327     setDebugLevel(l);
1328     debugOutput( DEBUG_LEVEL_VERBOSE, "Setting verbose level to %d...\n", l );
1329 }
1330
1331 void
1332 Ieee1394Service::show()
1333 {
1334     #ifdef DEBUG
1335     uint32_t cycle_timer;
1336     uint64_t local_time;
1337     if(!readCycleTimerReg(&cycle_timer, &local_time)) {
1338         debugWarning("Could not read cycle timer register\n");
1339    
1340     }
1341     uint64_t ctr = CYCLE_TIMER_TO_TICKS( cycle_timer );
1342
1343     debugOutput( DEBUG_LEVEL_VERBOSE, "Port:  %d\n", getPort() );
1344     debugOutput( DEBUG_LEVEL_VERBOSE, " Name: %s\n", getPortName().c_str() );
1345     debugOutput( DEBUG_LEVEL_VERBOSE, " CycleTimerHelper: %p, IsoManager: %p, WatchDog: %p\n",
1346                  m_pCTRHelper, m_pIsoManager, m_pWatchdog );
1347     debugOutput( DEBUG_LEVEL_VERBOSE, " Time: %011llu (%03us %04ucy %04uticks)\n",
1348                 ctr,
1349                 (unsigned int)TICKS_TO_SECS( ctr ),
1350                 (unsigned int)TICKS_TO_CYCLES( ctr ),
1351                 (unsigned int)TICKS_TO_OFFSET( ctr ) );
1352     debugOutputShort( DEBUG_LEVEL_NORMAL, "Iso handler info:\n");
1353     #endif
1354     if (m_pIsoManager) m_pIsoManager->dumpInfo();
1355 }
Note: See TracBrowser for help on using the browser.