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

Revision 1136, 35.1 kB (checked in by ppalmers, 13 years ago)

keep bus direction in endian swapping functions

  • 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 "ARMHandler.h"
28 #include "cycletimer.h"
29 #include "IsoHandlerManager.h"
30 #include "CycleTimerHelper.h"
31
32 #include <libavc1394/avc1394.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
40 #include <errno.h>
41 #include "libutil/ByteSwap.h"
42
43 #include <string.h>
44
45 #include <iostream>
46 #include <iomanip>
47
48 IMPL_DEBUG_MODULE( Ieee1394Service, Ieee1394Service, DEBUG_LEVEL_NORMAL );
49
50 Ieee1394Service::Ieee1394Service()
51     : m_handle( 0 )
52     , m_handle_lock( new Util::PosixMutex() )
53     , m_resetHandle( 0 )
54     , m_util_handle( 0 )
55     , m_port( -1 )
56     , m_RHThread_lock( new Util::PosixMutex() )
57     , m_threadRunning( false )
58     , m_realtime ( false )
59     , m_base_priority ( 0 )
60     , m_pIsoManager( new IsoHandlerManager( *this ) )
61     , m_pCTRHelper ( new CycleTimerHelper( *this, IEEE1394SERVICE_CYCLETIMER_DLL_UPDATE_INTERVAL_USEC ) )
62     , m_have_new_ctr_read ( false )
63     , m_pWatchdog ( new Util::Watchdog() )
64 {
65     for (unsigned int i=0; i<64; i++) {
66         m_channels[i].channel=-1;
67         m_channels[i].bandwidth=-1;
68         m_channels[i].alloctype=AllocFree;
69         m_channels[i].xmit_node=0xFFFF;
70         m_channels[i].xmit_plug=-1;
71         m_channels[i].recv_node=0xFFFF;
72         m_channels[i].recv_plug=-1;
73     }
74 }
75
76 Ieee1394Service::Ieee1394Service(bool rt, int prio)
77     : m_handle( 0 )
78     , m_handle_lock( new Util::PosixMutex() )
79     , m_resetHandle( 0 )
80     , m_util_handle( 0 )
81     , m_port( -1 )
82     , m_RHThread_lock( new Util::PosixMutex() )
83     , m_threadRunning( false )
84     , m_realtime ( rt )
85     , m_base_priority ( prio )
86     , m_pIsoManager( new IsoHandlerManager( *this, rt, prio ) )
87     , m_pCTRHelper ( new CycleTimerHelper( *this, IEEE1394SERVICE_CYCLETIMER_DLL_UPDATE_INTERVAL_USEC,
88                                            rt && IEEE1394SERVICE_CYCLETIMER_HELPER_RUN_REALTIME,
89                                            prio + IEEE1394SERVICE_CYCLETIMER_HELPER_PRIO_INCREASE ) )
90     , m_have_new_ctr_read ( false )
91     , m_pWatchdog ( new Util::Watchdog() )
92 {
93     for (unsigned int i=0; i<64; i++) {
94         m_channels[i].channel=-1;
95         m_channels[i].bandwidth=-1;
96         m_channels[i].alloctype=AllocFree;
97         m_channels[i].xmit_node=0xFFFF;
98         m_channels[i].xmit_plug=-1;
99         m_channels[i].recv_node=0xFFFF;
100         m_channels[i].recv_plug=-1;
101     }
102 }
103
104 Ieee1394Service::~Ieee1394Service()
105 {
106     delete m_pIsoManager;
107     delete m_pCTRHelper;
108     stopRHThread();
109     for ( arm_handler_vec_t::iterator it = m_armHandlers.begin();
110           it != m_armHandlers.end();
111           ++it )
112     {
113         debugOutput(DEBUG_LEVEL_VERBOSE, "Unregistering ARM handler for 0x%016llX\n", (*it)->getStart());
114         int err=raw1394_arm_unregister(m_resetHandle, (*it)->getStart());
115         if (err) {
116             debugError(" Failed to unregister ARM handler for 0x%016llX\n", (*it)->getStart());
117             debugError(" Error: %s\n", strerror(errno));
118         }
119     }
120
121     delete m_pWatchdog;
122     if ( m_handle ) {
123         raw1394_destroy_handle( m_handle );
124     }
125     delete m_handle_lock;
126
127     if ( m_resetHandle ) {
128         raw1394_destroy_handle( m_resetHandle );
129     }
130     delete m_RHThread_lock;
131     if ( m_util_handle ) {
132         raw1394_destroy_handle( m_util_handle );
133     }
134 }
135
136 int
137 Ieee1394Service::detectNbPorts()
138 {
139     raw1394handle_t tmp_handle = raw1394_new_handle();
140     if ( tmp_handle == NULL ) {
141         debugError("Could not get libraw1394 handle.\n");
142         return -1;
143     }
144     struct raw1394_portinfo pinf[IEEE1394SERVICE_MAX_FIREWIRE_PORTS];
145     int nb_detected_ports = raw1394_get_port_info(tmp_handle, pinf, IEEE1394SERVICE_MAX_FIREWIRE_PORTS);
146     raw1394_destroy_handle(tmp_handle);
147
148     if (nb_detected_ports < 0) {
149         debugError("Failed to detect number of ports\n");
150         return -1;
151     }
152     return nb_detected_ports;
153 }
154
155 bool
156 Ieee1394Service::initialize( int port )
157 {
158     using namespace std;
159
160     int nb_ports = detectNbPorts();
161     if (port + 1 > nb_ports) {
162         debugFatal("Requested port (%d) out of range (# ports: %d)\n", port, nb_ports);
163     }
164
165     if(!m_pWatchdog) {
166         debugError("No valid RT watchdog found.\n");
167         return false;
168     }
169     if(!m_pWatchdog->start()) {
170         debugError("Could not start RT watchdog.\n");
171         return false;
172     }
173
174     m_handle = raw1394_new_handle_on_port( port );
175     if ( !m_handle ) {
176         if ( !errno ) {
177             debugFatal("libraw1394 not compatible\n");
178         } else {
179             debugFatal("Ieee1394Service::initialize: Could not get 1394 handle: %s\n",
180                 strerror(errno) );
181             debugFatal("Is ieee1394 and raw1394 driver loaded?\n");
182         }
183         return false;
184     }
185
186     m_resetHandle = raw1394_new_handle_on_port( port );
187     if ( !m_resetHandle ) {
188         if ( !errno ) {
189             debugFatal("libraw1394 not compatible\n");
190         } else {
191             debugFatal("Ieee1394Service::initialize: Could not get 1394 handle: %s",
192                 strerror(errno) );
193             debugFatal("Is ieee1394 and raw1394 driver loaded?\n");
194         }
195         return false;
196     }
197
198     m_util_handle = raw1394_new_handle_on_port( port );
199     if ( !m_util_handle ) {
200         if ( !errno ) {
201             debugFatal("libraw1394 not compatible\n");
202         } else {
203             debugFatal("Ieee1394Service::initialize: Could not get 1394 handle: %s",
204                 strerror(errno) );
205             debugFatal("Is ieee1394 and raw1394 driver loaded?\n");
206         }
207         return false;
208     }
209
210     // test the cycle timer read function
211     int err;
212     uint32_t cycle_timer;
213     uint64_t local_time;
214     err=raw1394_read_cycle_timer(m_handle, &cycle_timer, &local_time);
215     if(err) {
216         debugOutput(DEBUG_LEVEL_VERBOSE, "raw1394_read_cycle_timer failed.\n");
217         debugOutput(DEBUG_LEVEL_VERBOSE, " Error descr: %s\n", strerror(err));
218         debugWarning("==================================================================\n");
219         debugWarning(" This system doesn't support the raw1394_read_cycle_timer call.   \n");
220         debugWarning(" Fallback to indirect CTR read method.                            \n");
221         debugWarning(" FFADO should work, but achieving low-latency might be a problem. \n");
222         debugWarning(" Upgrade the kernel to version 2.6.21 or higher to solve this.    \n");
223         debugWarning("==================================================================\n");
224         m_have_new_ctr_read = false;
225     } else {
226         debugOutput(DEBUG_LEVEL_VERBOSE, "This system supports the raw1394_read_cycle_timer call, using it.\n");
227         m_have_new_ctr_read = true;
228     }
229
230     m_port = port;
231
232     // obtain port name
233     raw1394handle_t tmp_handle = raw1394_new_handle();
234     if ( tmp_handle == NULL ) {
235         debugError("Could not get temporaty libraw1394 handle.\n");
236         return false;
237     }
238     struct raw1394_portinfo pinf[IEEE1394SERVICE_MAX_FIREWIRE_PORTS];
239     int nb_detected_ports = raw1394_get_port_info(tmp_handle, pinf, IEEE1394SERVICE_MAX_FIREWIRE_PORTS);
240     raw1394_destroy_handle(tmp_handle);
241
242     if (nb_detected_ports < 0) {
243         debugError("Failed to detect number of ports\n");
244         return false;
245     }
246
247     if(nb_detected_ports && port < IEEE1394SERVICE_MAX_FIREWIRE_PORTS) {
248         m_portName = pinf[port].name;
249     } else {
250         m_portName = "Unknown";
251     }
252     if (m_portName == "") {
253         m_portName = "Unknown";
254     }
255
256     raw1394_set_userdata( m_handle, this );
257     raw1394_set_userdata( m_resetHandle, this );
258     raw1394_set_userdata( m_util_handle, this );
259     raw1394_set_bus_reset_handler( m_resetHandle,
260                                    this->resetHandlerLowLevel );
261
262     m_default_arm_handler = raw1394_set_arm_tag_handler( m_resetHandle,
263                                    this->armHandlerLowLevel );
264
265     if(!m_pCTRHelper) {
266         debugFatal("No CycleTimerHelper available, bad!\n");
267         return false;
268     }
269     m_pCTRHelper->setVerboseLevel(getDebugLevel());
270     if(!m_pCTRHelper->Start()) {
271         debugFatal("Could not start CycleTimerHelper\n");
272         return false;
273     }
274
275     if(!m_pIsoManager) {
276         debugFatal("No IsoHandlerManager available, bad!\n");
277         return false;
278     }
279     m_pIsoManager->setVerboseLevel(getDebugLevel());
280     if(!m_pIsoManager->init()) {
281         debugFatal("Could not initialize IsoHandlerManager\n");
282         return false;
283     }
284
285     startRHThread();
286
287     // make sure that the thread parameters of all our helper threads are OK
288     if(!setThreadParameters(m_realtime, m_base_priority)) {
289         debugFatal("Could not set thread parameters\n");
290         return false;
291     }
292     return true;
293 }
294
295 bool
296 Ieee1394Service::setThreadParameters(bool rt, int priority) {
297     bool result = true;
298     if (priority > THREAD_MAX_RTPRIO) priority = THREAD_MAX_RTPRIO;
299     m_base_priority = priority;
300     m_realtime = rt;
301     if (m_pIsoManager) {
302         debugOutput(DEBUG_LEVEL_VERBOSE, "Switching IsoManager to (rt=%d, prio=%d)\n",
303                                          rt, priority);
304         result &= m_pIsoManager->setThreadParameters(rt, priority);
305     }
306     if (m_pCTRHelper) {
307         debugOutput(DEBUG_LEVEL_VERBOSE, "Switching CycleTimerHelper to (rt=%d, prio=%d)\n",
308                                          rt && IEEE1394SERVICE_CYCLETIMER_HELPER_RUN_REALTIME,
309                                          priority + IEEE1394SERVICE_CYCLETIMER_HELPER_PRIO_INCREASE);
310         result &= m_pCTRHelper->setThreadParameters(rt && IEEE1394SERVICE_CYCLETIMER_HELPER_RUN_REALTIME,
311                                                     priority + IEEE1394SERVICE_CYCLETIMER_HELPER_PRIO_INCREASE);
312     }
313     return result;
314 }
315
316 int
317 Ieee1394Service::getNodeCount()
318 {
319     Util::MutexLockHelper lock(*m_handle_lock);
320     return raw1394_get_nodecount( m_handle );
321 }
322
323 nodeid_t Ieee1394Service::getLocalNodeId() {
324     Util::MutexLockHelper lock(*m_handle_lock);
325     return raw1394_get_local_id(m_handle) & 0x3F;
326 }
327
328 /**
329  * Returns the current value of the cycle timer (in ticks)
330  *
331  * @return the current value of the cycle timer (in ticks)
332  */
333
334 uint32_t
335 Ieee1394Service::getCycleTimerTicks() {
336     return m_pCTRHelper->getCycleTimerTicks();
337 }
338
339 /**
340  * Returns the current value of the cycle timer (as is)
341  *
342  * @return the current value of the cycle timer (as is)
343  */
344 uint32_t
345 Ieee1394Service::getCycleTimer() {
346     return m_pCTRHelper->getCycleTimer();
347 }
348
349 /**
350  * Returns the current value of the cycle timer (in ticks)
351  * for a specific time instant (usecs since epoch)
352  * @return the current value of the cycle timer (in ticks)
353  */
354
355 uint32_t
356 Ieee1394Service::getCycleTimerTicks(uint64_t t) {
357     return m_pCTRHelper->getCycleTimerTicks(t);
358 }
359
360 /**
361  * Returns the current value of the cycle timer (as is)
362  * for a specific time instant (usecs since epoch)
363  * @return the current value of the cycle timer (as is)
364  */
365 uint32_t
366 Ieee1394Service::getCycleTimer(uint64_t t) {
367     return m_pCTRHelper->getCycleTimer(t);
368 }
369
370 bool
371 Ieee1394Service::readCycleTimerReg(uint32_t *cycle_timer, uint64_t *local_time)
372 {
373     if(m_have_new_ctr_read) {
374         int err;
375         err = raw1394_read_cycle_timer(m_util_handle, cycle_timer, local_time);
376         if(err) {
377             debugWarning("raw1394_read_cycle_timer: %s\n", strerror(err));
378             return false;
379         }
380         return true;
381     } else {
382         // do a normal read of the CTR register
383         // the disadvantage is that local_time and cycle time are not
384         // read at the same time instant (scheduling issues)
385         *local_time = getCurrentTimeAsUsecs();
386         if ( raw1394_read( m_util_handle,
387                 getLocalNodeId() | 0xFFC0,
388                 CSR_REGISTER_BASE | CSR_CYCLE_TIME,
389                 sizeof(uint32_t), cycle_timer ) == 0 ) {
390             *cycle_timer = CondSwapFromBus32(*cycle_timer);
391             return true;
392         } else {
393             return false;
394         }
395     }
396 }
397
398 uint64_t
399 Ieee1394Service::getCurrentTimeAsUsecs() {
400     return Util::SystemTimeSource::getCurrentTimeAsUsecs();
401 }
402
403 bool
404 Ieee1394Service::read( fb_nodeid_t nodeId,
405                        fb_nodeaddr_t addr,
406                        size_t length,
407                        fb_quadlet_t* buffer )
408 {
409     Util::MutexLockHelper lock(*m_handle_lock);
410     using namespace std;
411     if ( raw1394_read( m_handle, nodeId, addr, length*4, buffer ) == 0 ) {
412
413         #ifdef DEBUG
414         debugOutput(DEBUG_LEVEL_VERY_VERBOSE,
415             "read: node 0x%hX, addr = 0x%016llX, length = %u\n",
416             nodeId, addr, length);
417         printBuffer( DEBUG_LEVEL_VERY_VERBOSE, length, buffer );
418         #endif
419
420         return true;
421     } else {
422         #ifdef DEBUG
423         debugOutput(DEBUG_LEVEL_NORMAL,
424                     "raw1394_read failed: node 0x%hX, addr = 0x%016llX, length = %u\n",
425                     nodeId, addr, length);
426         #endif
427         return false;
428     }
429 }
430
431 bool
432 Ieee1394Service::read_quadlet( fb_nodeid_t nodeId,
433                                fb_nodeaddr_t addr,
434                                fb_quadlet_t* buffer )
435 {
436     return read( nodeId,  addr, sizeof( *buffer )/4, buffer );
437 }
438
439 bool
440 Ieee1394Service::read_octlet( fb_nodeid_t nodeId,
441                               fb_nodeaddr_t addr,
442                               fb_octlet_t* buffer )
443 {
444     return read( nodeId, addr, sizeof( *buffer )/4,
445                  reinterpret_cast<fb_quadlet_t*>( buffer ) );
446 }
447
448 bool
449 Ieee1394Service::write( fb_nodeid_t nodeId,
450                         fb_nodeaddr_t addr,
451                         size_t length,
452                         fb_quadlet_t* data )
453 {
454     Util::MutexLockHelper lock(*m_handle_lock);
455     using namespace std;
456
457     #ifdef DEBUG
458     debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"write: node 0x%hX, addr = 0x%016llX, length = %d\n",
459                 nodeId, addr, length);
460     printBuffer( DEBUG_LEVEL_VERY_VERBOSE, length, data );
461     #endif
462
463     return raw1394_write( m_handle, nodeId, addr, length*4, data ) == 0;
464 }
465
466 bool
467 Ieee1394Service::write_quadlet( fb_nodeid_t nodeId,
468                                 fb_nodeaddr_t addr,
469                                 fb_quadlet_t data )
470 {
471     return write( nodeId, addr, sizeof( data )/4, &data );
472 }
473
474 bool
475 Ieee1394Service::write_octlet( fb_nodeid_t nodeId,
476                                fb_nodeaddr_t addr,
477                                fb_octlet_t data )
478 {
479     return write( nodeId, addr, sizeof( data )/4,
480                   reinterpret_cast<fb_quadlet_t*>( &data ) );
481 }
482
483 bool
484 Ieee1394Service::lockCompareSwap64(  fb_nodeid_t nodeId,
485                         fb_nodeaddr_t addr,
486                         fb_octlet_t  compare_value,
487                         fb_octlet_t  swap_value,
488                         fb_octlet_t* result )
489 {
490     #ifdef DEBUG
491     debugOutput(DEBUG_LEVEL_VERBOSE,"lockCompareSwap64: node 0x%X, addr = 0x%016llX\n",
492                 nodeId, addr);
493     debugOutput(DEBUG_LEVEL_VERBOSE,"  if (*(addr)==0x%016llX) *(addr)=0x%016llX\n",
494                 compare_value, swap_value);
495     fb_octlet_t buffer;
496     if(!read_octlet( nodeId, addr,&buffer )) {
497         debugWarning("Could not read register\n");
498     } else {
499         debugOutput(DEBUG_LEVEL_VERBOSE,"before = 0x%016llX\n", buffer);
500     }
501     #endif
502
503     // do endiannes swapping
504     compare_value = CondSwapToBus64(compare_value);
505     swap_value    = CondSwapToBus64(swap_value);
506
507     // do separate locking here (no MutexLockHelper) since
508     // we use read_octlet in the DEBUG code in this function
509     m_handle_lock->Lock();
510     int retval=raw1394_lock64(m_handle, nodeId, addr,
511                               RAW1394_EXTCODE_COMPARE_SWAP,
512                               swap_value, compare_value, result);
513     m_handle_lock->Unlock();
514
515     if(retval) {
516         debugError("raw1394_lock64 failed: %s\n", strerror(errno));
517     }
518
519     #ifdef DEBUG
520     if(!read_octlet( nodeId, addr,&buffer )) {
521         debugWarning("Could not read register\n");
522     } else {
523         debugOutput(DEBUG_LEVEL_VERBOSE,"after = 0x%016llX\n", buffer);
524     }
525     #endif
526
527     *result = CondSwapFromBus64(*result);
528
529     return (retval == 0);
530 }
531
532 fb_quadlet_t*
533 Ieee1394Service::transactionBlock( fb_nodeid_t nodeId,
534                                    fb_quadlet_t* buf,
535                                    int len,
536                                    unsigned int* resp_len )
537 {
538     // FIXME: this requires transactionBlockClose to unlock
539     m_handle_lock->Lock();
540     for (int i = 0; i < len; ++i) {
541         buf[i] = CondSwapFromBus32( buf[i] );
542     }
543
544     fb_quadlet_t* result =
545         avc1394_transaction_block2( m_handle,
546                                     nodeId,
547                                     buf,
548                                     len,
549                                     resp_len,
550                                     10 );
551
552     for ( unsigned int i = 0; i < *resp_len; ++i ) {
553         result[i] = CondSwapToBus32( result[i] );
554     }
555
556     return result;
557 }
558
559
560 bool
561 Ieee1394Service::transactionBlockClose()
562 {
563     avc1394_transaction_block_close( m_handle );
564     m_handle_lock->Unlock();
565     return true;
566 }
567
568 int
569 Ieee1394Service::getVerboseLevel()
570 {
571     return getDebugLevel();
572 }
573
574 void
575 Ieee1394Service::printBuffer( unsigned int level, size_t length, fb_quadlet_t* buffer ) const
576 {
577
578     for ( unsigned int i=0; i < length; ++i ) {
579         if ( ( i % 4 ) == 0 ) {
580             if ( i > 0 ) {
581                 debugOutputShort(level,"\n");
582             }
583             debugOutputShort(level," %4d: ",i*4);
584         }
585         debugOutputShort(level,"%08X ",buffer[i]);
586     }
587     debugOutputShort(level,"\n");
588 }
589 void
590 Ieee1394Service::printBufferBytes( unsigned int level, size_t length, byte_t* buffer ) const
591 {
592
593     for ( unsigned int i=0; i < length; ++i ) {
594         if ( ( i % 16 ) == 0 ) {
595             if ( i > 0 ) {
596                 debugOutputShort(level,"\n");
597             }
598             debugOutputShort(level," %4d: ",i*16);
599         }
600         debugOutputShort(level,"%02X ",buffer[i]);
601     }
602     debugOutputShort(level,"\n");
603 }
604
605 int
606 Ieee1394Service::resetHandlerLowLevel( raw1394handle_t handle,
607                                        unsigned int generation )
608 {
609     raw1394_update_generation ( handle, generation );
610     Ieee1394Service* instance
611         = (Ieee1394Service*) raw1394_get_userdata( handle );
612     instance->resetHandler( generation );
613
614     return 0;
615 }
616
617 bool
618 Ieee1394Service::resetHandler( unsigned int generation )
619 {
620     quadlet_t buf=0;
621
622     // do a simple read on ourself in order to update the internal structures
623     // this avoids failures after a bus reset
624     read_quadlet( getLocalNodeId() | 0xFFC0,
625                   CSR_REGISTER_BASE | CSR_CYCLE_TIME,
626                   &buf );
627
628     for ( reset_handler_vec_t::iterator it = m_busResetHandlers.begin();
629           it != m_busResetHandlers.end();
630           ++it )
631     {
632         Util::Functor* func = *it;
633         ( *func )();
634     }
635
636     return true;
637 }
638
639 bool Ieee1394Service::registerARMHandler(ARMHandler *h) {
640     debugOutput(DEBUG_LEVEL_VERBOSE, "Registering ARM handler (%p) for 0x%016llX, length %u\n",
641         h, h->getStart(), h->getLength());
642
643     int err=raw1394_arm_register(m_resetHandle, h->getStart(),
644                                  h->getLength(), h->getBuffer(), (octlet_t)h,
645                                  h->getAccessRights(),
646                                  h->getNotificationOptions(),
647                                  h->getClientTransactions());
648     if (err) {
649         debugError("Failed to register ARM handler for 0x%016llX\n", h->getStart());
650         debugError(" Error: %s\n", strerror(errno));
651         return false;
652     }
653
654     m_armHandlers.push_back( h );
655
656     return true;
657 }
658
659 bool Ieee1394Service::unregisterARMHandler( ARMHandler *h ) {
660     debugOutput(DEBUG_LEVEL_VERBOSE, "Unregistering ARM handler (%p) for 0x%016llX\n",
661         h, h->getStart());
662
663     for ( arm_handler_vec_t::iterator it = m_armHandlers.begin();
664           it != m_armHandlers.end();
665           ++it )
666     {
667         if((*it) == h) {
668             int err=raw1394_arm_unregister(m_resetHandle, h->getStart());
669             if (err) {
670                 debugError("Failed to unregister ARM handler (%p)\n", h);
671                 debugError(" Error: %s\n", strerror(errno));
672             } else {
673                 m_armHandlers.erase(it);
674                 return true;
675             }
676         }
677     }
678     debugOutput(DEBUG_LEVEL_VERBOSE, " handler not found!\n");
679
680     return false;
681 }
682 /**
683  * @brief Tries to find a free ARM address range
684  *
685  * @param start  address to start with
686  * @param length length of the block needed (bytes)
687  * @param step   step to use when searching (bytes)
688  * @return The base address that is free, and 0xFFFFFFFFFFFFFFFF when failed
689  */
690 nodeaddr_t Ieee1394Service::findFreeARMBlock( nodeaddr_t start, size_t length, size_t step ) {
691     debugOutput(DEBUG_LEVEL_VERBOSE, "Finding free ARM block of %d bytes, from 0x%016llX in steps of %d bytes\n",
692         length, start, step);
693
694     int cnt=0;
695     const int maxcnt=10;
696     int err=1;
697     while(err && cnt++ < maxcnt) {
698         // try to register
699         err=raw1394_arm_register(m_resetHandle, start, length, 0, 0, 0, 0, 0);
700
701         if (err) {
702             debugOutput(DEBUG_LEVEL_VERBOSE, " -> cannot use 0x%016llX\n", start);
703             debugError("    Error: %s\n", strerror(errno));
704             start += step;
705         } else {
706             debugOutput(DEBUG_LEVEL_VERBOSE, " -> use 0x%016llX\n", start);
707             err=raw1394_arm_unregister(m_resetHandle, start);
708             if (err) {
709                 debugOutput(DEBUG_LEVEL_VERBOSE, " error unregistering test handler\n");
710                 debugError("    Error: %s\n", strerror(errno));
711                 return 0xFFFFFFFFFFFFFFFFLLU;
712             }
713             return start;
714         }
715     }
716     debugOutput(DEBUG_LEVEL_VERBOSE, " Could not find free block in %d tries\n",cnt);
717     return 0xFFFFFFFFFFFFFFFFLLU;
718 }
719
720 int
721 Ieee1394Service::armHandlerLowLevel(raw1394handle_t handle,
722                      unsigned long arm_tag,
723                      byte_t request_type, unsigned int requested_length,
724                      void *data)
725 {
726     Ieee1394Service* instance
727         = (Ieee1394Service*) raw1394_get_userdata( handle );
728     instance->armHandler( arm_tag, request_type, requested_length, data );
729
730     return 0;
731 }
732
733 bool
734 Ieee1394Service::armHandler(  unsigned long arm_tag,
735                      byte_t request_type, unsigned int requested_length,
736                      void *data)
737 {
738     for ( arm_handler_vec_t::iterator it = m_armHandlers.begin();
739           it != m_armHandlers.end();
740           ++it )
741     {
742         if((*it) == (ARMHandler *)arm_tag) {
743             struct raw1394_arm_request_response *arm_req_resp;
744             arm_req_resp  = (struct raw1394_arm_request_response *) data;
745             raw1394_arm_request_t arm_req=arm_req_resp->request;
746             raw1394_arm_response_t arm_resp=arm_req_resp->response;
747
748             debugOutput(DEBUG_LEVEL_VERBOSE,"ARM handler for address 0x%016llX called\n",
749                 (*it)->getStart());
750             debugOutput(DEBUG_LEVEL_VERBOSE," request type   : 0x%02X\n",request_type);
751             debugOutput(DEBUG_LEVEL_VERBOSE," request length : %04d\n",requested_length);
752
753             switch(request_type) {
754                 case RAW1394_ARM_READ:
755                     (*it)->handleRead(arm_req);
756                     *arm_resp=*((*it)->getResponse());
757                     break;
758                 case RAW1394_ARM_WRITE:
759                     (*it)->handleWrite(arm_req);
760                     *arm_resp=*((*it)->getResponse());
761                     break;
762                 case RAW1394_ARM_LOCK:
763                     (*it)->handleLock(arm_req);
764                     *arm_resp=*((*it)->getResponse());
765                     break;
766                 default:
767                     debugWarning("Unknown request type received, ignoring...\n");
768             }
769
770             return true;
771         }
772     }
773
774     debugOutput(DEBUG_LEVEL_VERBOSE,"default ARM handler called\n");
775
776     m_default_arm_handler(m_resetHandle, arm_tag, request_type, requested_length, data );
777     return true;
778 }
779
780 bool
781 Ieee1394Service::startRHThread()
782 {
783     int i;
784
785     if ( m_threadRunning ) {
786         return true;
787     }
788     m_RHThread_lock->Lock();
789     i = pthread_create( &m_thread, 0, rHThread, this );
790     m_RHThread_lock->Unlock();
791     if (i) {
792         debugFatal("Could not start ieee1394 service thread\n");
793         return false;
794     }
795     m_threadRunning = true;
796
797     return true;
798 }
799
800 void
801 Ieee1394Service::stopRHThread()
802 {
803     if ( m_threadRunning ) {
804         m_RHThread_lock->Lock();
805         pthread_cancel (m_thread);
806         pthread_join (m_thread, 0);
807         m_RHThread_lock->Unlock();
808         m_threadRunning = false;
809     }
810 }
811
812 void*
813 Ieee1394Service::rHThread( void* arg )
814 {
815     Ieee1394Service* pIeee1394Service = (Ieee1394Service*) arg;
816
817     while (true) {
818         raw1394_loop_iterate (pIeee1394Service->m_resetHandle);
819         pthread_testcancel ();
820     }
821
822     return 0;
823 }
824
825 bool
826 Ieee1394Service::addBusResetHandler( Util::Functor* functor )
827 {
828     debugOutput(DEBUG_LEVEL_VERBOSE, "Adding busreset handler (%p)\n", functor);
829     m_busResetHandlers.push_back( functor );
830     return true;
831 }
832
833 bool
834 Ieee1394Service::remBusResetHandler( Util::Functor* functor )
835 {
836     debugOutput(DEBUG_LEVEL_VERBOSE, "Removing busreset handler (%p)\n", functor);
837
838     for ( reset_handler_vec_t::iterator it = m_busResetHandlers.begin();
839           it != m_busResetHandlers.end();
840           ++it )
841     {
842         if ( *it == functor ) {
843             debugOutput(DEBUG_LEVEL_VERBOSE, " found\n");
844             m_busResetHandlers.erase( it );
845             return true;
846         }
847     }
848     debugOutput(DEBUG_LEVEL_VERBOSE, " not found\n");
849     return false;
850 }
851
852 /**
853  * Allocates an iso channel for use by the interface in a similar way to
854  * libiec61883.  Returns -1 on error (due to there being no free channels)
855  * or an allocated channel number.
856  *
857  * Does not perform anything other than registering the channel and the
858  * bandwidth at the IRM
859  *
860  * Also allocates the necessary bandwidth (in ISO allocation units).
861  *
862  * FIXME: As in libiec61883, channel 63 is not requested; this is either a
863  * bug or it's omitted since that's the channel preferred by video devices.
864  *
865  * @param bandwidth the bandwidth to allocate for this channel
866  * @return the channel number
867  */
868 signed int Ieee1394Service::allocateIsoChannelGeneric(unsigned int bandwidth) {
869     debugOutput(DEBUG_LEVEL_VERBOSE, "Allocating ISO channel using generic method...\n" );
870
871     Util::MutexLockHelper lock(*m_handle_lock);
872     struct ChannelInfo cinfo;
873
874     int c = -1;
875     for (c = 0; c < 63; c++) {
876         if (raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_ALLOC) == 0)
877             break;
878     }
879     if (c < 63) {
880         if (raw1394_bandwidth_modify(m_handle, bandwidth, RAW1394_MODIFY_ALLOC) < 0) {
881             debugFatal("Could not allocate bandwidth of %d\n", bandwidth);
882
883             raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_FREE);
884             return -1;
885         } else {
886             cinfo.channel=c;
887             cinfo.bandwidth=bandwidth;
888             cinfo.alloctype=AllocGeneric;
889
890             cinfo.xmit_node=-1;
891             cinfo.xmit_plug=-1;
892             cinfo.recv_node=-1;
893             cinfo.recv_plug=-1;
894
895             if (registerIsoChannel(c, cinfo)) {
896                 return c;
897             } else {
898                 raw1394_bandwidth_modify(m_handle, bandwidth, RAW1394_MODIFY_FREE);
899                 raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_FREE);
900                 return -1;
901             }
902         }
903     }
904     return -1;
905 }
906
907 /**
908  * Allocates an iso channel for use by the interface in a similar way to
909  * libiec61883.  Returns -1 on error (due to there being no free channels)
910  * or an allocated channel number.
911  *
912  * Uses IEC61883 Connection Management Procedure to establish the connection.
913  *
914  * Also allocates the necessary bandwidth (in ISO allocation units).
915  *
916  * @param xmit_node  node id of the transmitter
917  * @param xmit_plug  the output plug to use. If -1, find the first online plug, and
918  * upon return, contains the plug number used.
919  * @param recv_node  node id of the receiver
920  * @param recv_plug the input plug to use. If -1, find the first online plug, and
921  * upon return, contains the plug number used.
922  *
923  * @return the channel number
924  */
925
926 signed int Ieee1394Service::allocateIsoChannelCMP(
927     nodeid_t xmit_node, int xmit_plug,
928     nodeid_t recv_node, int recv_plug
929     ) {
930
931     debugOutput(DEBUG_LEVEL_VERBOSE, "Allocating ISO channel using IEC61883 CMP...\n" );
932     Util::MutexLockHelper lock(*m_handle_lock);
933
934     struct ChannelInfo cinfo;
935
936     int c = -1;
937     int bandwidth=1;
938     #if IEEE1394SERVICE_SKIP_IEC61883_BANDWIDTH_ALLOCATION
939     bandwidth=0;
940     #endif
941
942     // do connection management: make connection
943     c = iec61883_cmp_connect(
944         m_handle,
945         xmit_node | 0xffc0,
946         &xmit_plug,
947         recv_node | 0xffc0,
948         &recv_plug,
949         &bandwidth);
950
951     if((c<0) || (c>63)) {
952         debugError("Could not do CMP from %04X:%02d to %04X:%02d\n",
953             xmit_node, xmit_plug, recv_node, recv_plug
954             );
955         return -1;
956     }
957
958     cinfo.channel=c;
959     cinfo.bandwidth=bandwidth;
960     cinfo.alloctype=AllocCMP;
961
962     cinfo.xmit_node=xmit_node;
963     cinfo.xmit_plug=xmit_plug;
964     cinfo.recv_node=recv_node;
965     cinfo.recv_plug=recv_plug;
966
967     if (registerIsoChannel(c, cinfo)) {
968         return c;
969     }
970
971     return -1;
972 }
973
974 /**
975  * Deallocates an iso channel.  Silently ignores a request to deallocate
976  * a negative channel number.
977  *
978  * Figures out the method that was used to allocate the channel (generic, cmp, ...)
979  * and uses the appropriate method to deallocate. Also frees the bandwidth
980  * that was reserved along with this channel.
981  *
982  * @param c channel number
983  * @return true if successful
984  */
985 bool Ieee1394Service::freeIsoChannel(signed int c) {
986     debugOutput(DEBUG_LEVEL_VERBOSE, "Freeing ISO channel %d...\n", c );
987     Util::MutexLockHelper lock(*m_handle_lock);
988
989     if (c < 0 || c > 63) {
990         debugWarning("Invalid channel number: %d\n", c);
991         return false;
992     }
993
994     switch (m_channels[c].alloctype) {
995         default:
996             debugError(" BUG: invalid allocation type!\n");
997             return false;
998
999         case AllocFree:
1000             debugWarning(" Channel %d not registered\n", c);
1001             return false;
1002
1003         case AllocGeneric:
1004             debugOutput(DEBUG_LEVEL_VERBOSE, " allocated using generic routine...\n" );
1005             debugOutput(DEBUG_LEVEL_VERBOSE, " freeing %d bandwidth units...\n", m_channels[c].bandwidth );
1006             if (raw1394_bandwidth_modify(m_handle, m_channels[c].bandwidth, RAW1394_MODIFY_FREE) !=0) {
1007                 debugWarning("Failed to deallocate bandwidth\n");
1008             }
1009             debugOutput(DEBUG_LEVEL_VERBOSE, " freeing channel %d...\n", m_channels[c].channel );
1010             if (raw1394_channel_modify (m_handle, m_channels[c].channel, RAW1394_MODIFY_FREE) != 0) {
1011                 debugWarning("Failed to free channel\n");
1012             }
1013             if (!unregisterIsoChannel(c))
1014                 return false;
1015             return true;
1016
1017         case AllocCMP:
1018             debugOutput(DEBUG_LEVEL_VERBOSE, " allocated using IEC61883 CMP...\n" );
1019             debugOutput(DEBUG_LEVEL_VERBOSE, " performing IEC61883 CMP disconnect...\n" );
1020             if(iec61883_cmp_disconnect(
1021                     m_handle,
1022                     m_channels[c].xmit_node | 0xffc0,
1023                     m_channels[c].xmit_plug,
1024                     m_channels[c].recv_node | 0xffc0,
1025                     m_channels[c].recv_plug,
1026                     m_channels[c].channel,
1027                     m_channels[c].bandwidth) != 0) {
1028                 debugWarning("Could not do CMP disconnect for channel %d!\n",c);
1029             }
1030             if (!unregisterIsoChannel(c))
1031                 return false;
1032             return true;
1033     }
1034
1035     // unreachable
1036     debugError("BUG: unreachable code reached!\n");
1037
1038     return false;
1039 }
1040
1041 /**
1042  * Registers a channel as managed by this ieee1394service
1043  * @param c channel number
1044  * @param cinfo channel info struct
1045  * @return true if successful
1046  */
1047 bool Ieee1394Service::registerIsoChannel(unsigned int c, struct ChannelInfo cinfo) {
1048     if (c < 63) {
1049         if (m_channels[c].alloctype != AllocFree) {
1050             debugWarning("Channel %d already registered with bandwidth %d\n",
1051                 m_channels[c].channel, m_channels[c].bandwidth);
1052         }
1053
1054         memcpy(&m_channels[c], &cinfo, sizeof(struct ChannelInfo));
1055
1056     } else return false;
1057     return true;
1058 }
1059
1060 /**
1061  * unegisters a channel from this ieee1394service
1062  * @param c channel number
1063  * @return true if successful
1064  */
1065 bool Ieee1394Service::unregisterIsoChannel(unsigned int c) {
1066     if (c < 63) {
1067         if (m_channels[c].alloctype == AllocFree) {
1068             debugWarning("Channel %d not registered\n", c);
1069             return false;
1070         }
1071
1072         m_channels[c].channel=-1;
1073         m_channels[c].bandwidth=-1;
1074         m_channels[c].alloctype=AllocFree;
1075         m_channels[c].xmit_node=0xFFFF;
1076         m_channels[c].xmit_plug=-1;
1077         m_channels[c].recv_node=0xFFFF;
1078         m_channels[c].recv_plug=-1;
1079
1080     } else return false;
1081     return true;
1082 }
1083
1084 /**
1085  * Returns the current value of the `bandwidth available' register on
1086  * the IRM, or -1 on error.
1087  * @return
1088  */
1089 signed int Ieee1394Service::getAvailableBandwidth() {
1090     quadlet_t buffer;
1091     Util::MutexLockHelper lock(*m_handle_lock);
1092     signed int result = raw1394_read (m_handle, raw1394_get_irm_id (m_handle),
1093         CSR_REGISTER_BASE + CSR_BANDWIDTH_AVAILABLE,
1094         sizeof (quadlet_t), &buffer);
1095
1096     if (result < 0)
1097         return -1;
1098     return CondSwapFromBus32(buffer);
1099 }
1100
1101 void
1102 Ieee1394Service::setVerboseLevel(int l)
1103 {
1104     if (m_pIsoManager) m_pIsoManager->setVerboseLevel(l);
1105     if (m_pCTRHelper) m_pCTRHelper->setVerboseLevel(l);
1106     if (m_pWatchdog) m_pWatchdog->setVerboseLevel(l);
1107     setDebugLevel(l);
1108     debugOutput( DEBUG_LEVEL_VERBOSE, "Setting verbose level to %d...\n", l );
1109 }
1110
1111 void
1112 Ieee1394Service::show()
1113 {
1114     #ifdef DEBUG
1115     uint32_t cycle_timer;
1116     uint64_t local_time;
1117     if(!readCycleTimerReg(&cycle_timer, &local_time)) {
1118         debugWarning("Could not read cycle timer register\n");
1119    
1120     }
1121     uint64_t ctr = CYCLE_TIMER_TO_TICKS( cycle_timer );
1122
1123     debugOutput( DEBUG_LEVEL_VERBOSE, "Port:  %d\n", getPort() );
1124     debugOutput( DEBUG_LEVEL_VERBOSE, " Name: %s\n", getPortName().c_str() );
1125     debugOutput( DEBUG_LEVEL_VERBOSE, " CycleTimerHelper: %p, IsoManager: %p, WatchDog: %p\n",
1126                  m_pCTRHelper, m_pIsoManager, m_pWatchdog );
1127     debugOutput( DEBUG_LEVEL_VERBOSE, " Time: %011llu (%03us %04ucy %04uticks)\n",
1128                 ctr,
1129                 (unsigned int)TICKS_TO_SECS( ctr ),
1130                 (unsigned int)TICKS_TO_CYCLES( ctr ),
1131                 (unsigned int)TICKS_TO_OFFSET( ctr ) );
1132     debugOutputShort( DEBUG_LEVEL_NORMAL, "Iso handler info:\n");
1133     #endif
1134     if (m_pIsoManager) m_pIsoManager->dumpInfo();
1135 }
Note: See TracBrowser for help on using the browser.