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

Revision 909, 33.8 kB (checked in by ppalmers, 13 years ago)

fix port out of range bug

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