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

Revision 937, 34.2 kB (checked in by ppalmers, 16 years ago)

cleanup some compilation warnings

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