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

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

fix cycle timer DLL code

  • 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 "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.