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

Revision 1046, 35.6 kB (checked in by ppalmers, 16 years ago)

debug message cleanup. move #defines into config.h.in.

  • 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 <netinet/in.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 = ntohl(*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 fb_octlet_t
484 Ieee1394Service::byteSwap_octlet(fb_octlet_t value) {
485     #if __BYTE_ORDER == __BIG_ENDIAN
486         return value;
487     #elif __BYTE_ORDER == __LITTLE_ENDIAN
488         fb_quadlet_t in_hi = (value >> 32) & 0xFFFFFFFF;
489         fb_quadlet_t in_lo = value & 0xFFFFFFFF;
490         in_hi = htonl(in_hi);
491         in_lo = htonl(in_lo);
492         fb_octlet_t value_new = in_lo;
493         value_new <<= 32;
494         value_new |= in_hi;
495         return value_new;
496     #else
497         #error Unknown endiannes
498     #endif
499 }
500
501 bool
502 Ieee1394Service::lockCompareSwap64(  fb_nodeid_t nodeId,
503                         fb_nodeaddr_t addr,
504                         fb_octlet_t  compare_value,
505                         fb_octlet_t  swap_value,
506                         fb_octlet_t* result )
507 {
508     #ifdef DEBUG
509     debugOutput(DEBUG_LEVEL_VERBOSE,"lockCompareSwap64: node 0x%X, addr = 0x%016llX\n",
510                 nodeId, addr);
511     debugOutput(DEBUG_LEVEL_VERBOSE,"  if (*(addr)==0x%016llX) *(addr)=0x%016llX\n",
512                 compare_value, swap_value);
513     fb_octlet_t buffer;
514     if(!read_octlet( nodeId, addr,&buffer )) {
515         debugWarning("Could not read register\n");
516     } else {
517         debugOutput(DEBUG_LEVEL_VERBOSE,"before = 0x%016llX\n", buffer);
518     }
519     #endif
520
521     // do endiannes swapping
522     compare_value = byteSwap_octlet(compare_value);
523     swap_value    = byteSwap_octlet(swap_value);
524
525     // do separate locking here (no MutexLockHelper) since
526     // we use read_octlet in the DEBUG code in this function
527     m_handle_lock->Lock();
528     int retval=raw1394_lock64(m_handle, nodeId, addr,
529                               RAW1394_EXTCODE_COMPARE_SWAP,
530                               swap_value, compare_value, result);
531     m_handle_lock->Unlock();
532
533     if(retval) {
534         debugError("raw1394_lock64 failed: %s\n", strerror(errno));
535     }
536
537     #ifdef DEBUG
538     if(!read_octlet( nodeId, addr,&buffer )) {
539         debugWarning("Could not read register\n");
540     } else {
541         debugOutput(DEBUG_LEVEL_VERBOSE,"after = 0x%016llX\n", buffer);
542     }
543     #endif
544
545     *result = byteSwap_octlet(*result);
546
547     return (retval == 0);
548 }
549
550 fb_quadlet_t*
551 Ieee1394Service::transactionBlock( fb_nodeid_t nodeId,
552                                    fb_quadlet_t* buf,
553                                    int len,
554                                    unsigned int* resp_len )
555 {
556     // FIXME: this requires transactionBlockClose to unlock
557     m_handle_lock->Lock();
558     for (int i = 0; i < len; ++i) {
559         buf[i] = ntohl( buf[i] );
560     }
561
562     fb_quadlet_t* result =
563         avc1394_transaction_block2( m_handle,
564                                     nodeId,
565                                     buf,
566                                     len,
567                                     resp_len,
568                                     10 );
569
570     for ( unsigned int i = 0; i < *resp_len; ++i ) {
571         result[i] = htonl( result[i] );
572     }
573
574     return result;
575 }
576
577
578 bool
579 Ieee1394Service::transactionBlockClose()
580 {
581     avc1394_transaction_block_close( m_handle );
582     m_handle_lock->Unlock();
583     return true;
584 }
585
586 int
587 Ieee1394Service::getVerboseLevel()
588 {
589     return getDebugLevel();
590 }
591
592 void
593 Ieee1394Service::printBuffer( unsigned int level, size_t length, fb_quadlet_t* buffer ) const
594 {
595
596     for ( unsigned int i=0; i < length; ++i ) {
597         if ( ( i % 4 ) == 0 ) {
598             if ( i > 0 ) {
599                 debugOutputShort(level,"\n");
600             }
601             debugOutputShort(level," %4d: ",i*4);
602         }
603         debugOutputShort(level,"%08X ",buffer[i]);
604     }
605     debugOutputShort(level,"\n");
606 }
607 void
608 Ieee1394Service::printBufferBytes( unsigned int level, size_t length, byte_t* buffer ) const
609 {
610
611     for ( unsigned int i=0; i < length; ++i ) {
612         if ( ( i % 16 ) == 0 ) {
613             if ( i > 0 ) {
614                 debugOutputShort(level,"\n");
615             }
616             debugOutputShort(level," %4d: ",i*16);
617         }
618         debugOutputShort(level,"%02X ",buffer[i]);
619     }
620     debugOutputShort(level,"\n");
621 }
622
623 int
624 Ieee1394Service::resetHandlerLowLevel( raw1394handle_t handle,
625                                        unsigned int generation )
626 {
627     raw1394_update_generation ( handle, generation );
628     Ieee1394Service* instance
629         = (Ieee1394Service*) raw1394_get_userdata( handle );
630     instance->resetHandler( generation );
631
632     return 0;
633 }
634
635 bool
636 Ieee1394Service::resetHandler( unsigned int generation )
637 {
638     quadlet_t buf=0;
639
640     // do a simple read on ourself in order to update the internal structures
641     // this avoids failures after a bus reset
642     read_quadlet( getLocalNodeId() | 0xFFC0,
643                   CSR_REGISTER_BASE | CSR_CYCLE_TIME,
644                   &buf );
645
646     for ( reset_handler_vec_t::iterator it = m_busResetHandlers.begin();
647           it != m_busResetHandlers.end();
648           ++it )
649     {
650         Util::Functor* func = *it;
651         ( *func )();
652     }
653
654     return true;
655 }
656
657 bool Ieee1394Service::registerARMHandler(ARMHandler *h) {
658     debugOutput(DEBUG_LEVEL_VERBOSE, "Registering ARM handler (%p) for 0x%016llX, length %u\n",
659         h, h->getStart(), h->getLength());
660
661     int err=raw1394_arm_register(m_resetHandle, h->getStart(),
662                                  h->getLength(), h->getBuffer(), (octlet_t)h,
663                                  h->getAccessRights(),
664                                  h->getNotificationOptions(),
665                                  h->getClientTransactions());
666     if (err) {
667         debugError("Failed to register ARM handler for 0x%016llX\n", h->getStart());
668         debugError(" Error: %s\n", strerror(errno));
669         return false;
670     }
671
672     m_armHandlers.push_back( h );
673
674     return true;
675 }
676
677 bool Ieee1394Service::unregisterARMHandler( ARMHandler *h ) {
678     debugOutput(DEBUG_LEVEL_VERBOSE, "Unregistering ARM handler (%p) for 0x%016llX\n",
679         h, h->getStart());
680
681     for ( arm_handler_vec_t::iterator it = m_armHandlers.begin();
682           it != m_armHandlers.end();
683           ++it )
684     {
685         if((*it) == h) {
686             int err=raw1394_arm_unregister(m_resetHandle, h->getStart());
687             if (err) {
688                 debugError("Failed to unregister ARM handler (%p)\n", h);
689                 debugError(" Error: %s\n", strerror(errno));
690             } else {
691                 m_armHandlers.erase(it);
692                 return true;
693             }
694         }
695     }
696     debugOutput(DEBUG_LEVEL_VERBOSE, " handler not found!\n");
697
698     return false;
699 }
700 /**
701  * @brief Tries to find a free ARM address range
702  *
703  * @param start  address to start with
704  * @param length length of the block needed (bytes)
705  * @param step   step to use when searching (bytes)
706  * @return The base address that is free, and 0xFFFFFFFFFFFFFFFF when failed
707  */
708 nodeaddr_t Ieee1394Service::findFreeARMBlock( nodeaddr_t start, size_t length, size_t step ) {
709     debugOutput(DEBUG_LEVEL_VERBOSE, "Finding free ARM block of %d bytes, from 0x%016llX in steps of %d bytes\n",
710         length, start, step);
711
712     int cnt=0;
713     const int maxcnt=10;
714     int err=1;
715     while(err && cnt++ < maxcnt) {
716         // try to register
717         err=raw1394_arm_register(m_resetHandle, start, length, 0, 0, 0, 0, 0);
718
719         if (err) {
720             debugOutput(DEBUG_LEVEL_VERBOSE, " -> cannot use 0x%016llX\n", start);
721             debugError("    Error: %s\n", strerror(errno));
722             start += step;
723         } else {
724             debugOutput(DEBUG_LEVEL_VERBOSE, " -> use 0x%016llX\n", start);
725             err=raw1394_arm_unregister(m_resetHandle, start);
726             if (err) {
727                 debugOutput(DEBUG_LEVEL_VERBOSE, " error unregistering test handler\n");
728                 debugError("    Error: %s\n", strerror(errno));
729                 return 0xFFFFFFFFFFFFFFFFLLU;
730             }
731             return start;
732         }
733     }
734     debugOutput(DEBUG_LEVEL_VERBOSE, " Could not find free block in %d tries\n",cnt);
735     return 0xFFFFFFFFFFFFFFFFLLU;
736 }
737
738 int
739 Ieee1394Service::armHandlerLowLevel(raw1394handle_t handle,
740                      unsigned long arm_tag,
741                      byte_t request_type, unsigned int requested_length,
742                      void *data)
743 {
744     Ieee1394Service* instance
745         = (Ieee1394Service*) raw1394_get_userdata( handle );
746     instance->armHandler( arm_tag, request_type, requested_length, data );
747
748     return 0;
749 }
750
751 bool
752 Ieee1394Service::armHandler(  unsigned long arm_tag,
753                      byte_t request_type, unsigned int requested_length,
754                      void *data)
755 {
756     for ( arm_handler_vec_t::iterator it = m_armHandlers.begin();
757           it != m_armHandlers.end();
758           ++it )
759     {
760         if((*it) == (ARMHandler *)arm_tag) {
761             struct raw1394_arm_request_response *arm_req_resp;
762             arm_req_resp  = (struct raw1394_arm_request_response *) data;
763             raw1394_arm_request_t arm_req=arm_req_resp->request;
764             raw1394_arm_response_t arm_resp=arm_req_resp->response;
765
766             debugOutput(DEBUG_LEVEL_VERBOSE,"ARM handler for address 0x%016llX called\n",
767                 (*it)->getStart());
768             debugOutput(DEBUG_LEVEL_VERBOSE," request type   : 0x%02X\n",request_type);
769             debugOutput(DEBUG_LEVEL_VERBOSE," request length : %04d\n",requested_length);
770
771             switch(request_type) {
772                 case RAW1394_ARM_READ:
773                     (*it)->handleRead(arm_req);
774                     *arm_resp=*((*it)->getResponse());
775                     break;
776                 case RAW1394_ARM_WRITE:
777                     (*it)->handleWrite(arm_req);
778                     *arm_resp=*((*it)->getResponse());
779                     break;
780                 case RAW1394_ARM_LOCK:
781                     (*it)->handleLock(arm_req);
782                     *arm_resp=*((*it)->getResponse());
783                     break;
784                 default:
785                     debugWarning("Unknown request type received, ignoring...\n");
786             }
787
788             return true;
789         }
790     }
791
792     debugOutput(DEBUG_LEVEL_VERBOSE,"default ARM handler called\n");
793
794     m_default_arm_handler(m_resetHandle, arm_tag, request_type, requested_length, data );
795     return true;
796 }
797
798 bool
799 Ieee1394Service::startRHThread()
800 {
801     int i;
802
803     if ( m_threadRunning ) {
804         return true;
805     }
806     m_RHThread_lock->Lock();
807     i = pthread_create( &m_thread, 0, rHThread, this );
808     m_RHThread_lock->Unlock();
809     if (i) {
810         debugFatal("Could not start ieee1394 service thread\n");
811         return false;
812     }
813     m_threadRunning = true;
814
815     return true;
816 }
817
818 void
819 Ieee1394Service::stopRHThread()
820 {
821     if ( m_threadRunning ) {
822         m_RHThread_lock->Lock();
823         pthread_cancel (m_thread);
824         pthread_join (m_thread, 0);
825         m_RHThread_lock->Unlock();
826         m_threadRunning = false;
827     }
828 }
829
830 void*
831 Ieee1394Service::rHThread( void* arg )
832 {
833     Ieee1394Service* pIeee1394Service = (Ieee1394Service*) arg;
834
835     while (true) {
836         raw1394_loop_iterate (pIeee1394Service->m_resetHandle);
837         pthread_testcancel ();
838     }
839
840     return 0;
841 }
842
843 bool
844 Ieee1394Service::addBusResetHandler( Util::Functor* functor )
845 {
846     debugOutput(DEBUG_LEVEL_VERBOSE, "Adding busreset handler (%p)\n", functor);
847     m_busResetHandlers.push_back( functor );
848     return true;
849 }
850
851 bool
852 Ieee1394Service::remBusResetHandler( Util::Functor* functor )
853 {
854     debugOutput(DEBUG_LEVEL_VERBOSE, "Removing busreset handler (%p)\n", functor);
855
856     for ( reset_handler_vec_t::iterator it = m_busResetHandlers.begin();
857           it != m_busResetHandlers.end();
858           ++it )
859     {
860         if ( *it == functor ) {
861             debugOutput(DEBUG_LEVEL_VERBOSE, " found\n");
862             m_busResetHandlers.erase( it );
863             return true;
864         }
865     }
866     debugOutput(DEBUG_LEVEL_VERBOSE, " not found\n");
867     return false;
868 }
869
870 /**
871  * Allocates an iso channel for use by the interface in a similar way to
872  * libiec61883.  Returns -1 on error (due to there being no free channels)
873  * or an allocated channel number.
874  *
875  * Does not perform anything other than registering the channel and the
876  * bandwidth at the IRM
877  *
878  * Also allocates the necessary bandwidth (in ISO allocation units).
879  *
880  * FIXME: As in libiec61883, channel 63 is not requested; this is either a
881  * bug or it's omitted since that's the channel preferred by video devices.
882  *
883  * @param bandwidth the bandwidth to allocate for this channel
884  * @return the channel number
885  */
886 signed int Ieee1394Service::allocateIsoChannelGeneric(unsigned int bandwidth) {
887     debugOutput(DEBUG_LEVEL_VERBOSE, "Allocating ISO channel using generic method...\n" );
888
889     Util::MutexLockHelper lock(*m_handle_lock);
890     struct ChannelInfo cinfo;
891
892     int c = -1;
893     for (c = 0; c < 63; c++) {
894         if (raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_ALLOC) == 0)
895             break;
896     }
897     if (c < 63) {
898         if (raw1394_bandwidth_modify(m_handle, bandwidth, RAW1394_MODIFY_ALLOC) < 0) {
899             debugFatal("Could not allocate bandwidth of %d\n", bandwidth);
900
901             raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_FREE);
902             return -1;
903         } else {
904             cinfo.channel=c;
905             cinfo.bandwidth=bandwidth;
906             cinfo.alloctype=AllocGeneric;
907
908             cinfo.xmit_node=-1;
909             cinfo.xmit_plug=-1;
910             cinfo.recv_node=-1;
911             cinfo.recv_plug=-1;
912
913             if (registerIsoChannel(c, cinfo)) {
914                 return c;
915             } else {
916                 raw1394_bandwidth_modify(m_handle, bandwidth, RAW1394_MODIFY_FREE);
917                 raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_FREE);
918                 return -1;
919             }
920         }
921     }
922     return -1;
923 }
924
925 /**
926  * Allocates an iso channel for use by the interface in a similar way to
927  * libiec61883.  Returns -1 on error (due to there being no free channels)
928  * or an allocated channel number.
929  *
930  * Uses IEC61883 Connection Management Procedure to establish the connection.
931  *
932  * Also allocates the necessary bandwidth (in ISO allocation units).
933  *
934  * @param xmit_node  node id of the transmitter
935  * @param xmit_plug  the output plug to use. If -1, find the first online plug, and
936  * upon return, contains the plug number used.
937  * @param recv_node  node id of the receiver
938  * @param recv_plug the input plug to use. If -1, find the first online plug, and
939  * upon return, contains the plug number used.
940  *
941  * @return the channel number
942  */
943
944 signed int Ieee1394Service::allocateIsoChannelCMP(
945     nodeid_t xmit_node, int xmit_plug,
946     nodeid_t recv_node, int recv_plug
947     ) {
948
949     debugOutput(DEBUG_LEVEL_VERBOSE, "Allocating ISO channel using IEC61883 CMP...\n" );
950     Util::MutexLockHelper lock(*m_handle_lock);
951
952     struct ChannelInfo cinfo;
953
954     int c = -1;
955     int bandwidth=1;
956     #if IEEE1394SERVICE_SKIP_IEC61883_BANDWIDTH_ALLOCATION
957     bandwidth=0;
958     #endif
959
960     // do connection management: make connection
961     c = iec61883_cmp_connect(
962         m_handle,
963         xmit_node | 0xffc0,
964         &xmit_plug,
965         recv_node | 0xffc0,
966         &recv_plug,
967         &bandwidth);
968
969     if((c<0) || (c>63)) {
970         debugError("Could not do CMP from %04X:%02d to %04X:%02d\n",
971             xmit_node, xmit_plug, recv_node, recv_plug
972             );
973         return -1;
974     }
975
976     cinfo.channel=c;
977     cinfo.bandwidth=bandwidth;
978     cinfo.alloctype=AllocCMP;
979
980     cinfo.xmit_node=xmit_node;
981     cinfo.xmit_plug=xmit_plug;
982     cinfo.recv_node=recv_node;
983     cinfo.recv_plug=recv_plug;
984
985     if (registerIsoChannel(c, cinfo)) {
986         return c;
987     }
988
989     return -1;
990 }
991
992 /**
993  * Deallocates an iso channel.  Silently ignores a request to deallocate
994  * a negative channel number.
995  *
996  * Figures out the method that was used to allocate the channel (generic, cmp, ...)
997  * and uses the appropriate method to deallocate. Also frees the bandwidth
998  * that was reserved along with this channel.
999  *
1000  * @param c channel number
1001  * @return true if successful
1002  */
1003 bool Ieee1394Service::freeIsoChannel(signed int c) {
1004     debugOutput(DEBUG_LEVEL_VERBOSE, "Freeing ISO channel %d...\n", c );
1005     Util::MutexLockHelper lock(*m_handle_lock);
1006
1007     if (c < 0 || c > 63) {
1008         debugWarning("Invalid channel number: %d\n", c);
1009         return false;
1010     }
1011
1012     switch (m_channels[c].alloctype) {
1013         default:
1014             debugError(" BUG: invalid allocation type!\n");
1015             return false;
1016
1017         case AllocFree:
1018             debugWarning(" Channel %d not registered\n", c);
1019             return false;
1020
1021         case AllocGeneric:
1022             debugOutput(DEBUG_LEVEL_VERBOSE, " allocated using generic routine...\n" );
1023             debugOutput(DEBUG_LEVEL_VERBOSE, " freeing %d bandwidth units...\n", m_channels[c].bandwidth );
1024             if (raw1394_bandwidth_modify(m_handle, m_channels[c].bandwidth, RAW1394_MODIFY_FREE) !=0) {
1025                 debugWarning("Failed to deallocate bandwidth\n");
1026             }
1027             debugOutput(DEBUG_LEVEL_VERBOSE, " freeing channel %d...\n", m_channels[c].channel );
1028             if (raw1394_channel_modify (m_handle, m_channels[c].channel, RAW1394_MODIFY_FREE) != 0) {
1029                 debugWarning("Failed to free channel\n");
1030             }
1031             if (!unregisterIsoChannel(c))
1032                 return false;
1033             return true;
1034
1035         case AllocCMP:
1036             debugOutput(DEBUG_LEVEL_VERBOSE, " allocated using IEC61883 CMP...\n" );
1037             debugOutput(DEBUG_LEVEL_VERBOSE, " performing IEC61883 CMP disconnect...\n" );
1038             if(iec61883_cmp_disconnect(
1039                     m_handle,
1040                     m_channels[c].xmit_node | 0xffc0,
1041                     m_channels[c].xmit_plug,
1042                     m_channels[c].recv_node | 0xffc0,
1043                     m_channels[c].recv_plug,
1044                     m_channels[c].channel,
1045                     m_channels[c].bandwidth) != 0) {
1046                 debugWarning("Could not do CMP disconnect for channel %d!\n",c);
1047             }
1048             if (!unregisterIsoChannel(c))
1049                 return false;
1050             return true;
1051     }
1052
1053     // unreachable
1054     debugError("BUG: unreachable code reached!\n");
1055
1056     return false;
1057 }
1058
1059 /**
1060  * Registers a channel as managed by this ieee1394service
1061  * @param c channel number
1062  * @param cinfo channel info struct
1063  * @return true if successful
1064  */
1065 bool Ieee1394Service::registerIsoChannel(unsigned int c, struct ChannelInfo cinfo) {
1066     if (c < 63) {
1067         if (m_channels[c].alloctype != AllocFree) {
1068             debugWarning("Channel %d already registered with bandwidth %d\n",
1069                 m_channels[c].channel, m_channels[c].bandwidth);
1070         }
1071
1072         memcpy(&m_channels[c], &cinfo, sizeof(struct ChannelInfo));
1073
1074     } else return false;
1075     return true;
1076 }
1077
1078 /**
1079  * unegisters a channel from this ieee1394service
1080  * @param c channel number
1081  * @return true if successful
1082  */
1083 bool Ieee1394Service::unregisterIsoChannel(unsigned int c) {
1084     if (c < 63) {
1085         if (m_channels[c].alloctype == AllocFree) {
1086             debugWarning("Channel %d not registered\n", c);
1087             return false;
1088         }
1089
1090         m_channels[c].channel=-1;
1091         m_channels[c].bandwidth=-1;
1092         m_channels[c].alloctype=AllocFree;
1093         m_channels[c].xmit_node=0xFFFF;
1094         m_channels[c].xmit_plug=-1;
1095         m_channels[c].recv_node=0xFFFF;
1096         m_channels[c].recv_plug=-1;
1097
1098     } else return false;
1099     return true;
1100 }
1101
1102 /**
1103  * Returns the current value of the `bandwidth available' register on
1104  * the IRM, or -1 on error.
1105  * @return
1106  */
1107 signed int Ieee1394Service::getAvailableBandwidth() {
1108     quadlet_t buffer;
1109     Util::MutexLockHelper lock(*m_handle_lock);
1110     signed int result = raw1394_read (m_handle, raw1394_get_irm_id (m_handle),
1111         CSR_REGISTER_BASE + CSR_BANDWIDTH_AVAILABLE,
1112         sizeof (quadlet_t), &buffer);
1113
1114     if (result < 0)
1115         return -1;
1116     return ntohl(buffer);
1117 }
1118
1119 void
1120 Ieee1394Service::setVerboseLevel(int l)
1121 {
1122     if (m_pIsoManager) m_pIsoManager->setVerboseLevel(l);
1123     if (m_pCTRHelper) m_pCTRHelper->setVerboseLevel(l);
1124     if (m_pWatchdog) m_pWatchdog->setVerboseLevel(l);
1125     setDebugLevel(l);
1126     debugOutput( DEBUG_LEVEL_VERBOSE, "Setting verbose level to %d...\n", l );
1127 }
1128
1129 void
1130 Ieee1394Service::show()
1131 {
1132     #ifdef DEBUG
1133     uint32_t cycle_timer;
1134     uint64_t local_time;
1135     if(!readCycleTimerReg(&cycle_timer, &local_time)) {
1136         debugWarning("Could not read cycle timer register\n");
1137    
1138     }
1139     uint64_t ctr = CYCLE_TIMER_TO_TICKS( cycle_timer );
1140
1141     debugOutput( DEBUG_LEVEL_VERBOSE, "Port:  %d\n", getPort() );
1142     debugOutput( DEBUG_LEVEL_VERBOSE, " Name: %s\n", getPortName().c_str() );
1143     debugOutput( DEBUG_LEVEL_VERBOSE, " CycleTimerHelper: %p, IsoManager: %p, WatchDog: %p\n",
1144                  m_pCTRHelper, m_pIsoManager, m_pWatchdog );
1145     debugOutput( DEBUG_LEVEL_VERBOSE, " Time: %011llu (%03us %04ucy %04uticks)\n",
1146                 ctr,
1147                 (unsigned int)TICKS_TO_SECS( ctr ),
1148                 (unsigned int)TICKS_TO_CYCLES( ctr ),
1149                 (unsigned int)TICKS_TO_OFFSET( ctr ) );
1150     debugOutputShort( DEBUG_LEVEL_NORMAL, "Iso handler info:\n");
1151     #endif
1152     if (m_pIsoManager) m_pIsoManager->dumpInfo();
1153 }
Note: See TracBrowser for help on using the browser.