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

Revision 753, 32.5 kB (checked in by ppalmers, 13 years ago)

have separate threads for every handler

  • 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         10
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, 1000 ) )
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_pCTRHelper;
103     delete m_pIsoManager;
104     delete m_pTimeSource;
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     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     if (priority > 98) priority = 98;
277     m_base_priority = priority;
278     m_realtime = rt;
279     if (m_pIsoManager) {
280         return m_pIsoManager->setThreadParameters(rt, priority + ISOMANAGER_PRIO_INCREASE);
281     } else {
282         return true;
283     }
284     if (m_pCTRHelper) {
285         return m_pCTRHelper->setThreadParameters(rt, priority + CYCLETIMER_HELPER_PRIO_INCREASE);
286     } else {
287         return true;
288     }
289 }
290
291 int
292 Ieee1394Service::getNodeCount()
293 {
294     return raw1394_get_nodecount( m_handle );
295 }
296
297 nodeid_t Ieee1394Service::getLocalNodeId() {
298     return raw1394_get_local_id(m_handle) & 0x3F;
299 }
300
301 /**
302  * Returns the current value of the cycle timer (in ticks)
303  *
304  * @return the current value of the cycle timer (in ticks)
305  */
306
307 uint32_t
308 Ieee1394Service::getCycleTimerTicks() {
309     return m_pCTRHelper->getCycleTimerTicks();
310 }
311
312 /**
313  * Returns the current value of the cycle timer (as is)
314  *
315  * @return the current value of the cycle timer (as is)
316  */
317
318 uint32_t
319 Ieee1394Service::getCycleTimer() {
320     // the new api should be realtime safe.
321     // it might cause a reschedule when turning preemption,
322     // back on but that won't hurt us if we have sufficient
323     // priority
324     int err;
325     uint32_t cycle_timer;
326     uint64_t local_time;
327     err=raw1394_read_cycle_timer(m_util_handle, &cycle_timer, &local_time);
328     if(err) {
329         debugWarning("raw1394_read_cycle_timer: %s\n", strerror(err));
330     }
331     return cycle_timer;
332 }
333
334 bool
335 Ieee1394Service::readCycleTimerReg(uint32_t *cycle_timer, uint64_t *local_time)
336 {
337     if(m_have_new_ctr_read) {
338         int err;
339         err = raw1394_read_cycle_timer(m_util_handle, cycle_timer, local_time);
340         if(err) {
341             debugWarning("raw1394_read_cycle_timer: %s\n", strerror(err));
342             return false;
343         }
344         return true;
345     } else {
346         // do a normal read of the CTR register
347         // the disadvantage is that local_time and cycle time are not
348         // read at the same time instant (scheduling issues)
349         *local_time = getCurrentTimeAsUsecs();
350         if ( raw1394_read( m_util_handle,
351                 getLocalNodeId() | 0xFFC0,
352                 CSR_REGISTER_BASE | CSR_CYCLE_TIME,
353                 sizeof(uint32_t), cycle_timer ) == 0 ) {
354             *cycle_timer = ntohl(*cycle_timer);
355             return true;
356         } else {
357             return false;
358         }
359     }
360 }
361
362 uint64_t
363 Ieee1394Service::getCurrentTimeAsUsecs() {
364     assert(m_pTimeSource);
365     return m_pTimeSource->getCurrentTimeAsUsecs();
366 }
367
368 bool
369 Ieee1394Service::read( fb_nodeid_t nodeId,
370                        fb_nodeaddr_t addr,
371                        size_t length,
372                        fb_quadlet_t* buffer )
373 {
374     using namespace std;
375     if ( raw1394_read( m_handle, nodeId, addr, length*4, buffer ) == 0 ) {
376
377         #ifdef DEBUG
378         debugOutput(DEBUG_LEVEL_VERY_VERBOSE,
379             "read: node 0x%X, addr = 0x%016llX, length = %u\n",
380             nodeId, addr, length);
381         printBuffer( DEBUG_LEVEL_VERY_VERBOSE, length, buffer );
382         #endif
383
384         return true;
385     } else {
386         #ifdef DEBUG
387         debugError("raw1394_read failed: node 0x%X, addr = 0x%016llX, length = %u\n",
388               nodeId, addr, length);
389         #endif
390
391         return false;
392     }
393 }
394
395
396 bool
397 Ieee1394Service::read_quadlet( fb_nodeid_t nodeId,
398                                fb_nodeaddr_t addr,
399                                fb_quadlet_t* buffer )
400 {
401     return read( nodeId,  addr, sizeof( *buffer )/4, buffer );
402 }
403
404 bool
405 Ieee1394Service::read_octlet( fb_nodeid_t nodeId,
406                               fb_nodeaddr_t addr,
407                               fb_octlet_t* buffer )
408 {
409     return read( nodeId, addr, sizeof( *buffer )/4,
410                  reinterpret_cast<fb_quadlet_t*>( buffer ) );
411 }
412
413
414 bool
415 Ieee1394Service::write( fb_nodeid_t nodeId,
416                         fb_nodeaddr_t addr,
417                         size_t length,
418                         fb_quadlet_t* data )
419 {
420     using namespace std;
421
422     #ifdef DEBUG
423     debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"write: node 0x%X, addr = 0x%016X, length = %d\n",
424                 nodeId, addr, length);
425     printBuffer( DEBUG_LEVEL_VERY_VERBOSE, length, data );
426     #endif
427
428     return raw1394_write( m_handle, nodeId, addr, length*4, data ) == 0;
429 }
430
431
432 bool
433 Ieee1394Service::write_quadlet( fb_nodeid_t nodeId,
434                                 fb_nodeaddr_t addr,
435                                 fb_quadlet_t data )
436 {
437     return write( nodeId, addr, sizeof( data )/4, &data );
438 }
439
440 bool
441 Ieee1394Service::write_octlet( fb_nodeid_t nodeId,
442                                fb_nodeaddr_t addr,
443                                fb_octlet_t data )
444 {
445     return write( nodeId, addr, sizeof( data )/4,
446                   reinterpret_cast<fb_quadlet_t*>( &data ) );
447 }
448
449 fb_octlet_t
450 Ieee1394Service::byteSwap_octlet(fb_octlet_t value) {
451     #if __BYTE_ORDER == __BIG_ENDIAN
452         return value;
453     #elif __BYTE_ORDER == __LITTLE_ENDIAN
454         fb_octlet_t value_new;
455         fb_quadlet_t *in_ptr=reinterpret_cast<fb_quadlet_t *>(&value);
456         fb_quadlet_t *out_ptr=reinterpret_cast<fb_quadlet_t *>(&value_new);
457         *(out_ptr+1)=htonl(*(in_ptr));
458         *(out_ptr)=htonl(*(in_ptr+1));
459         return value_new;
460     #else
461         #error Unknown endiannes
462     #endif
463 }
464
465 bool
466 Ieee1394Service::lockCompareSwap64(  fb_nodeid_t nodeId,
467                         fb_nodeaddr_t addr,
468                         fb_octlet_t  compare_value,
469                         fb_octlet_t  swap_value,
470                         fb_octlet_t* result )
471 {
472     #ifdef DEBUG
473     debugOutput(DEBUG_LEVEL_VERBOSE,"lockCompareSwap64: node 0x%X, addr = 0x%016llX\n",
474                 nodeId, addr);
475     debugOutput(DEBUG_LEVEL_VERBOSE,"  if (*(addr)==0x%016llX) *(addr)=0x%016llX\n",
476                 compare_value, swap_value);
477     fb_octlet_t buffer;
478     if(!read_octlet( nodeId, addr,&buffer )) {
479         debugWarning("Could not read register\n");
480     } else {
481         debugOutput(DEBUG_LEVEL_VERBOSE,"before = 0x%016llX\n", buffer);
482     }
483
484     #endif
485
486     // do endiannes swapping
487     compare_value=byteSwap_octlet(compare_value);
488     swap_value=byteSwap_octlet(swap_value);
489
490     int retval=raw1394_lock64(m_handle, nodeId, addr, RAW1394_EXTCODE_COMPARE_SWAP,
491                           swap_value, compare_value, result);
492
493     #ifdef DEBUG
494     if(!read_octlet( nodeId, addr,&buffer )) {
495         debugWarning("Could not read register\n");
496     } else {
497         debugOutput(DEBUG_LEVEL_VERBOSE,"after = 0x%016llX\n", buffer);
498     }
499     #endif
500
501     *result=byteSwap_octlet(*result);
502
503     return (retval == 0);
504 }
505
506 fb_quadlet_t*
507 Ieee1394Service::transactionBlock( fb_nodeid_t nodeId,
508                                    fb_quadlet_t* buf,
509                                    int len,
510                                    unsigned int* resp_len )
511 {
512     for (int i = 0; i < len; ++i) {
513         buf[i] = ntohl( buf[i] );
514     }
515
516     fb_quadlet_t* result =
517         avc1394_transaction_block2( m_handle,
518                                     nodeId,
519                                     buf,
520                                     len,
521                                     resp_len,
522                                     10 );
523
524     for ( unsigned int i = 0; i < *resp_len; ++i ) {
525         result[i] = htonl( result[i] );
526     }
527
528     return result;
529 }
530
531
532 bool
533 Ieee1394Service::transactionBlockClose()
534 {
535     avc1394_transaction_block_close( m_handle );
536     return true;
537 }
538
539 int
540 Ieee1394Service::getVerboseLevel()
541 {
542     return getDebugLevel();
543 }
544
545 void
546 Ieee1394Service::printBuffer( unsigned int level, size_t length, fb_quadlet_t* buffer ) const
547 {
548
549     for ( unsigned int i=0; i < length; ++i ) {
550         if ( ( i % 4 ) == 0 ) {
551             if ( i > 0 ) {
552                 debugOutputShort(level,"\n");
553             }
554             debugOutputShort(level," %4d: ",i*4);
555         }
556         debugOutputShort(level,"%08X ",buffer[i]);
557     }
558     debugOutputShort(level,"\n");
559 }
560 void
561 Ieee1394Service::printBufferBytes( unsigned int level, size_t length, byte_t* buffer ) const
562 {
563
564     for ( unsigned int i=0; i < length; ++i ) {
565         if ( ( i % 16 ) == 0 ) {
566             if ( i > 0 ) {
567                 debugOutputShort(level,"\n");
568             }
569             debugOutputShort(level," %4d: ",i*16);
570         }
571         debugOutputShort(level,"%02X ",buffer[i]);
572     }
573     debugOutputShort(level,"\n");
574 }
575
576 int
577 Ieee1394Service::resetHandlerLowLevel( raw1394handle_t handle,
578                                        unsigned int generation )
579 {
580     raw1394_update_generation ( handle, generation );
581     Ieee1394Service* instance
582         = (Ieee1394Service*) raw1394_get_userdata( handle );
583     instance->resetHandler( generation );
584
585     return 0;
586 }
587
588 bool
589 Ieee1394Service::resetHandler( unsigned int generation )
590 {
591     quadlet_t buf=0;
592
593     // do a simple read on ourself in order to update the internal structures
594     // this avoids failures after a bus reset
595     read_quadlet( getLocalNodeId() | 0xFFC0,
596                   CSR_REGISTER_BASE | CSR_CYCLE_TIME,
597                   &buf );
598
599     for ( reset_handler_vec_t::iterator it = m_busResetHandlers.begin();
600           it != m_busResetHandlers.end();
601           ++it )
602     {
603         Functor* func = *it;
604         ( *func )();
605     }
606
607     return true;
608 }
609
610 bool Ieee1394Service::registerARMHandler(ARMHandler *h) {
611     debugOutput(DEBUG_LEVEL_VERBOSE, "Registering ARM handler (%p) for 0x%016llX, length %u\n",
612         h, h->getStart(), h->getLength());
613
614     int err=raw1394_arm_register(m_resetHandle, h->getStart(),
615                          h->getLength(), h->getBuffer(), (octlet_t)h,
616                          h->getAccessRights(),
617                          h->getNotificationOptions(),
618                          h->getClientTransactions());
619     if (err) {
620         debugError("Failed to register ARM handler for 0x%016llX\n", h->getStart());
621         debugError(" Error: %s\n", strerror(errno));
622         return false;
623     }
624
625     m_armHandlers.push_back( h );
626
627     return true;
628 }
629
630 bool Ieee1394Service::unregisterARMHandler( ARMHandler *h ) {
631     debugOutput(DEBUG_LEVEL_VERBOSE, "Unregistering ARM handler (%p) for 0x%016llX\n",
632         h, h->getStart());
633
634     for ( arm_handler_vec_t::iterator it = m_armHandlers.begin();
635           it != m_armHandlers.end();
636           ++it )
637     {
638         if((*it) == h) {
639             int err=raw1394_arm_unregister(m_resetHandle, h->getStart());
640             if (err) {
641                 debugError("Failed to unregister ARM handler (%p)\n", h);
642                 debugError(" Error: %s\n", strerror(errno));
643             } else {
644                 m_armHandlers.erase(it);
645                 return true;
646             }
647         }
648     }
649     debugOutput(DEBUG_LEVEL_VERBOSE, " handler not found!\n");
650
651     return false;
652 }
653 /**
654  * @brief Tries to find a free ARM address range
655  *
656  * @param start  address to start with
657  * @param length length of the block needed (bytes)
658  * @param step   step to use when searching (bytes)
659  * @return The base address that is free, and 0xFFFFFFFFFFFFFFFF when failed
660  */
661 nodeaddr_t Ieee1394Service::findFreeARMBlock( nodeaddr_t start, size_t length, size_t step ) {
662     debugOutput(DEBUG_LEVEL_VERBOSE, "Finding free ARM block of %d bytes, from 0x%016llX in steps of %d bytes\n",
663         length, start, step);
664
665     int cnt=0;
666     const int maxcnt=10;
667     int err=1;
668     while(err && cnt++ < maxcnt) {
669         // try to register
670         err=raw1394_arm_register(m_resetHandle, start, length, 0, 0, 0, 0, 0);
671
672         if (err) {
673             debugOutput(DEBUG_LEVEL_VERBOSE, " -> cannot use 0x%016llX\n", start);
674             debugError("    Error: %s\n", strerror(errno));
675             start += step;
676         } else {
677             debugOutput(DEBUG_LEVEL_VERBOSE, " -> use 0x%016llX\n", start);
678             err=raw1394_arm_unregister(m_resetHandle, start);
679             if (err) {
680                 debugOutput(DEBUG_LEVEL_VERBOSE, " error unregistering test handler\n");
681                 debugError("    Error: %s\n", strerror(errno));
682                 return 0xFFFFFFFFFFFFFFFFLLU;
683             }
684             return start;
685         }
686     }
687     debugOutput(DEBUG_LEVEL_VERBOSE, " Could not find free block in %d tries\n",cnt);
688     return 0xFFFFFFFFFFFFFFFFLLU;
689 }
690
691 int
692 Ieee1394Service::armHandlerLowLevel(raw1394handle_t handle,
693                      unsigned long arm_tag,
694                      byte_t request_type, unsigned int requested_length,
695                      void *data)
696 {
697     Ieee1394Service* instance
698         = (Ieee1394Service*) raw1394_get_userdata( handle );
699     instance->armHandler( arm_tag, request_type, requested_length, data );
700
701     return 0;
702 }
703
704 bool
705 Ieee1394Service::armHandler(  unsigned long arm_tag,
706                      byte_t request_type, unsigned int requested_length,
707                      void *data)
708 {
709     for ( arm_handler_vec_t::iterator it = m_armHandlers.begin();
710           it != m_armHandlers.end();
711           ++it )
712     {
713         if((*it) == (ARMHandler *)arm_tag) {
714             struct raw1394_arm_request_response *arm_req_resp;
715             arm_req_resp  = (struct raw1394_arm_request_response *) data;
716             raw1394_arm_request_t arm_req=arm_req_resp->request;
717             raw1394_arm_response_t arm_resp=arm_req_resp->response;
718
719             debugOutput(DEBUG_LEVEL_VERBOSE,"ARM handler for address 0x%016llX called\n",
720                 (*it)->getStart());
721             debugOutput(DEBUG_LEVEL_VERBOSE," request type   : 0x%02X\n",request_type);
722             debugOutput(DEBUG_LEVEL_VERBOSE," request length : %04d\n",requested_length);
723
724             switch(request_type) {
725                 case RAW1394_ARM_READ:
726                     (*it)->handleRead(arm_req);
727                     *arm_resp=*((*it)->getResponse());
728                     break;
729                 case RAW1394_ARM_WRITE:
730                     (*it)->handleWrite(arm_req);
731                     *arm_resp=*((*it)->getResponse());
732                     break;
733                 case RAW1394_ARM_LOCK:
734                     (*it)->handleLock(arm_req);
735                     *arm_resp=*((*it)->getResponse());
736                     break;
737                 default:
738                     debugWarning("Unknown request type received, ignoring...\n");
739             }
740
741             return true;
742         }
743     }
744
745     debugOutput(DEBUG_LEVEL_VERBOSE,"default ARM handler called\n");
746
747     m_default_arm_handler(m_resetHandle, arm_tag, request_type, requested_length, data );
748     return true;
749 }
750
751 bool
752 Ieee1394Service::startRHThread()
753 {
754     int i;
755
756     if ( m_threadRunning ) {
757         return true;
758     }
759     pthread_mutex_lock( &m_mutex );
760     i = pthread_create( &m_thread, 0, rHThread, this );
761     pthread_mutex_unlock( &m_mutex );
762     if (i) {
763         debugFatal("Could not start ieee1394 service thread\n");
764         return false;
765     }
766     m_threadRunning = true;
767
768     return true;
769 }
770
771 void
772 Ieee1394Service::stopRHThread()
773 {
774     if ( m_threadRunning ) {
775         pthread_mutex_lock (&m_mutex);
776         pthread_cancel (m_thread);
777         pthread_join (m_thread, 0);
778         pthread_mutex_unlock (&m_mutex);
779         m_threadRunning = false;
780     }
781 }
782
783 void*
784 Ieee1394Service::rHThread( void* arg )
785 {
786     Ieee1394Service* pIeee1394Service = (Ieee1394Service*) arg;
787
788     while (true) {
789         raw1394_loop_iterate (pIeee1394Service->m_resetHandle);
790         pthread_testcancel ();
791     }
792
793     return 0;
794 }
795
796 bool
797 Ieee1394Service::addBusResetHandler( Functor* functor )
798 {
799     debugOutput(DEBUG_LEVEL_VERBOSE, "Adding busreset handler (%p)\n", functor);
800     m_busResetHandlers.push_back( functor );
801     return true;
802 }
803
804 bool
805 Ieee1394Service::remBusResetHandler( Functor* functor )
806 {
807     debugOutput(DEBUG_LEVEL_VERBOSE, "Removing busreset handler (%p)\n", functor);
808
809     for ( reset_handler_vec_t::iterator it = m_busResetHandlers.begin();
810           it != m_busResetHandlers.end();
811           ++it )
812     {
813         if ( *it == functor ) {
814             debugOutput(DEBUG_LEVEL_VERBOSE, " found\n");
815             m_busResetHandlers.erase( it );
816             return true;
817         }
818     }
819     debugOutput(DEBUG_LEVEL_VERBOSE, " not found\n");
820     return false;
821 }
822
823 /**
824  * Allocates an iso channel for use by the interface in a similar way to
825  * libiec61883.  Returns -1 on error (due to there being no free channels)
826  * or an allocated channel number.
827  *
828  * Does not perform anything other than registering the channel and the
829  * bandwidth at the IRM
830  *
831  * Also allocates the necessary bandwidth (in ISO allocation units).
832  *
833  * FIXME: As in libiec61883, channel 63 is not requested; this is either a
834  * bug or it's omitted since that's the channel preferred by video devices.
835  *
836  * @param bandwidth the bandwidth to allocate for this channel
837  * @return the channel number
838  */
839 signed int Ieee1394Service::allocateIsoChannelGeneric(unsigned int bandwidth) {
840     debugOutput(DEBUG_LEVEL_VERBOSE, "Allocating ISO channel using generic method...\n" );
841
842     struct ChannelInfo cinfo;
843
844     int c = -1;
845     for (c = 0; c < 63; c++) {
846         if (raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_ALLOC) == 0)
847             break;
848     }
849     if (c < 63) {
850         if (raw1394_bandwidth_modify(m_handle, bandwidth, RAW1394_MODIFY_ALLOC) < 0) {
851             debugFatal("Could not allocate bandwidth of %d\n", bandwidth);
852
853             raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_FREE);
854             return -1;
855         } else {
856             cinfo.channel=c;
857             cinfo.bandwidth=bandwidth;
858             cinfo.alloctype=AllocGeneric;
859
860             cinfo.xmit_node=-1;
861             cinfo.xmit_plug=-1;
862             cinfo.recv_node=-1;
863             cinfo.recv_plug=-1;
864
865             if (registerIsoChannel(c, cinfo)) {
866                 return c;
867             } else {
868                 raw1394_bandwidth_modify(m_handle, bandwidth, RAW1394_MODIFY_FREE);
869                 raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_FREE);
870                 return -1;
871             }
872         }
873     }
874     return -1;
875 }
876
877 /**
878  * Allocates an iso channel for use by the interface in a similar way to
879  * libiec61883.  Returns -1 on error (due to there being no free channels)
880  * or an allocated channel number.
881  *
882  * Uses IEC61883 Connection Management Procedure to establish the connection.
883  *
884  * Also allocates the necessary bandwidth (in ISO allocation units).
885  *
886  * @param xmit_node  node id of the transmitter
887  * @param xmit_plug  the output plug to use. If -1, find the first online plug, and
888  * upon return, contains the plug number used.
889  * @param recv_node  node id of the receiver
890  * @param recv_plug the input plug to use. If -1, find the first online plug, and
891  * upon return, contains the plug number used.
892  *
893  * @return the channel number
894  */
895
896 signed int Ieee1394Service::allocateIsoChannelCMP(
897     nodeid_t xmit_node, int xmit_plug,
898     nodeid_t recv_node, int recv_plug
899     ) {
900
901     debugOutput(DEBUG_LEVEL_VERBOSE, "Allocating ISO channel using IEC61883 CMP...\n" );
902
903     struct ChannelInfo cinfo;
904
905     int c = -1;
906     int bandwidth=1;
907
908     // do connection management: make connection
909     c = iec61883_cmp_connect(
910         m_handle,
911         xmit_node | 0xffc0,
912         &xmit_plug,
913         recv_node | 0xffc0,
914         &recv_plug,
915         &bandwidth);
916
917     if((c<0) || (c>63)) {
918         debugError("Could not do CMP from %04X:%02d to %04X:%02d\n",
919             xmit_node, xmit_plug, recv_node, recv_plug
920             );
921         return -1;
922     }
923
924     cinfo.channel=c;
925     cinfo.bandwidth=bandwidth;
926     cinfo.alloctype=AllocCMP;
927
928     cinfo.xmit_node=xmit_node;
929     cinfo.xmit_plug=xmit_plug;
930     cinfo.recv_node=recv_node;
931     cinfo.recv_plug=recv_plug;
932
933     if (registerIsoChannel(c, cinfo)) {
934         return c;
935     }
936
937     return -1;
938 }
939
940 /**
941  * Deallocates an iso channel.  Silently ignores a request to deallocate
942  * a negative channel number.
943  *
944  * Figures out the method that was used to allocate the channel (generic, cmp, ...)
945  * and uses the appropriate method to deallocate. Also frees the bandwidth
946  * that was reserved along with this channel.
947  *
948  * @param c channel number
949  * @return true if successful
950  */
951 bool Ieee1394Service::freeIsoChannel(signed int c) {
952     debugOutput(DEBUG_LEVEL_VERBOSE, "Freeing ISO channel %d...\n", c );
953
954     if (c < 0 || c > 63) {
955         debugWarning("Invalid channel number: %d\n", c);
956         return false;
957     }
958
959     switch (m_channels[c].alloctype) {
960         default:
961             debugError(" BUG: invalid allocation type!\n");
962             return false;
963
964         case AllocFree:
965             debugWarning(" Channel %d not registered\n", c);
966             return false;
967
968         case AllocGeneric:
969             debugOutput(DEBUG_LEVEL_VERBOSE, " allocated using generic routine...\n" );
970             debugOutput(DEBUG_LEVEL_VERBOSE, " freeing %d bandwidth units...\n", m_channels[c].bandwidth );
971             if (raw1394_bandwidth_modify(m_handle, m_channels[c].bandwidth, RAW1394_MODIFY_FREE) !=0) {
972                 debugWarning("Failed to deallocate bandwidth\n");
973             }
974             debugOutput(DEBUG_LEVEL_VERBOSE, " freeing channel %d...\n", m_channels[c].channel );
975             if (raw1394_channel_modify (m_handle, m_channels[c].channel, RAW1394_MODIFY_FREE) != 0) {
976                 debugWarning("Failed to free channel\n");
977             }
978             if (!unregisterIsoChannel(c))
979                 return false;
980             return true;
981
982         case AllocCMP:
983             debugOutput(DEBUG_LEVEL_VERBOSE, " allocated using IEC61883 CMP...\n" );
984             debugOutput(DEBUG_LEVEL_VERBOSE, " performing IEC61883 CMP disconnect...\n" );
985             if(iec61883_cmp_disconnect(
986                     m_handle,
987                     m_channels[c].xmit_node | 0xffc0,
988                     m_channels[c].xmit_plug,
989                     m_channels[c].recv_node | 0xffc0,
990                     m_channels[c].recv_plug,
991                     m_channels[c].channel,
992                     m_channels[c].bandwidth) != 0) {
993                 debugWarning("Could not do CMP disconnect for channel %d!\n",c);
994             }
995             if (!unregisterIsoChannel(c))
996                 return false;
997             return true;
998     }
999
1000     // unreachable
1001     debugError("BUG: unreachable code reached!\n");
1002
1003     return false;
1004 }
1005
1006 /**
1007  * Registers a channel as managed by this ieee1394service
1008  * @param c channel number
1009  * @param cinfo channel info struct
1010  * @return true if successful
1011  */
1012 bool Ieee1394Service::registerIsoChannel(unsigned int c, struct ChannelInfo cinfo) {
1013     if (c < 63) {
1014         if (m_channels[c].alloctype != AllocFree) {
1015             debugWarning("Channel %d already registered with bandwidth %d\n",
1016                 m_channels[c].channel, m_channels[c].bandwidth);
1017         }
1018
1019         memcpy(&m_channels[c], &cinfo, sizeof(struct ChannelInfo));
1020
1021     } else return false;
1022     return true;
1023 }
1024
1025 /**
1026  * unegisters a channel from this ieee1394service
1027  * @param c channel number
1028  * @return true if successful
1029  */
1030 bool Ieee1394Service::unregisterIsoChannel(unsigned int c) {
1031     if (c < 63) {
1032         if (m_channels[c].alloctype == AllocFree) {
1033             debugWarning("Channel %d not registered\n", c);
1034             return false;
1035         }
1036
1037         m_channels[c].channel=-1;
1038         m_channels[c].bandwidth=-1;
1039         m_channels[c].alloctype=AllocFree;
1040         m_channels[c].xmit_node=0xFFFF;
1041         m_channels[c].xmit_plug=-1;
1042         m_channels[c].recv_node=0xFFFF;
1043         m_channels[c].recv_plug=-1;
1044
1045     } else return false;
1046     return true;
1047 }
1048
1049 /**
1050  * Returns the current value of the `bandwidth available' register on
1051  * the IRM, or -1 on error.
1052  * @return
1053  */
1054 signed int Ieee1394Service::getAvailableBandwidth() {
1055     quadlet_t buffer;
1056     signed int result = raw1394_read (m_handle, raw1394_get_irm_id (m_handle),
1057         CSR_REGISTER_BASE + CSR_BANDWIDTH_AVAILABLE,
1058         sizeof (quadlet_t), &buffer);
1059
1060     if (result < 0)
1061         return -1;
1062     return ntohl(buffer);
1063 }
1064
1065 void
1066 Ieee1394Service::setVerboseLevel(int l)
1067 {
1068     if (m_pIsoManager) m_pIsoManager->setVerboseLevel(l);
1069     if (m_pCTRHelper) m_pCTRHelper->setVerboseLevel(l);
1070     setDebugLevel(l);
1071     debugOutput( DEBUG_LEVEL_VERBOSE, "Setting verbose level to %d...\n", l );
1072 }
1073
1074 void
1075 Ieee1394Service::show()
1076 {
1077     debugOutput( DEBUG_LEVEL_VERBOSE, "Port:  %d\n", getPort() );
1078     debugOutput( DEBUG_LEVEL_VERBOSE, " Name: %s\n", getPortName().c_str() );
1079     debugOutputShort( DEBUG_LEVEL_NORMAL, "Iso handler info:\n");
1080     if (m_pIsoManager) m_pIsoManager->dumpInfo();
1081 }
Note: See TracBrowser for help on using the browser.