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

Revision 1008, 34.7 kB (checked in by ppalmers, 13 years ago)

fix verbose level

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