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

Revision 782, 32.5 kB (checked in by ppalmers, 16 years ago)

fix mutex macro's; don't run thread in old_style mode

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