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

Revision 864, 33.7 kB (checked in by ppalmers, 16 years ago)

update license to GPLv2 or GPLv3 instead of GPLv2 or any later version. Update copyrights to reflect the new year

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