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

Revision 754, 32.4 kB (checked in by ppalmers, 13 years ago)

- simplify IsoHandler?
- fix some small issues

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