1 |
/* |
---|
2 |
* Copyright (C) 2005-2008 by Daniel Wagner |
---|
3 |
* Copyright (C) 2005-2008 by Pieter Palmers |
---|
4 |
* Copyright (C) 2012 by Jonathan Woithe |
---|
5 |
* |
---|
6 |
* This file is part of FFADO |
---|
7 |
* FFADO = Free Firewire (pro-)audio drivers for linux |
---|
8 |
* |
---|
9 |
* FFADO is based upon FreeBoB |
---|
10 |
* |
---|
11 |
* This program is free software: you can redistribute it and/or modify |
---|
12 |
* it under the terms of the GNU General Public License as published by |
---|
13 |
* the Free Software Foundation, either version 2 of the License, or |
---|
14 |
* (at your option) version 3 of the License. |
---|
15 |
* |
---|
16 |
* This program is distributed in the hope that it will be useful, |
---|
17 |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
---|
18 |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
---|
19 |
* GNU General Public License for more details. |
---|
20 |
* |
---|
21 |
* You should have received a copy of the GNU General Public License |
---|
22 |
* along with this program. If not, see <http://www.gnu.org/licenses/>. |
---|
23 |
* |
---|
24 |
*/ |
---|
25 |
|
---|
26 |
#include "config.h" |
---|
27 |
|
---|
28 |
#include "ieee1394service.h" |
---|
29 |
#include "cycletimer.h" |
---|
30 |
#include "IsoHandlerManager.h" |
---|
31 |
#include "CycleTimerHelper.h" |
---|
32 |
|
---|
33 |
#include <unistd.h> |
---|
34 |
#include <libraw1394/csr.h> |
---|
35 |
#include <libiec61883/iec61883.h> |
---|
36 |
|
---|
37 |
#include "libutil/SystemTimeSource.h" |
---|
38 |
#include "libutil/Watchdog.h" |
---|
39 |
#include "libutil/PosixMutex.h" |
---|
40 |
#include "libutil/PosixThread.h" |
---|
41 |
#include "libutil/Configuration.h" |
---|
42 |
|
---|
43 |
#include <errno.h> |
---|
44 |
#include "libutil/ByteSwap.h" |
---|
45 |
|
---|
46 |
#include <string.h> |
---|
47 |
|
---|
48 |
#include <iostream> |
---|
49 |
#include <iomanip> |
---|
50 |
|
---|
51 |
// Permit linking against older libraw1394 which didn't include this |
---|
52 |
// function. |
---|
53 |
#ifdef __GNUC__ |
---|
54 |
#ifdef __APPLE__ |
---|
55 |
#define WEAK_ATTRIBUTE weak_import |
---|
56 |
#else |
---|
57 |
#define WEAK_ATTRIBUTE __weak__ |
---|
58 |
#endif |
---|
59 |
int raw1394_read_cycle_timer_and_clock(raw1394handle_t handle, |
---|
60 |
u_int32_t *cycle_timer, u_int64_t *local_time, clockid_t clk_id) |
---|
61 |
__attribute__((WEAK_ATTRIBUTE)); |
---|
62 |
#endif |
---|
63 |
|
---|
64 |
using namespace std; |
---|
65 |
|
---|
66 |
IMPL_DEBUG_MODULE( Ieee1394Service, Ieee1394Service, DEBUG_LEVEL_NORMAL ); |
---|
67 |
|
---|
68 |
Ieee1394Service::Ieee1394Service() |
---|
69 |
: m_configuration( NULL ) |
---|
70 |
, m_resetHelper( NULL ) |
---|
71 |
, m_armHelperNormal( NULL ) |
---|
72 |
, m_armHelperRealtime( NULL ) |
---|
73 |
, m_handle( 0 ) |
---|
74 |
, m_handle_lock( new Util::PosixMutex("SRVCHND") ) |
---|
75 |
, m_util_handle( 0 ) |
---|
76 |
, m_port( -1 ) |
---|
77 |
, m_realtime ( false ) |
---|
78 |
, m_base_priority ( 0 ) |
---|
79 |
, m_pIsoManager( new IsoHandlerManager( *this ) ) |
---|
80 |
, m_pCTRHelper ( new CycleTimerHelper( *this, IEEE1394SERVICE_CYCLETIMER_DLL_UPDATE_INTERVAL_USEC ) ) |
---|
81 |
, m_have_new_ctr_read ( false ) |
---|
82 |
, m_filterFCPResponse ( false ) |
---|
83 |
, m_pWatchdog ( new Util::Watchdog() ) |
---|
84 |
{ |
---|
85 |
for (unsigned int i=0; i<64; i++) { |
---|
86 |
m_channels[i].channel=-1; |
---|
87 |
m_channels[i].bandwidth=-1; |
---|
88 |
m_channels[i].alloctype=AllocFree; |
---|
89 |
m_channels[i].xmit_node=0xFFFF; |
---|
90 |
m_channels[i].xmit_plug=-1; |
---|
91 |
m_channels[i].recv_node=0xFFFF; |
---|
92 |
m_channels[i].recv_plug=-1; |
---|
93 |
} |
---|
94 |
} |
---|
95 |
|
---|
96 |
Ieee1394Service::Ieee1394Service(bool rt, int prio) |
---|
97 |
: m_configuration( NULL ) |
---|
98 |
, m_resetHelper( NULL ) |
---|
99 |
, m_armHelperNormal( NULL ) |
---|
100 |
, m_armHelperRealtime( NULL ) |
---|
101 |
, m_handle( 0 ) |
---|
102 |
, m_handle_lock( new Util::PosixMutex("SRVCHND") ) |
---|
103 |
, m_util_handle( 0 ) |
---|
104 |
, m_port( -1 ) |
---|
105 |
, m_realtime ( rt ) |
---|
106 |
, m_base_priority ( prio ) |
---|
107 |
, m_pIsoManager( new IsoHandlerManager( *this, rt, prio ) ) |
---|
108 |
, m_pCTRHelper ( new CycleTimerHelper( *this, IEEE1394SERVICE_CYCLETIMER_DLL_UPDATE_INTERVAL_USEC, |
---|
109 |
rt && IEEE1394SERVICE_CYCLETIMER_HELPER_RUN_REALTIME, |
---|
110 |
IEEE1394SERVICE_CYCLETIMER_HELPER_PRIO ) ) |
---|
111 |
, m_have_new_ctr_read ( false ) |
---|
112 |
, m_filterFCPResponse ( false ) |
---|
113 |
, m_pWatchdog ( new Util::Watchdog() ) |
---|
114 |
{ |
---|
115 |
for (unsigned int i=0; i<64; i++) { |
---|
116 |
m_channels[i].channel=-1; |
---|
117 |
m_channels[i].bandwidth=-1; |
---|
118 |
m_channels[i].alloctype=AllocFree; |
---|
119 |
m_channels[i].xmit_node=0xFFFF; |
---|
120 |
m_channels[i].xmit_plug=-1; |
---|
121 |
m_channels[i].recv_node=0xFFFF; |
---|
122 |
m_channels[i].recv_plug=-1; |
---|
123 |
} |
---|
124 |
} |
---|
125 |
|
---|
126 |
Ieee1394Service::~Ieee1394Service() |
---|
127 |
{ |
---|
128 |
delete m_pIsoManager; |
---|
129 |
delete m_pCTRHelper; |
---|
130 |
|
---|
131 |
m_resetHelper->Stop(); |
---|
132 |
m_armHelperNormal->Stop(); |
---|
133 |
m_armHelperRealtime->Stop(); |
---|
134 |
|
---|
135 |
for ( arm_handler_vec_t::iterator it = m_armHandlers.begin(); |
---|
136 |
it != m_armHandlers.end(); |
---|
137 |
++it ) |
---|
138 |
{ |
---|
139 |
debugOutput(DEBUG_LEVEL_VERBOSE, "Unregistering ARM handler for 0x%016" PRIX64 "\n", (*it)->getStart()); |
---|
140 |
if(m_armHelperNormal) { |
---|
141 |
int err = raw1394_arm_unregister(m_armHelperNormal->get1394Handle(), (*it)->getStart()); |
---|
142 |
if (err) { |
---|
143 |
debugError(" Failed to unregister ARM handler for 0x%016" PRIX64 "\n", (*it)->getStart()); |
---|
144 |
debugError(" Error: %s\n", strerror(errno)); |
---|
145 |
} |
---|
146 |
} else { |
---|
147 |
debugWarning("ARM handler registered without valid ARM helper thread\n"); |
---|
148 |
} |
---|
149 |
} |
---|
150 |
|
---|
151 |
delete m_pWatchdog; |
---|
152 |
if ( m_handle ) { |
---|
153 |
raw1394_destroy_handle( m_handle ); |
---|
154 |
} |
---|
155 |
delete m_handle_lock; |
---|
156 |
|
---|
157 |
if(m_resetHelper) delete m_resetHelper; |
---|
158 |
if(m_armHelperNormal) delete m_armHelperNormal; |
---|
159 |
if(m_armHelperRealtime) delete m_armHelperRealtime; |
---|
160 |
|
---|
161 |
if ( m_util_handle ) { |
---|
162 |
raw1394_destroy_handle( m_util_handle ); |
---|
163 |
} |
---|
164 |
} |
---|
165 |
|
---|
166 |
bool |
---|
167 |
Ieee1394Service::useConfiguration(Util::Configuration *c) |
---|
168 |
{ |
---|
169 |
m_configuration = c; |
---|
170 |
return configurationUpdated(); |
---|
171 |
} |
---|
172 |
|
---|
173 |
bool |
---|
174 |
Ieee1394Service::configurationUpdated() |
---|
175 |
{ |
---|
176 |
if(m_configuration) { |
---|
177 |
|
---|
178 |
} |
---|
179 |
return true; |
---|
180 |
} |
---|
181 |
|
---|
182 |
#define DEVICEFAILTEXT "Could not get libraw1394 handle.\n\ |
---|
183 |
This usually means:\n\ |
---|
184 |
a) The device-node /dev/raw1394 doesn't exists because you don't have a\n\ |
---|
185 |
(recognized) firewire controller.\n \ |
---|
186 |
b) The modules needed aren't loaded. This is not in the scope of ffado but of\n\ |
---|
187 |
your distribution, so if you have a firewire controller that should be\n\ |
---|
188 |
supported and the modules aren't loaded, file a bug with your distributions\n\ |
---|
189 |
bug tracker.\n \ |
---|
190 |
c) You don't have permissions to access /dev/raw1394. 'ls -l /dev/raw1394'\n\ |
---|
191 |
shows the device-node with its permissions, make sure you belong to the\n\ |
---|
192 |
right group and the group is allowed to access the device.\n" |
---|
193 |
|
---|
194 |
int |
---|
195 |
Ieee1394Service::detectNbPorts() |
---|
196 |
{ |
---|
197 |
raw1394handle_t tmp_handle = raw1394_new_handle(); |
---|
198 |
if ( tmp_handle == NULL ) { |
---|
199 |
debugError(DEVICEFAILTEXT); |
---|
200 |
return -1; |
---|
201 |
} |
---|
202 |
struct raw1394_portinfo pinf[IEEE1394SERVICE_MAX_FIREWIRE_PORTS]; |
---|
203 |
int nb_detected_ports = raw1394_get_port_info(tmp_handle, pinf, IEEE1394SERVICE_MAX_FIREWIRE_PORTS); |
---|
204 |
raw1394_destroy_handle(tmp_handle); |
---|
205 |
|
---|
206 |
if (nb_detected_ports < 0) { |
---|
207 |
debugError("Failed to detect number of ports\n"); |
---|
208 |
return -1; |
---|
209 |
} |
---|
210 |
return nb_detected_ports; |
---|
211 |
} |
---|
212 |
|
---|
213 |
void |
---|
214 |
Ieee1394Service::doBusReset() { |
---|
215 |
debugOutput(DEBUG_LEVEL_VERBOSE, "Issue bus reset on service %p (port %d).\n", this, getPort()); |
---|
216 |
raw1394_reset_bus(m_handle); |
---|
217 |
} |
---|
218 |
|
---|
219 |
/** |
---|
220 |
* This function waits until there are no bus resets generated in a sleep_time_ms interval |
---|
221 |
* @param nb_tries number of tries to take |
---|
222 |
* @param sleep_time_ms sleep between tries |
---|
223 |
* @return true if the storm passed |
---|
224 |
*/ |
---|
225 |
bool |
---|
226 |
Ieee1394Service::waitForBusResetStormToEnd( int nb_tries, int sleep_time_ms ) { |
---|
227 |
unsigned int gen_current; |
---|
228 |
do { |
---|
229 |
gen_current = getGeneration(); |
---|
230 |
debugOutput(DEBUG_LEVEL_VERBOSE, "Waiting... (gen: %u)\n", gen_current); |
---|
231 |
|
---|
232 |
// wait for a while |
---|
233 |
Util::SystemTimeSource::SleepUsecRelative( sleep_time_ms * 1000); |
---|
234 |
} while (gen_current != getGeneration() && --nb_tries); |
---|
235 |
|
---|
236 |
debugOutput(DEBUG_LEVEL_VERBOSE, "Bus reset storm over at gen: %u\n", gen_current); |
---|
237 |
|
---|
238 |
if (!nb_tries) { |
---|
239 |
debugError( "Bus reset storm did not stop on time...\n"); |
---|
240 |
return false; |
---|
241 |
} |
---|
242 |
return true; |
---|
243 |
} |
---|
244 |
|
---|
245 |
bool |
---|
246 |
Ieee1394Service::initialize( int port ) |
---|
247 |
{ |
---|
248 |
using namespace std; |
---|
249 |
|
---|
250 |
int nb_ports = detectNbPorts(); |
---|
251 |
if (port + 1 > nb_ports) { |
---|
252 |
debugFatal("Requested port (%d) out of range (# ports: %d)\n", port, nb_ports); |
---|
253 |
} |
---|
254 |
m_port = port; |
---|
255 |
|
---|
256 |
if(!m_pWatchdog) { |
---|
257 |
debugError("No valid RT watchdog found.\n"); |
---|
258 |
return false; |
---|
259 |
} |
---|
260 |
if(!m_pWatchdog->start()) { |
---|
261 |
debugError("Could not start RT watchdog.\n"); |
---|
262 |
return false; |
---|
263 |
} |
---|
264 |
|
---|
265 |
m_handle = raw1394_new_handle_on_port( port ); |
---|
266 |
if ( !m_handle ) { |
---|
267 |
if ( !errno ) { |
---|
268 |
debugFatal("libraw1394 not compatible\n"); |
---|
269 |
} else { |
---|
270 |
debugFatal("Ieee1394Service::initialize: Could not get 1394 handle: %s\n", |
---|
271 |
strerror(errno) ); |
---|
272 |
debugFatal("Is ieee1394 and raw1394 driver loaded?\n"); |
---|
273 |
} |
---|
274 |
return false; |
---|
275 |
} |
---|
276 |
|
---|
277 |
// helper threads for all sorts of ASYNC events |
---|
278 |
// note: m_port has to be set! |
---|
279 |
m_resetHelper = new HelperThread(*this, "BUSRST"); |
---|
280 |
if ( !m_resetHelper ) { |
---|
281 |
debugFatal("Could not allocate busreset handler helper\n"); |
---|
282 |
return false; |
---|
283 |
} |
---|
284 |
m_armHelperNormal = new HelperThread(*this, "ARMSTD"); |
---|
285 |
if ( !m_armHelperNormal ) { |
---|
286 |
debugFatal("Could not allocate standard ARM handler helper\n"); |
---|
287 |
return false; |
---|
288 |
} |
---|
289 |
m_armHelperRealtime = new HelperThread(*this, "ARMRT", m_realtime, m_base_priority); |
---|
290 |
if ( !m_armHelperRealtime ) { |
---|
291 |
debugFatal("Could not allocate realtime ARM handler helper\n"); |
---|
292 |
return false; |
---|
293 |
} |
---|
294 |
|
---|
295 |
// start helper threads |
---|
296 |
if(!m_resetHelper->Start()) { |
---|
297 |
debugFatal("Could not start busreset helper thread\n"); |
---|
298 |
return false; |
---|
299 |
} |
---|
300 |
if(!m_armHelperNormal->Start()) { |
---|
301 |
debugFatal("Could not start standard ARM helper thread\n"); |
---|
302 |
return false; |
---|
303 |
} |
---|
304 |
if(!m_armHelperRealtime->Start()) { |
---|
305 |
debugFatal("Could not start realtime ARM helper thread\n"); |
---|
306 |
return false; |
---|
307 |
} |
---|
308 |
|
---|
309 |
// attach the reset and ARM handlers |
---|
310 |
// NOTE: the handlers have to be started first, or there is no 1394handle |
---|
311 |
raw1394_set_bus_reset_handler( m_resetHelper->get1394Handle(), |
---|
312 |
this->resetHandlerLowLevel ); |
---|
313 |
|
---|
314 |
m_default_arm_handler = raw1394_set_arm_tag_handler( m_armHelperNormal->get1394Handle(), |
---|
315 |
this->armHandlerLowLevel ); |
---|
316 |
|
---|
317 |
// utility handle (used to read the CTR register) |
---|
318 |
m_util_handle = raw1394_new_handle_on_port( port ); |
---|
319 |
if ( !m_util_handle ) { |
---|
320 |
if ( !errno ) { |
---|
321 |
debugFatal("libraw1394 not compatible\n"); |
---|
322 |
} else { |
---|
323 |
debugFatal("Ieee1394Service::initialize: Could not get 1394 handle: %s", |
---|
324 |
strerror(errno) ); |
---|
325 |
debugFatal("Is ieee1394 and raw1394 driver loaded?\n"); |
---|
326 |
} |
---|
327 |
return false; |
---|
328 |
} |
---|
329 |
|
---|
330 |
// test the cycle timer read function |
---|
331 |
int err; |
---|
332 |
uint32_t cycle_timer; |
---|
333 |
uint64_t local_time; |
---|
334 |
m_have_read_ctr_and_clock = false; |
---|
335 |
err = raw1394_read_cycle_timer(m_util_handle, &cycle_timer, &local_time); |
---|
336 |
if(err) { |
---|
337 |
debugOutput(DEBUG_LEVEL_VERBOSE, "raw1394_read_cycle_timer failed.\n"); |
---|
338 |
debugOutput(DEBUG_LEVEL_VERBOSE, " Error descr: %s\n", strerror(err)); |
---|
339 |
debugWarning("==================================================================\n"); |
---|
340 |
debugWarning(" This system doesn't support the raw1394_read_cycle_timer call. \n"); |
---|
341 |
debugWarning(" Fallback to indirect CTR read method. \n"); |
---|
342 |
debugWarning(" FFADO should work, but achieving low-latency might be a problem. \n"); |
---|
343 |
debugWarning(" Upgrade the kernel to version 2.6.21 or higher to solve this. \n"); |
---|
344 |
debugWarning("==================================================================\n"); |
---|
345 |
m_have_new_ctr_read = false; |
---|
346 |
} else { |
---|
347 |
m_have_new_ctr_read = true; |
---|
348 |
|
---|
349 |
// Only if raw1394_read_cycle_timer() is present is it worth even |
---|
350 |
// considering the option that raw1394_read_cycle_timer_and_clock() |
---|
351 |
// might be available. |
---|
352 |
if (raw1394_read_cycle_timer_and_clock != NULL) { |
---|
353 |
err = raw1394_read_cycle_timer_and_clock(m_util_handle, &cycle_timer, &local_time, CLOCK_MONOTONIC); |
---|
354 |
if (!err && Util::SystemTimeSource::setSource(CLOCK_MONOTONIC)==true) |
---|
355 |
m_have_read_ctr_and_clock = true; |
---|
356 |
} |
---|
357 |
|
---|
358 |
if (m_have_read_ctr_and_clock) { |
---|
359 |
debugOutput(DEBUG_LEVEL_VERBOSE, "This system supports the raw1394_read_cycle_timer_and_clock call and the\n"); |
---|
360 |
debugOutput(DEBUG_LEVEL_VERBOSE, "CLOCK_MONOTONIC clock source; using them.\n"); |
---|
361 |
} else { |
---|
362 |
debugOutput(DEBUG_LEVEL_VERBOSE, "This system supports the raw1394_read_cycle_timer call, using it.\n"); |
---|
363 |
debugOutput(DEBUG_LEVEL_NORMAL, "The raw1394_read_cycle_timer_and_clock call and/or the CLOCK_MONOTONIC\n"); |
---|
364 |
debugOutput(DEBUG_LEVEL_NORMAL, "clock source is not available.\n"); |
---|
365 |
debugOutput(DEBUG_LEVEL_NORMAL, "Fallback to raw1394_read_cycle_timer.\n"); |
---|
366 |
debugOutput(DEBUG_LEVEL_NORMAL, "FFADO may be susceptible to NTP-induced clock discontinuities.\n"); |
---|
367 |
debugOutput(DEBUG_LEVEL_NORMAL, "If this is an issue, upgrade libraw1394 to version 2.1.0 or later and/or\n"); |
---|
368 |
debugOutput(DEBUG_LEVEL_NORMAL, "kernel 2.6.36 or later.\n"); |
---|
369 |
} |
---|
370 |
} |
---|
371 |
|
---|
372 |
// obtain port name |
---|
373 |
raw1394handle_t tmp_handle = raw1394_new_handle(); |
---|
374 |
if ( tmp_handle == NULL ) { |
---|
375 |
debugError("Could not get temporary libraw1394 handle.\n"); |
---|
376 |
return false; |
---|
377 |
} |
---|
378 |
struct raw1394_portinfo pinf[IEEE1394SERVICE_MAX_FIREWIRE_PORTS]; |
---|
379 |
int nb_detected_ports = raw1394_get_port_info(tmp_handle, pinf, IEEE1394SERVICE_MAX_FIREWIRE_PORTS); |
---|
380 |
raw1394_destroy_handle(tmp_handle); |
---|
381 |
|
---|
382 |
if (nb_detected_ports < 0) { |
---|
383 |
debugError("Failed to detect number of ports\n"); |
---|
384 |
return false; |
---|
385 |
} |
---|
386 |
|
---|
387 |
if(nb_detected_ports && port < IEEE1394SERVICE_MAX_FIREWIRE_PORTS) { |
---|
388 |
m_portName = pinf[port].name; |
---|
389 |
} else { |
---|
390 |
m_portName = "Unknown"; |
---|
391 |
} |
---|
392 |
if (m_portName == "") { |
---|
393 |
m_portName = "Unknown"; |
---|
394 |
} |
---|
395 |
|
---|
396 |
// set userdata |
---|
397 |
raw1394_set_userdata( m_handle, this ); |
---|
398 |
raw1394_set_userdata( m_util_handle, this ); |
---|
399 |
|
---|
400 |
// increase the split-transaction timeout if required (e.g. for bebob's) |
---|
401 |
int split_timeout = IEEE1394SERVICE_MIN_SPLIT_TIMEOUT_USECS; |
---|
402 |
if(m_configuration) { |
---|
403 |
m_configuration->getValueForSetting("ieee1394.min_split_timeout_usecs", split_timeout); |
---|
404 |
} |
---|
405 |
|
---|
406 |
// set SPLIT_TIMEOUT to one second to cope with DM1x00 devices that |
---|
407 |
// send responses regardless of the timeout |
---|
408 |
int timeout = getSplitTimeoutUsecs(getLocalNodeId()); |
---|
409 |
debugOutput(DEBUG_LEVEL_VERBOSE, "Minimum SPLIT_TIMEOUT: %d. Current: %d\n", split_timeout, timeout); |
---|
410 |
if (timeout < split_timeout) { |
---|
411 |
if(!setSplitTimeoutUsecs(getLocalNodeId(), split_timeout+124)) { |
---|
412 |
debugOutput(DEBUG_LEVEL_VERBOSE, "Could not set SPLIT_TIMEOUT to min requested (%d)\n", split_timeout); |
---|
413 |
} |
---|
414 |
timeout = getSplitTimeoutUsecs(getLocalNodeId()); |
---|
415 |
if (timeout < split_timeout) { |
---|
416 |
debugOutput(DEBUG_LEVEL_VERBOSE, "Set SPLIT_TIMEOUT to min requested (%d) did not succeed\n", split_timeout); |
---|
417 |
} |
---|
418 |
} |
---|
419 |
|
---|
420 |
// init helpers |
---|
421 |
if(!m_pCTRHelper) { |
---|
422 |
debugFatal("No CycleTimerHelper available, bad!\n"); |
---|
423 |
return false; |
---|
424 |
} |
---|
425 |
m_pCTRHelper->setVerboseLevel(getDebugLevel()); |
---|
426 |
if(!m_pCTRHelper->Start()) { |
---|
427 |
debugFatal("Could not start CycleTimerHelper\n"); |
---|
428 |
return false; |
---|
429 |
} |
---|
430 |
|
---|
431 |
if(!m_pIsoManager) { |
---|
432 |
debugFatal("No IsoHandlerManager available, bad!\n"); |
---|
433 |
return false; |
---|
434 |
} |
---|
435 |
m_pIsoManager->setVerboseLevel(getDebugLevel()); |
---|
436 |
|
---|
437 |
if(!m_pIsoManager->init()) { |
---|
438 |
debugFatal("Could not initialize IsoHandlerManager\n"); |
---|
439 |
return false; |
---|
440 |
} |
---|
441 |
|
---|
442 |
// make sure that the thread parameters of all our helper threads are OK |
---|
443 |
if(!setThreadParameters(m_realtime, m_base_priority)) { |
---|
444 |
debugFatal("Could not set thread parameters\n"); |
---|
445 |
return false; |
---|
446 |
} |
---|
447 |
return true; |
---|
448 |
} |
---|
449 |
|
---|
450 |
bool |
---|
451 |
Ieee1394Service::setThreadParameters(bool rt, int priority) { |
---|
452 |
bool result = true; |
---|
453 |
if (priority > THREAD_MAX_RTPRIO) priority = THREAD_MAX_RTPRIO; |
---|
454 |
if (priority < THREAD_MIN_RTPRIO) priority = THREAD_MIN_RTPRIO; |
---|
455 |
m_base_priority = priority; |
---|
456 |
m_realtime = rt; |
---|
457 |
if (m_pIsoManager) { |
---|
458 |
debugOutput(DEBUG_LEVEL_VERBOSE, "Switching IsoManager to (rt=%d, prio=%d)\n", |
---|
459 |
rt, priority); |
---|
460 |
result &= m_pIsoManager->setThreadParameters(rt, priority); |
---|
461 |
} //else debugError("Bogus isomanager\n"); |
---|
462 |
if (m_pCTRHelper) { |
---|
463 |
debugOutput(DEBUG_LEVEL_VERBOSE, "Switching CycleTimerHelper to (rt=%d, prio=%d)\n", |
---|
464 |
rt && IEEE1394SERVICE_CYCLETIMER_HELPER_RUN_REALTIME, |
---|
465 |
IEEE1394SERVICE_CYCLETIMER_HELPER_PRIO); |
---|
466 |
result &= m_pCTRHelper->setThreadParameters(rt && IEEE1394SERVICE_CYCLETIMER_HELPER_RUN_REALTIME, |
---|
467 |
IEEE1394SERVICE_CYCLETIMER_HELPER_PRIO); |
---|
468 |
} //else debugError("Bogus CTR helper\n"); |
---|
469 |
if(m_armHelperRealtime) { |
---|
470 |
m_armHelperRealtime->setThreadParameters(rt, priority); |
---|
471 |
} //else debugError("Bogus RT ARM helper\n"); |
---|
472 |
return result; |
---|
473 |
} |
---|
474 |
|
---|
475 |
int |
---|
476 |
Ieee1394Service::getNodeCount() |
---|
477 |
{ |
---|
478 |
Util::MutexLockHelper lock(*m_handle_lock); |
---|
479 |
return raw1394_get_nodecount( m_handle ); |
---|
480 |
} |
---|
481 |
|
---|
482 |
nodeid_t Ieee1394Service::getLocalNodeId() { |
---|
483 |
Util::MutexLockHelper lock(*m_handle_lock); |
---|
484 |
return raw1394_get_local_id(m_handle) & 0x3F; |
---|
485 |
} |
---|
486 |
|
---|
487 |
/** |
---|
488 |
* Returns the current value of the cycle timer (in ticks) |
---|
489 |
* |
---|
490 |
* @return the current value of the cycle timer (in ticks) |
---|
491 |
*/ |
---|
492 |
|
---|
493 |
uint32_t |
---|
494 |
Ieee1394Service::getCycleTimerTicks() { |
---|
495 |
return m_pCTRHelper->getCycleTimerTicks(); |
---|
496 |
} |
---|
497 |
|
---|
498 |
/** |
---|
499 |
* Returns the current value of the cycle timer (as is) |
---|
500 |
* |
---|
501 |
* @return the current value of the cycle timer (as is) |
---|
502 |
*/ |
---|
503 |
uint32_t |
---|
504 |
Ieee1394Service::getCycleTimer() { |
---|
505 |
return m_pCTRHelper->getCycleTimer(); |
---|
506 |
} |
---|
507 |
|
---|
508 |
/** |
---|
509 |
* Returns the current value of the cycle timer (in ticks) |
---|
510 |
* for a specific time instant (usecs since epoch) |
---|
511 |
* @return the current value of the cycle timer (in ticks) |
---|
512 |
*/ |
---|
513 |
|
---|
514 |
uint32_t |
---|
515 |
Ieee1394Service::getCycleTimerTicks(uint64_t t) { |
---|
516 |
return m_pCTRHelper->getCycleTimerTicks(t); |
---|
517 |
} |
---|
518 |
|
---|
519 |
/** |
---|
520 |
* Returns the current value of the cycle timer (as is) |
---|
521 |
* for a specific time instant (usecs since epoch) |
---|
522 |
* @return the current value of the cycle timer (as is) |
---|
523 |
*/ |
---|
524 |
uint32_t |
---|
525 |
Ieee1394Service::getCycleTimer(uint64_t t) { |
---|
526 |
return m_pCTRHelper->getCycleTimer(t); |
---|
527 |
} |
---|
528 |
|
---|
529 |
uint64_t |
---|
530 |
Ieee1394Service::getSystemTimeForCycleTimerTicks(uint32_t ticks) { |
---|
531 |
return m_pCTRHelper->getSystemTimeForCycleTimerTicks(ticks); |
---|
532 |
} |
---|
533 |
|
---|
534 |
uint64_t |
---|
535 |
Ieee1394Service::getSystemTimeForCycleTimer(uint32_t ctr) { |
---|
536 |
return m_pCTRHelper->getSystemTimeForCycleTimer(ctr); |
---|
537 |
} |
---|
538 |
|
---|
539 |
bool |
---|
540 |
Ieee1394Service::readCycleTimerReg(uint32_t *cycle_timer, uint64_t *local_time) |
---|
541 |
{ |
---|
542 |
if (m_have_read_ctr_and_clock) { |
---|
543 |
int err; |
---|
544 |
err = raw1394_read_cycle_timer_and_clock(m_util_handle, cycle_timer, local_time, |
---|
545 |
Util::SystemTimeSource::getSource()); |
---|
546 |
if(err) { |
---|
547 |
debugWarning("raw1394_read_cycle_timer_and_clock error: %s\n", strerror(errno)); |
---|
548 |
return false; |
---|
549 |
} |
---|
550 |
return true; |
---|
551 |
} else |
---|
552 |
if(m_have_new_ctr_read) { |
---|
553 |
int err; |
---|
554 |
err = raw1394_read_cycle_timer(m_util_handle, cycle_timer, local_time); |
---|
555 |
if(err) { |
---|
556 |
debugWarning("raw1394_read_cycle_timer error: %s\n", strerror(errno)); |
---|
557 |
return false; |
---|
558 |
} |
---|
559 |
return true; |
---|
560 |
} else { |
---|
561 |
// do a normal read of the CTR register |
---|
562 |
// the disadvantage is that local_time and cycle time are not |
---|
563 |
// read at the same time instant (scheduling issues) |
---|
564 |
*local_time = getCurrentTimeAsUsecs(); |
---|
565 |
if ( raw1394_read( m_util_handle, |
---|
566 |
getLocalNodeId() | 0xFFC0, |
---|
567 |
CSR_REGISTER_BASE | CSR_CYCLE_TIME, |
---|
568 |
sizeof(uint32_t), cycle_timer ) == 0 ) { |
---|
569 |
*cycle_timer = CondSwapFromBus32(*cycle_timer); |
---|
570 |
return true; |
---|
571 |
} else { |
---|
572 |
return false; |
---|
573 |
} |
---|
574 |
} |
---|
575 |
} |
---|
576 |
|
---|
577 |
uint64_t |
---|
578 |
Ieee1394Service::getCurrentTimeAsUsecs() { |
---|
579 |
return Util::SystemTimeSource::getCurrentTimeAsUsecs(); |
---|
580 |
} |
---|
581 |
|
---|
582 |
bool |
---|
583 |
Ieee1394Service::read( fb_nodeid_t nodeId, |
---|
584 |
fb_nodeaddr_t addr, |
---|
585 |
size_t length, |
---|
586 |
fb_quadlet_t* buffer ) |
---|
587 |
{ |
---|
588 |
Util::MutexLockHelper lock(*m_handle_lock); |
---|
589 |
return readNoLock(nodeId, addr, length, buffer); |
---|
590 |
} |
---|
591 |
|
---|
592 |
bool |
---|
593 |
Ieee1394Service::readNoLock( fb_nodeid_t nodeId, |
---|
594 |
fb_nodeaddr_t addr, |
---|
595 |
size_t length, |
---|
596 |
fb_quadlet_t* buffer ) |
---|
597 |
{ |
---|
598 |
if (nodeId == INVALID_NODE_ID) { |
---|
599 |
debugWarning("operation on invalid node\n"); |
---|
600 |
return false; |
---|
601 |
} |
---|
602 |
if ( raw1394_read( m_handle, nodeId, addr, length*4, buffer ) == 0 ) { |
---|
603 |
|
---|
604 |
#ifdef DEBUG |
---|
605 |
debugOutput(DEBUG_LEVEL_VERY_VERBOSE, |
---|
606 |
"read: node 0x%hX, addr = 0x%016" PRIX64 ", length = %zd\n", |
---|
607 |
nodeId, addr, length); |
---|
608 |
printBuffer( DEBUG_LEVEL_VERY_VERBOSE, length, buffer ); |
---|
609 |
#endif |
---|
610 |
|
---|
611 |
return true; |
---|
612 |
} else { |
---|
613 |
#ifdef DEBUG |
---|
614 |
debugOutput(DEBUG_LEVEL_VERBOSE, |
---|
615 |
"raw1394_read failed: node 0x%hX, addr = 0x%016" PRIX64 ", length = %zd\n", |
---|
616 |
nodeId, addr, length); |
---|
617 |
#endif |
---|
618 |
return false; |
---|
619 |
} |
---|
620 |
} |
---|
621 |
|
---|
622 |
bool |
---|
623 |
Ieee1394Service::read_quadlet( fb_nodeid_t nodeId, |
---|
624 |
fb_nodeaddr_t addr, |
---|
625 |
fb_quadlet_t* buffer ) |
---|
626 |
{ |
---|
627 |
return read( nodeId, addr, sizeof( *buffer )/4, buffer ); |
---|
628 |
} |
---|
629 |
|
---|
630 |
bool |
---|
631 |
Ieee1394Service::read_octlet( fb_nodeid_t nodeId, |
---|
632 |
fb_nodeaddr_t addr, |
---|
633 |
fb_octlet_t* buffer ) |
---|
634 |
{ |
---|
635 |
return read( nodeId, addr, sizeof( *buffer )/4, |
---|
636 |
reinterpret_cast<fb_quadlet_t*>( buffer ) ); |
---|
637 |
} |
---|
638 |
|
---|
639 |
bool |
---|
640 |
Ieee1394Service::write( fb_nodeid_t nodeId, |
---|
641 |
fb_nodeaddr_t addr, |
---|
642 |
size_t length, |
---|
643 |
fb_quadlet_t* data ) |
---|
644 |
{ |
---|
645 |
Util::MutexLockHelper lock(*m_handle_lock); |
---|
646 |
return writeNoLock(nodeId, addr, length, data); |
---|
647 |
} |
---|
648 |
|
---|
649 |
bool |
---|
650 |
Ieee1394Service::writeNoLock( fb_nodeid_t nodeId, |
---|
651 |
fb_nodeaddr_t addr, |
---|
652 |
size_t length, |
---|
653 |
fb_quadlet_t* data ) |
---|
654 |
{ |
---|
655 |
if (nodeId == INVALID_NODE_ID) { |
---|
656 |
debugWarning("operation on invalid node\n"); |
---|
657 |
return false; |
---|
658 |
} |
---|
659 |
|
---|
660 |
#ifdef DEBUG |
---|
661 |
debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"write: node 0x%hX, addr = 0x%016" PRIX64 ", length = %zd\n", |
---|
662 |
nodeId, addr, length); |
---|
663 |
printBuffer( DEBUG_LEVEL_VERY_VERBOSE, length, data ); |
---|
664 |
#endif |
---|
665 |
|
---|
666 |
return raw1394_write( m_handle, nodeId, addr, length*4, data ) == 0; |
---|
667 |
} |
---|
668 |
|
---|
669 |
bool |
---|
670 |
Ieee1394Service::write_quadlet( fb_nodeid_t nodeId, |
---|
671 |
fb_nodeaddr_t addr, |
---|
672 |
fb_quadlet_t data ) |
---|
673 |
{ |
---|
674 |
return write( nodeId, addr, sizeof( data )/4, &data ); |
---|
675 |
} |
---|
676 |
|
---|
677 |
bool |
---|
678 |
Ieee1394Service::write_octlet( fb_nodeid_t nodeId, |
---|
679 |
fb_nodeaddr_t addr, |
---|
680 |
fb_octlet_t data ) |
---|
681 |
{ |
---|
682 |
return write( nodeId, addr, sizeof( data )/4, |
---|
683 |
reinterpret_cast<fb_quadlet_t*>( &data ) ); |
---|
684 |
} |
---|
685 |
|
---|
686 |
bool |
---|
687 |
Ieee1394Service::lockCompareSwap64( fb_nodeid_t nodeId, |
---|
688 |
fb_nodeaddr_t addr, |
---|
689 |
fb_octlet_t compare_value, |
---|
690 |
fb_octlet_t swap_value, |
---|
691 |
fb_octlet_t* result ) |
---|
692 |
{ |
---|
693 |
if (nodeId == INVALID_NODE_ID) { |
---|
694 |
debugWarning("operation on invalid node\n"); |
---|
695 |
return false; |
---|
696 |
} |
---|
697 |
#ifdef DEBUG |
---|
698 |
debugOutput(DEBUG_LEVEL_VERBOSE,"lockCompareSwap64: node 0x%X, addr = 0x%016" PRIX64 "\n", |
---|
699 |
nodeId, addr); |
---|
700 |
debugOutput(DEBUG_LEVEL_VERBOSE," if (*(addr)==0x%016" PRIX64 ") *(addr)=0x%016" PRIX64 "\n", |
---|
701 |
compare_value, swap_value); |
---|
702 |
fb_octlet_t buffer; |
---|
703 |
if(!read_octlet( nodeId, addr,&buffer )) { |
---|
704 |
debugWarning("Could not read register\n"); |
---|
705 |
} else { |
---|
706 |
debugOutput(DEBUG_LEVEL_VERBOSE,"before = 0x%016" PRIX64 "\n", buffer); |
---|
707 |
} |
---|
708 |
#endif |
---|
709 |
|
---|
710 |
// do endiannes swapping |
---|
711 |
compare_value = CondSwapToBus64(compare_value); |
---|
712 |
swap_value = CondSwapToBus64(swap_value); |
---|
713 |
|
---|
714 |
// do separate locking here (no MutexLockHelper) since |
---|
715 |
// we use read_octlet in the DEBUG code in this function |
---|
716 |
m_handle_lock->Lock(); |
---|
717 |
int retval=raw1394_lock64(m_handle, nodeId, addr, |
---|
718 |
RAW1394_EXTCODE_COMPARE_SWAP, |
---|
719 |
swap_value, compare_value, result); |
---|
720 |
m_handle_lock->Unlock(); |
---|
721 |
|
---|
722 |
if(retval) { |
---|
723 |
debugError("raw1394_lock64 failed: %s\n", strerror(errno)); |
---|
724 |
} |
---|
725 |
|
---|
726 |
#ifdef DEBUG |
---|
727 |
if(!read_octlet( nodeId, addr,&buffer )) { |
---|
728 |
debugWarning("Could not read register\n"); |
---|
729 |
} else { |
---|
730 |
debugOutput(DEBUG_LEVEL_VERBOSE,"after = 0x%016" PRIX64 "\n", buffer); |
---|
731 |
} |
---|
732 |
#endif |
---|
733 |
|
---|
734 |
*result = CondSwapFromBus64(*result); |
---|
735 |
|
---|
736 |
return (retval == 0); |
---|
737 |
} |
---|
738 |
|
---|
739 |
fb_quadlet_t* |
---|
740 |
Ieee1394Service::transactionBlock( fb_nodeid_t nodeId, |
---|
741 |
fb_quadlet_t* buf, |
---|
742 |
int len, |
---|
743 |
unsigned int* resp_len ) |
---|
744 |
{ |
---|
745 |
// FIXME: simplify semantics |
---|
746 |
if (nodeId == INVALID_NODE_ID) { |
---|
747 |
debugWarning("operation on invalid node\n"); |
---|
748 |
return NULL; |
---|
749 |
} |
---|
750 |
// NOTE: this expects a call to transactionBlockClose to unlock |
---|
751 |
m_handle_lock->Lock(); |
---|
752 |
|
---|
753 |
// clear the request & response memory |
---|
754 |
memset(&m_fcp_block, 0, sizeof(m_fcp_block)); |
---|
755 |
|
---|
756 |
// make a local copy of the request |
---|
757 |
if(len < MAX_FCP_BLOCK_SIZE_QUADS) { |
---|
758 |
memcpy(m_fcp_block.request, buf, len*sizeof(quadlet_t)); |
---|
759 |
m_fcp_block.request_length = len; |
---|
760 |
} else { |
---|
761 |
debugWarning("Truncating FCP request\n"); |
---|
762 |
memcpy(m_fcp_block.request, buf, MAX_FCP_BLOCK_SIZE_BYTES); |
---|
763 |
m_fcp_block.request_length = MAX_FCP_BLOCK_SIZE_QUADS; |
---|
764 |
} |
---|
765 |
m_fcp_block.target_nodeid = 0xffc0 | nodeId; |
---|
766 |
|
---|
767 |
bool success = doFcpTransaction(); |
---|
768 |
if(success) { |
---|
769 |
*resp_len = m_fcp_block.response_length; |
---|
770 |
return m_fcp_block.response; |
---|
771 |
} else { |
---|
772 |
debugWarning("FCP transaction failed\n"); |
---|
773 |
*resp_len = 0; |
---|
774 |
return NULL; |
---|
775 |
} |
---|
776 |
} |
---|
777 |
|
---|
778 |
bool |
---|
779 |
Ieee1394Service::transactionBlockClose() |
---|
780 |
{ |
---|
781 |
m_handle_lock->Unlock(); |
---|
782 |
return true; |
---|
783 |
} |
---|
784 |
|
---|
785 |
// FCP code |
---|
786 |
bool |
---|
787 |
Ieee1394Service::doFcpTransaction() |
---|
788 |
{ |
---|
789 |
for(int i=0; i < IEEE1394SERVICE_FCP_MAX_TRIES; i++) { |
---|
790 |
if(doFcpTransactionTry()) { |
---|
791 |
return true; |
---|
792 |
} else { |
---|
793 |
debugOutput(DEBUG_LEVEL_VERBOSE, "FCP transaction try %d failed\n", i); |
---|
794 |
Util::SystemTimeSource::SleepUsecRelative( IEEE1394SERVICE_FCP_SLEEP_BETWEEN_FAILURES_USECS); |
---|
795 |
} |
---|
796 |
} |
---|
797 |
debugError("FCP transaction didn't succeed in %d tries\n", IEEE1394SERVICE_FCP_MAX_TRIES); |
---|
798 |
return false; |
---|
799 |
} |
---|
800 |
|
---|
801 |
#define FCP_COMMAND_ADDR 0xFFFFF0000B00ULL |
---|
802 |
#define FCP_RESPONSE_ADDR 0xFFFFF0000D00ULL |
---|
803 |
|
---|
804 |
/* AV/C FCP response codes */ |
---|
805 |
#define FCP_RESPONSE_NOT_IMPLEMENTED 0x08000000 |
---|
806 |
#define FCP_RESPONSE_ACCEPTED 0x09000000 |
---|
807 |
#define FCP_RESPONSE_REJECTED 0x0A000000 |
---|
808 |
#define FCP_RESPONSE_IN_TRANSITION 0x0B000000 |
---|
809 |
#define FCP_RESPONSE_IMPLEMENTED 0x0C000000 |
---|
810 |
#define FCP_RESPONSE_STABLE 0x0C000000 |
---|
811 |
#define FCP_RESPONSE_CHANGED 0x0D000000 |
---|
812 |
#define FCP_RESPONSE_INTERIM 0x0F000000 |
---|
813 |
|
---|
814 |
/* AV/C FCP mask macros */ |
---|
815 |
#define FCP_MASK_START(x) ((x) & 0xF0000000) |
---|
816 |
#define FCP_MASK_CTYPE(x) ((x) & 0x0F000000) |
---|
817 |
#define FCP_MASK_RESPONSE(x) ((x) & 0x0F000000) |
---|
818 |
#define FCP_MASK_SUBUNIT(x) ((x) & 0x00FF0000) |
---|
819 |
#define FCP_MASK_SUBUNIT_TYPE(x) ((x) & 0x00F80000) |
---|
820 |
#define FCP_MASK_SUBUNIT_ID(x) ((x) & 0x00070000) |
---|
821 |
#define FCP_MASK_OPCODE(x) ((x) & 0x0000FF00) |
---|
822 |
#define FCP_MASK_SUBUNIT_AND_OPCODE(x) ((x) & 0x00FFFF00) |
---|
823 |
#define FCP_MASK_OPERAND0(x) ((x) & 0x000000FF) |
---|
824 |
#define FCP_MASK_OPERAND(x, n) ((x) & (0xFF000000 >> ((((n)-1)%4)*8))) |
---|
825 |
#define FCP_MASK_RESPONSE_OPERAND(x, n) ((x) & (0xFF000000 >> (((n)%4)*8))) |
---|
826 |
|
---|
827 |
bool |
---|
828 |
Ieee1394Service::doFcpTransactionTry() |
---|
829 |
{ |
---|
830 |
// NOTE that access to this is protected by the m_handle lock |
---|
831 |
int err; |
---|
832 |
bool retval = true; |
---|
833 |
uint64_t timeout; |
---|
834 |
|
---|
835 |
// prepare an fcp response handler |
---|
836 |
raw1394_set_fcp_handler(m_handle, _avc_fcp_handler); |
---|
837 |
|
---|
838 |
// start listening for FCP requests |
---|
839 |
// this fails if some other program is listening for a FCP response |
---|
840 |
err = raw1394_start_fcp_listen(m_handle); |
---|
841 |
if(err) { |
---|
842 |
debugOutput(DEBUG_LEVEL_VERBOSE, "could not start FCP listen (err=%d, errno=%d)\n", err, errno); |
---|
843 |
retval = false; |
---|
844 |
goto out; |
---|
845 |
} |
---|
846 |
|
---|
847 |
m_fcp_block.status = eFS_Waiting; |
---|
848 |
|
---|
849 |
#ifdef DEBUG |
---|
850 |
debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"fcp request: node 0x%hX, length = %d bytes\n", |
---|
851 |
m_fcp_block.target_nodeid, m_fcp_block.request_length*4); |
---|
852 |
printBuffer(DEBUG_LEVEL_VERY_VERBOSE, m_fcp_block.request_length, m_fcp_block.request ); |
---|
853 |
#endif |
---|
854 |
|
---|
855 |
// write the FCP request |
---|
856 |
if(!writeNoLock( m_fcp_block.target_nodeid, FCP_COMMAND_ADDR, |
---|
857 |
m_fcp_block.request_length, m_fcp_block.request)) { |
---|
858 |
debugOutput(DEBUG_LEVEL_VERBOSE, "write of FCP request failed\n"); |
---|
859 |
retval = false; |
---|
860 |
goto out; |
---|
861 |
} |
---|
862 |
|
---|
863 |
// wait for the response to arrive |
---|
864 |
struct pollfd raw1394_poll; |
---|
865 |
raw1394_poll.fd = raw1394_get_fd(m_handle); |
---|
866 |
raw1394_poll.events = POLLIN; |
---|
867 |
|
---|
868 |
timeout = Util::SystemTimeSource::getCurrentTimeAsUsecs() + |
---|
869 |
IEEE1394SERVICE_FCP_RESPONSE_TIMEOUT_USEC; |
---|
870 |
|
---|
871 |
while(m_fcp_block.status == eFS_Waiting |
---|
872 |
&& Util::SystemTimeSource::getCurrentTimeAsUsecs() < timeout) { |
---|
873 |
if(poll( &raw1394_poll, 1, IEEE1394SERVICE_FCP_POLL_TIMEOUT_MSEC) > 0) { |
---|
874 |
if (raw1394_poll.revents & POLLIN) { |
---|
875 |
raw1394_loop_iterate(m_handle); |
---|
876 |
} |
---|
877 |
} |
---|
878 |
} |
---|
879 |
|
---|
880 |
// check the request and figure out what happened |
---|
881 |
if(m_fcp_block.status == eFS_Waiting) { |
---|
882 |
debugOutput(DEBUG_LEVEL_VERBOSE, "FCP response timed out\n"); |
---|
883 |
retval = false; |
---|
884 |
goto out; |
---|
885 |
} |
---|
886 |
if(m_fcp_block.status == eFS_Error) { |
---|
887 |
debugError("FCP request/response error\n"); |
---|
888 |
retval = false; |
---|
889 |
goto out; |
---|
890 |
} |
---|
891 |
|
---|
892 |
out: |
---|
893 |
// stop listening for FCP responses |
---|
894 |
err = raw1394_stop_fcp_listen(m_handle); |
---|
895 |
if(err) { |
---|
896 |
debugOutput(DEBUG_LEVEL_VERBOSE, "could not stop FCP listen (err=%d, errno=%d)\n", err, errno); |
---|
897 |
retval = false; |
---|
898 |
} |
---|
899 |
|
---|
900 |
m_fcp_block.status = eFS_Empty; |
---|
901 |
return retval; |
---|
902 |
} |
---|
903 |
|
---|
904 |
int |
---|
905 |
Ieee1394Service::_avc_fcp_handler(raw1394handle_t handle, nodeid_t nodeid, |
---|
906 |
int response, size_t length, |
---|
907 |
unsigned char *data) |
---|
908 |
{ |
---|
909 |
Ieee1394Service *service = static_cast<Ieee1394Service *>(raw1394_get_userdata(handle)); |
---|
910 |
if(service) { |
---|
911 |
return service->handleFcpResponse(nodeid, response, length, data); |
---|
912 |
} else return -1; |
---|
913 |
} |
---|
914 |
|
---|
915 |
int |
---|
916 |
Ieee1394Service::handleFcpResponse(nodeid_t nodeid, |
---|
917 |
int response, size_t length, |
---|
918 |
unsigned char *data) |
---|
919 |
{ |
---|
920 |
static struct sFcpBlock fcp_block_last; |
---|
921 |
|
---|
922 |
fb_quadlet_t *data_quads = (fb_quadlet_t *)data; |
---|
923 |
#ifdef DEBUG |
---|
924 |
debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"fcp response: node 0x%hX, response = %d, length = %zd bytes\n", |
---|
925 |
nodeid, response, length); |
---|
926 |
printBuffer(DEBUG_LEVEL_VERY_VERBOSE, (length+3)/4, data_quads ); |
---|
927 |
#endif |
---|
928 |
|
---|
929 |
if (response && length > 3) { |
---|
930 |
if(length > MAX_FCP_BLOCK_SIZE_BYTES) { |
---|
931 |
length = MAX_FCP_BLOCK_SIZE_BYTES; |
---|
932 |
debugWarning("Truncated FCP response\n"); |
---|
933 |
} |
---|
934 |
|
---|
935 |
// is it an actual response or is it INTERIM? |
---|
936 |
quadlet_t first_quadlet = CondSwapFromBus32(data_quads[0]); |
---|
937 |
if(FCP_MASK_RESPONSE(first_quadlet) == FCP_RESPONSE_INTERIM) { |
---|
938 |
debugOutput(DEBUG_LEVEL_VERBOSE, "INTERIM\n"); |
---|
939 |
} else { |
---|
940 |
// it's an actual response, check if it matches our request |
---|
941 |
if(nodeid != m_fcp_block.target_nodeid) { |
---|
942 |
debugOutput(DEBUG_LEVEL_VERBOSE, "FCP response node id's don't match! (%x, %x)\n", |
---|
943 |
m_fcp_block.target_nodeid, nodeid); |
---|
944 |
} else if (first_quadlet == 0) { |
---|
945 |
debugWarning("Bogus FCP response\n"); |
---|
946 |
printBuffer(DEBUG_LEVEL_WARNING, (length+3)/4, data_quads ); |
---|
947 |
#ifdef DEBUG |
---|
948 |
} else if(FCP_MASK_RESPONSE(first_quadlet) < 0x08000000) { |
---|
949 |
debugWarning("Bogus AV/C FCP response code\n"); |
---|
950 |
printBuffer(DEBUG_LEVEL_WARNING, (length+3)/4, data_quads ); |
---|
951 |
#endif |
---|
952 |
} else if(FCP_MASK_SUBUNIT_AND_OPCODE(first_quadlet) |
---|
953 |
!= FCP_MASK_SUBUNIT_AND_OPCODE(CondSwapFromBus32(m_fcp_block.request[0]))) { |
---|
954 |
debugOutput(DEBUG_LEVEL_VERBOSE, "FCP response not for this request: %08X != %08X\n", |
---|
955 |
FCP_MASK_SUBUNIT_AND_OPCODE(first_quadlet), |
---|
956 |
FCP_MASK_SUBUNIT_AND_OPCODE(CondSwapFromBus32(m_fcp_block.request[0]))); |
---|
957 |
} else if(m_filterFCPResponse && (memcmp(fcp_block_last.response, data, length) == 0)) { |
---|
958 |
// This is workaround for the Edirol FA-101. The device tends to send more than |
---|
959 |
// one responde to one request. This seems to happen when discovering |
---|
960 |
// function blocks and looks very likely there is a race condition in the |
---|
961 |
// device. The workaround here compares the just arrived FCP responde |
---|
962 |
// to the last one. If it is the same as the previously one then we |
---|
963 |
// just ignore it. The downside of this approach is, we cannot issue |
---|
964 |
// the same FCP twice. |
---|
965 |
debugWarning("Received duplicate FCP response. Ignore it\n"); |
---|
966 |
} else { |
---|
967 |
m_fcp_block.response_length = (length + sizeof(quadlet_t) - 1) / sizeof(quadlet_t); |
---|
968 |
memcpy(m_fcp_block.response, data, length); |
---|
969 |
if (m_filterFCPResponse) { |
---|
970 |
memcpy(fcp_block_last.response, data, length); |
---|
971 |
} |
---|
972 |
m_fcp_block.status = eFS_Responded; |
---|
973 |
} |
---|
974 |
} |
---|
975 |
} |
---|
976 |
return 0; |
---|
977 |
} |
---|
978 |
|
---|
979 |
bool |
---|
980 |
Ieee1394Service::setSplitTimeoutUsecs(fb_nodeid_t nodeId, unsigned int timeout) |
---|
981 |
{ |
---|
982 |
Util::MutexLockHelper lock(*m_handle_lock); |
---|
983 |
debugOutput(DEBUG_LEVEL_VERBOSE, "setting SPLIT_TIMEOUT on node 0x%X to %uusecs...\n", nodeId, timeout); |
---|
984 |
unsigned int secs = timeout / 1000000; |
---|
985 |
unsigned int usecs = timeout % 1000000; |
---|
986 |
|
---|
987 |
quadlet_t split_timeout_hi = CondSwapToBus32(secs & 7); |
---|
988 |
quadlet_t split_timeout_low = CondSwapToBus32(((usecs / 125) & 0x1FFF) << 19); |
---|
989 |
|
---|
990 |
// write the CSR registers |
---|
991 |
if(!writeNoLock( 0xffc0 | nodeId, CSR_REGISTER_BASE + CSR_SPLIT_TIMEOUT_HI, 1, |
---|
992 |
&split_timeout_hi)) { |
---|
993 |
debugOutput(DEBUG_LEVEL_VERBOSE, "write of CSR_SPLIT_TIMEOUT_HI failed\n"); |
---|
994 |
return false; |
---|
995 |
} |
---|
996 |
if(!writeNoLock( 0xffc0 | nodeId, CSR_REGISTER_BASE + CSR_SPLIT_TIMEOUT_LO, 1, |
---|
997 |
&split_timeout_low)) { |
---|
998 |
debugOutput(DEBUG_LEVEL_VERBOSE, "write of CSR_SPLIT_TIMEOUT_LO failed\n"); |
---|
999 |
return false; |
---|
1000 |
} |
---|
1001 |
return true; |
---|
1002 |
} |
---|
1003 |
|
---|
1004 |
int |
---|
1005 |
Ieee1394Service::getSplitTimeoutUsecs(fb_nodeid_t nodeId) |
---|
1006 |
{ |
---|
1007 |
Util::MutexLockHelper lock(*m_handle_lock); |
---|
1008 |
|
---|
1009 |
// Keep Valgrind quiet by including explicit assignment |
---|
1010 |
quadlet_t split_timeout_hi = 0; |
---|
1011 |
quadlet_t split_timeout_low = 0; |
---|
1012 |
|
---|
1013 |
debugOutput(DEBUG_LEVEL_VERBOSE, "reading SPLIT_TIMEOUT on node 0x%X...\n", nodeId); |
---|
1014 |
|
---|
1015 |
if(!readNoLock( 0xffc0 | nodeId, CSR_REGISTER_BASE + CSR_SPLIT_TIMEOUT_HI, 1, |
---|
1016 |
&split_timeout_hi)) { |
---|
1017 |
debugOutput(DEBUG_LEVEL_VERBOSE, "read of CSR_SPLIT_TIMEOUT_HI failed\n"); |
---|
1018 |
return 0; |
---|
1019 |
} |
---|
1020 |
debugOutput(DEBUG_LEVEL_VERBOSE, " READ HI: 0x%08X\n", split_timeout_hi); |
---|
1021 |
|
---|
1022 |
if(!readNoLock( 0xffc0 | nodeId, CSR_REGISTER_BASE + CSR_SPLIT_TIMEOUT_LO, 1, |
---|
1023 |
&split_timeout_low)) { |
---|
1024 |
debugOutput(DEBUG_LEVEL_VERBOSE, "read of CSR_SPLIT_TIMEOUT_LO failed\n"); |
---|
1025 |
return 0; |
---|
1026 |
} |
---|
1027 |
debugOutput(DEBUG_LEVEL_VERBOSE, " READ LO: 0x%08X\n", split_timeout_low); |
---|
1028 |
|
---|
1029 |
split_timeout_hi = CondSwapFromBus32(split_timeout_hi); |
---|
1030 |
split_timeout_low = CondSwapFromBus32(split_timeout_low); |
---|
1031 |
|
---|
1032 |
return (split_timeout_hi & 7) * 1000000 + (split_timeout_low >> 19) * 125; |
---|
1033 |
} |
---|
1034 |
|
---|
1035 |
void |
---|
1036 |
Ieee1394Service::setFCPResponseFiltering(bool enable) |
---|
1037 |
{ |
---|
1038 |
m_filterFCPResponse = enable; |
---|
1039 |
} |
---|
1040 |
|
---|
1041 |
int |
---|
1042 |
Ieee1394Service::getVerboseLevel() |
---|
1043 |
{ |
---|
1044 |
return getDebugLevel(); |
---|
1045 |
} |
---|
1046 |
|
---|
1047 |
void |
---|
1048 |
Ieee1394Service::printBuffer( unsigned int level, size_t length, fb_quadlet_t* buffer ) const |
---|
1049 |
{ |
---|
1050 |
|
---|
1051 |
for ( unsigned int i=0; i < length; ++i ) { |
---|
1052 |
if ( ( i % 4 ) == 0 ) { |
---|
1053 |
if ( i > 0 ) { |
---|
1054 |
debugOutputShort(level,"\n"); |
---|
1055 |
} |
---|
1056 |
debugOutputShort(level," %4d: ",i*4); |
---|
1057 |
} |
---|
1058 |
debugOutputShort(level,"%08X ",buffer[i]); |
---|
1059 |
} |
---|
1060 |
debugOutputShort(level,"\n"); |
---|
1061 |
} |
---|
1062 |
void |
---|
1063 |
Ieee1394Service::printBufferBytes( unsigned int level, size_t length, byte_t* buffer ) const |
---|
1064 |
{ |
---|
1065 |
|
---|
1066 |
for ( unsigned int i=0; i < length; ++i ) { |
---|
1067 |
if ( ( i % 16 ) == 0 ) { |
---|
1068 |
if ( i > 0 ) { |
---|
1069 |
debugOutputShort(level,"\n"); |
---|
1070 |
} |
---|
1071 |
debugOutputShort(level," %4d: ",i*16); |
---|
1072 |
} |
---|
1073 |
debugOutputShort(level,"%02X ",buffer[i]); |
---|
1074 |
} |
---|
1075 |
debugOutputShort(level,"\n"); |
---|
1076 |
} |
---|
1077 |
|
---|
1078 |
int |
---|
1079 |
Ieee1394Service::resetHandlerLowLevel( raw1394handle_t handle, |
---|
1080 |
unsigned int generation ) |
---|
1081 |
{ |
---|
1082 |
raw1394_update_generation ( handle, generation ); |
---|
1083 |
|
---|
1084 |
Ieee1394Service::HelperThread *thread = reinterpret_cast<Ieee1394Service::HelperThread *>(raw1394_get_userdata( handle )); |
---|
1085 |
if(thread == NULL) { |
---|
1086 |
debugFatal("Bogus 1394 handle private data\n"); |
---|
1087 |
return -1; |
---|
1088 |
} |
---|
1089 |
|
---|
1090 |
Ieee1394Service& service = thread->get1394Service(); |
---|
1091 |
service.resetHandler( generation ); |
---|
1092 |
|
---|
1093 |
return 0; |
---|
1094 |
} |
---|
1095 |
|
---|
1096 |
bool |
---|
1097 |
Ieee1394Service::resetHandler( unsigned int generation ) |
---|
1098 |
{ |
---|
1099 |
quadlet_t buf=0; |
---|
1100 |
|
---|
1101 |
m_handle_lock->Lock(); |
---|
1102 |
raw1394_update_generation(m_handle, generation); |
---|
1103 |
m_handle_lock->Unlock(); |
---|
1104 |
|
---|
1105 |
// do a simple read on ourself in order to update the internal structures |
---|
1106 |
// this avoids failures after a bus reset |
---|
1107 |
read_quadlet( getLocalNodeId() | 0xFFC0, |
---|
1108 |
CSR_REGISTER_BASE | CSR_CYCLE_TIME, |
---|
1109 |
&buf ); |
---|
1110 |
|
---|
1111 |
for ( reset_handler_vec_t::iterator it = m_busResetHandlers.begin(); |
---|
1112 |
it != m_busResetHandlers.end(); |
---|
1113 |
++it ) |
---|
1114 |
{ |
---|
1115 |
Util::Functor* func = *it; |
---|
1116 |
( *func )(); |
---|
1117 |
} |
---|
1118 |
|
---|
1119 |
return true; |
---|
1120 |
} |
---|
1121 |
|
---|
1122 |
bool Ieee1394Service::registerARMHandler(ARMHandler *h) { |
---|
1123 |
debugOutput(DEBUG_LEVEL_VERBOSE, |
---|
1124 |
"Registering ARM handler (%p) for 0x%016" PRIX64 ", length %zu\n", |
---|
1125 |
h, h->getStart(), h->getLength()); |
---|
1126 |
|
---|
1127 |
// FIXME: note that this will result in the ARM handlers not running in a realtime context |
---|
1128 |
int err = raw1394_arm_register(m_armHelperNormal->get1394Handle(), h->getStart(), |
---|
1129 |
h->getLength(), h->getBuffer(), (octlet_t)h, |
---|
1130 |
h->getAccessRights(), |
---|
1131 |
h->getNotificationOptions(), |
---|
1132 |
h->getClientTransactions()); |
---|
1133 |
if (err) { |
---|
1134 |
debugError("Failed to register ARM handler for 0x%016" PRIX64 "\n", h->getStart()); |
---|
1135 |
debugError(" Error: %s\n", strerror(errno)); |
---|
1136 |
return false; |
---|
1137 |
} |
---|
1138 |
m_armHandlers.push_back( h ); |
---|
1139 |
return true; |
---|
1140 |
} |
---|
1141 |
|
---|
1142 |
bool Ieee1394Service::unregisterARMHandler( ARMHandler *h ) { |
---|
1143 |
debugOutput(DEBUG_LEVEL_VERBOSE, "Unregistering ARM handler (%p) for 0x%016" PRIX64 "\n", |
---|
1144 |
h, h->getStart()); |
---|
1145 |
|
---|
1146 |
for ( arm_handler_vec_t::iterator it = m_armHandlers.begin(); |
---|
1147 |
it != m_armHandlers.end(); |
---|
1148 |
++it ) |
---|
1149 |
{ |
---|
1150 |
if((*it) == h) { |
---|
1151 |
int err = raw1394_arm_unregister(m_armHelperNormal->get1394Handle(), h->getStart()); |
---|
1152 |
if (err) { |
---|
1153 |
debugError("Failed to unregister ARM handler (%p)\n", h); |
---|
1154 |
debugError(" Error: %s\n", strerror(errno)); |
---|
1155 |
} else { |
---|
1156 |
m_armHandlers.erase(it); |
---|
1157 |
return true; |
---|
1158 |
} |
---|
1159 |
} |
---|
1160 |
} |
---|
1161 |
debugOutput(DEBUG_LEVEL_VERBOSE, " handler not found!\n"); |
---|
1162 |
|
---|
1163 |
return false; |
---|
1164 |
} |
---|
1165 |
/** |
---|
1166 |
* @brief Tries to find a free ARM address range |
---|
1167 |
* |
---|
1168 |
* @param start address to start with |
---|
1169 |
* @param length length of the block needed (bytes) |
---|
1170 |
* @param step step to use when searching (bytes) |
---|
1171 |
* @return The base address that is free, and 0xFFFFFFFFFFFFFFFF when failed |
---|
1172 |
*/ |
---|
1173 |
nodeaddr_t Ieee1394Service::findFreeARMBlock( nodeaddr_t start, size_t length, size_t step ) { |
---|
1174 |
debugOutput(DEBUG_LEVEL_VERBOSE, |
---|
1175 |
"Finding free ARM block of %zd bytes, from 0x%016" PRIX64 " in steps of %zd bytes\n", |
---|
1176 |
length, start, step); |
---|
1177 |
|
---|
1178 |
int cnt=0; |
---|
1179 |
const int maxcnt=10; |
---|
1180 |
int err=1; |
---|
1181 |
Util::MutexLockHelper lock(*m_handle_lock); |
---|
1182 |
while(err && cnt++ < maxcnt) { |
---|
1183 |
// try to register |
---|
1184 |
err = raw1394_arm_register(m_handle, start, length, 0, 0, 0, 0, 0); |
---|
1185 |
|
---|
1186 |
if (err) { |
---|
1187 |
debugOutput(DEBUG_LEVEL_VERBOSE, " -> cannot use 0x%016" PRIX64 "\n", start); |
---|
1188 |
debugError(" Error: %s\n", strerror(errno)); |
---|
1189 |
start += step; |
---|
1190 |
} else { |
---|
1191 |
debugOutput(DEBUG_LEVEL_VERBOSE, " -> use 0x%016" PRIX64 "\n", start); |
---|
1192 |
err = raw1394_arm_unregister(m_handle, start); |
---|
1193 |
if (err) { |
---|
1194 |
debugOutput(DEBUG_LEVEL_VERBOSE, " error unregistering test handler\n"); |
---|
1195 |
debugError(" Error: %s\n", strerror(errno)); |
---|
1196 |
return 0xFFFFFFFFFFFFFFFFLLU; |
---|
1197 |
} |
---|
1198 |
return start; |
---|
1199 |
} |
---|
1200 |
} |
---|
1201 |
debugOutput(DEBUG_LEVEL_VERBOSE, " Could not find free block in %d tries\n",cnt); |
---|
1202 |
return 0xFFFFFFFFFFFFFFFFLLU; |
---|
1203 |
} |
---|
1204 |
|
---|
1205 |
int |
---|
1206 |
Ieee1394Service::armHandlerLowLevel(raw1394handle_t handle, |
---|
1207 |
unsigned long arm_tag, |
---|
1208 |
byte_t request_type, unsigned int requested_length, |
---|
1209 |
void *data) |
---|
1210 |
{ |
---|
1211 |
Ieee1394Service::HelperThread *thread = reinterpret_cast<Ieee1394Service::HelperThread *>(raw1394_get_userdata( handle )); |
---|
1212 |
if(thread == NULL) { |
---|
1213 |
debugFatal("Bogus 1394 handle private data\n"); |
---|
1214 |
return -1; |
---|
1215 |
} |
---|
1216 |
|
---|
1217 |
Ieee1394Service& service = thread->get1394Service(); |
---|
1218 |
if(service.armHandler( arm_tag, request_type, requested_length, data )) { |
---|
1219 |
return 0; |
---|
1220 |
} else { |
---|
1221 |
return -1; |
---|
1222 |
} |
---|
1223 |
} |
---|
1224 |
|
---|
1225 |
bool |
---|
1226 |
Ieee1394Service::armHandler( unsigned long arm_tag, |
---|
1227 |
byte_t request_type, unsigned int requested_length, |
---|
1228 |
void *data) |
---|
1229 |
{ |
---|
1230 |
for ( arm_handler_vec_t::iterator it = m_armHandlers.begin(); |
---|
1231 |
it != m_armHandlers.end(); |
---|
1232 |
++it ) |
---|
1233 |
{ |
---|
1234 |
if((*it) == (ARMHandler *)arm_tag) { |
---|
1235 |
struct raw1394_arm_request_response *arm_req_resp; |
---|
1236 |
arm_req_resp = (struct raw1394_arm_request_response *) data; |
---|
1237 |
raw1394_arm_request_t arm_req = arm_req_resp->request; |
---|
1238 |
raw1394_arm_response_t arm_resp = arm_req_resp->response; |
---|
1239 |
|
---|
1240 |
debugOutput(DEBUG_LEVEL_VERBOSE,"ARM handler for address 0x%016" PRIX64 " called\n", |
---|
1241 |
(*it)->getStart()); |
---|
1242 |
debugOutput(DEBUG_LEVEL_VERBOSE," request type : 0x%02X\n", request_type); |
---|
1243 |
debugOutput(DEBUG_LEVEL_VERBOSE," request length : %04d\n", requested_length); |
---|
1244 |
|
---|
1245 |
switch(request_type) { |
---|
1246 |
case RAW1394_ARM_READ: |
---|
1247 |
(*it)->handleRead(arm_req); |
---|
1248 |
*arm_resp = *((*it)->getResponse()); |
---|
1249 |
break; |
---|
1250 |
case RAW1394_ARM_WRITE: |
---|
1251 |
(*it)->handleWrite(arm_req); |
---|
1252 |
*arm_resp = *((*it)->getResponse()); |
---|
1253 |
break; |
---|
1254 |
case RAW1394_ARM_LOCK: |
---|
1255 |
(*it)->handleLock(arm_req); |
---|
1256 |
*arm_resp = *((*it)->getResponse()); |
---|
1257 |
break; |
---|
1258 |
default: |
---|
1259 |
debugWarning("Unknown request type received, ignoring...\n"); |
---|
1260 |
} |
---|
1261 |
return true; |
---|
1262 |
} |
---|
1263 |
} |
---|
1264 |
|
---|
1265 |
debugOutput(DEBUG_LEVEL_VERBOSE,"default ARM handler called\n"); |
---|
1266 |
|
---|
1267 |
m_default_arm_handler(m_armHelperNormal->get1394Handle(), arm_tag, request_type, requested_length, data ); |
---|
1268 |
return true; |
---|
1269 |
} |
---|
1270 |
|
---|
1271 |
bool |
---|
1272 |
Ieee1394Service::addBusResetHandler( Util::Functor* functor ) |
---|
1273 |
{ |
---|
1274 |
debugOutput(DEBUG_LEVEL_VERBOSE, "Adding busreset handler (%p)\n", functor); |
---|
1275 |
m_busResetHandlers.push_back( functor ); |
---|
1276 |
return true; |
---|
1277 |
} |
---|
1278 |
|
---|
1279 |
bool |
---|
1280 |
Ieee1394Service::remBusResetHandler( Util::Functor* functor ) |
---|
1281 |
{ |
---|
1282 |
debugOutput(DEBUG_LEVEL_VERBOSE, "Removing busreset handler (%p)\n", functor); |
---|
1283 |
|
---|
1284 |
for ( reset_handler_vec_t::iterator it = m_busResetHandlers.begin(); |
---|
1285 |
it != m_busResetHandlers.end(); |
---|
1286 |
++it ) |
---|
1287 |
{ |
---|
1288 |
if ( *it == functor ) { |
---|
1289 |
debugOutput(DEBUG_LEVEL_VERBOSE, " found\n"); |
---|
1290 |
m_busResetHandlers.erase( it ); |
---|
1291 |
return true; |
---|
1292 |
} |
---|
1293 |
} |
---|
1294 |
debugOutput(DEBUG_LEVEL_VERBOSE, " not found\n"); |
---|
1295 |
return false; |
---|
1296 |
} |
---|
1297 |
|
---|
1298 |
/** |
---|
1299 |
* Allocates an iso channel for use by the interface in a similar way to |
---|
1300 |
* libiec61883. Returns -1 on error (due to there being no free channels) |
---|
1301 |
* or an allocated channel number. |
---|
1302 |
* |
---|
1303 |
* Does not perform anything other than registering the channel and the |
---|
1304 |
* bandwidth at the IRM |
---|
1305 |
* |
---|
1306 |
* Also allocates the necessary bandwidth (in ISO allocation units). |
---|
1307 |
* |
---|
1308 |
* FIXME: As in libiec61883, channel 63 is not requested; this is either a |
---|
1309 |
* bug or it's omitted since that's the channel preferred by video devices. |
---|
1310 |
* |
---|
1311 |
* @param bandwidth the bandwidth to allocate for this channel |
---|
1312 |
* @return the channel number |
---|
1313 |
*/ |
---|
1314 |
signed int Ieee1394Service::allocateIsoChannelGeneric(unsigned int bandwidth) { |
---|
1315 |
debugOutput(DEBUG_LEVEL_VERBOSE, "Allocating ISO channel using generic method...\n" ); |
---|
1316 |
|
---|
1317 |
Util::MutexLockHelper lock(*m_handle_lock); |
---|
1318 |
struct ChannelInfo cinfo; |
---|
1319 |
|
---|
1320 |
int c = -1; |
---|
1321 |
for (c = 0; c < 63; c++) { |
---|
1322 |
if (raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_ALLOC) == 0) |
---|
1323 |
break; |
---|
1324 |
} |
---|
1325 |
if (c < 63) { |
---|
1326 |
debugOutput(DEBUG_LEVEL_VERBOSE, "found free iso channel %d\n", c); |
---|
1327 |
if (raw1394_bandwidth_modify(m_handle, bandwidth, RAW1394_MODIFY_ALLOC) < 0) { |
---|
1328 |
debugFatal("Could not allocate bandwidth of %d\n", bandwidth); |
---|
1329 |
|
---|
1330 |
raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_FREE); |
---|
1331 |
return -1; |
---|
1332 |
} else { |
---|
1333 |
cinfo.channel=c; |
---|
1334 |
cinfo.bandwidth=bandwidth; |
---|
1335 |
cinfo.alloctype=AllocGeneric; |
---|
1336 |
|
---|
1337 |
cinfo.xmit_node=-1; |
---|
1338 |
cinfo.xmit_plug=-1; |
---|
1339 |
cinfo.recv_node=-1; |
---|
1340 |
cinfo.recv_plug=-1; |
---|
1341 |
|
---|
1342 |
if (registerIsoChannel(c, cinfo)) { |
---|
1343 |
return c; |
---|
1344 |
} else { |
---|
1345 |
raw1394_bandwidth_modify(m_handle, bandwidth, RAW1394_MODIFY_FREE); |
---|
1346 |
raw1394_channel_modify (m_handle, c, RAW1394_MODIFY_FREE); |
---|
1347 |
return -1; |
---|
1348 |
} |
---|
1349 |
} |
---|
1350 |
} |
---|
1351 |
return -1; |
---|
1352 |
} |
---|
1353 |
|
---|
1354 |
/** |
---|
1355 |
* Allocates a specific fixed iso channel for use by the interface. Returns |
---|
1356 |
* -1 on error (due to the requested channel not being free) or the fixed iso |
---|
1357 |
* channel number. |
---|
1358 |
* |
---|
1359 |
* Does not perform anything other than registering the channel and the |
---|
1360 |
* bandwidth at the IRM |
---|
1361 |
* |
---|
1362 |
* Also allocates the necessary bandwidth (in ISO allocation units). |
---|
1363 |
* |
---|
1364 |
* FIXME: As in libiec61883, channel 63 is not requested; this is either a |
---|
1365 |
* bug or it's omitted since that's the channel preferred by video devices. |
---|
1366 |
* |
---|
1367 |
* @chan the channel number being requested |
---|
1368 |
* @param bandwidth the bandwidth to allocate for this channel |
---|
1369 |
* @return the channel number |
---|
1370 |
*/ |
---|
1371 |
signed int Ieee1394Service::allocateFixedIsoChannelGeneric( |
---|
1372 |
unsigned int chan, unsigned int bandwidth |
---|
1373 |
) { |
---|
1374 |
debugOutput(DEBUG_LEVEL_VERBOSE, "Allocating ISO channel %d using generic method...\n", chan ); |
---|
1375 |
|
---|
1376 |
Util::MutexLockHelper lock(*m_handle_lock); |
---|
1377 |
struct ChannelInfo cinfo; |
---|
1378 |
|
---|
1379 |
if (raw1394_channel_modify (m_handle, chan, RAW1394_MODIFY_ALLOC) == 0) { |
---|
1380 |
if (raw1394_bandwidth_modify(m_handle, bandwidth, RAW1394_MODIFY_ALLOC) < 0) { |
---|
1381 |
debugFatal("Could not allocate bandwidth of %d\n", bandwidth); |
---|
1382 |
|
---|
1383 |
raw1394_channel_modify (m_handle, chan, RAW1394_MODIFY_FREE); |
---|
1384 |
return -1; |
---|
1385 |
} else { |
---|
1386 |
cinfo.channel=chan; |
---|
1387 |
cinfo.bandwidth=bandwidth; |
---|
1388 |
cinfo.alloctype=AllocGeneric; |
---|
1389 |
|
---|
1390 |
cinfo.xmit_node=-1; |
---|
1391 |
cinfo.xmit_plug=-1; |
---|
1392 |
cinfo.recv_node=-1; |
---|
1393 |
cinfo.recv_plug=-1; |
---|
1394 |
|
---|
1395 |
if (registerIsoChannel(chan, cinfo)) { |
---|
1396 |
return chan; |
---|
1397 |
} else { |
---|
1398 |
raw1394_bandwidth_modify(m_handle, bandwidth, RAW1394_MODIFY_FREE); |
---|
1399 |
raw1394_channel_modify (m_handle, chan, RAW1394_MODIFY_FREE); |
---|
1400 |
return -1; |
---|
1401 |
} |
---|
1402 |
} |
---|
1403 |
} |
---|
1404 |
return -1; |
---|
1405 |
} |
---|
1406 |
|
---|
1407 |
/** |
---|
1408 |
* Allocates an iso channel for use by the interface in a similar way to |
---|
1409 |
* libiec61883. Returns -1 on error (due to there being no free channels) |
---|
1410 |
* or an allocated channel number. |
---|
1411 |
* |
---|
1412 |
* Uses IEC61883 Connection Management Procedure to establish the connection. |
---|
1413 |
* |
---|
1414 |
* Also allocates the necessary bandwidth (in ISO allocation units). |
---|
1415 |
* |
---|
1416 |
* @param xmit_node node id of the transmitter |
---|
1417 |
* @param xmit_plug the output plug to use. If -1, find the first online plug, and |
---|
1418 |
* upon return, contains the plug number used. |
---|
1419 |
* @param recv_node node id of the receiver |
---|
1420 |
* @param recv_plug the input plug to use. If -1, find the first online plug, and |
---|
1421 |
* upon return, contains the plug number used. |
---|
1422 |
* |
---|
1423 |
* @return the channel number |
---|
1424 |
*/ |
---|
1425 |
|
---|
1426 |
signed int Ieee1394Service::allocateIsoChannelCMP( |
---|
1427 |
nodeid_t xmit_node, int xmit_plug, |
---|
1428 |
nodeid_t recv_node, int recv_plug |
---|
1429 |
) { |
---|
1430 |
|
---|
1431 |
if (xmit_node == INVALID_NODE_ID) { |
---|
1432 |
debugWarning("operation on invalid node (XMIT)\n"); |
---|
1433 |
return -1; |
---|
1434 |
} |
---|
1435 |
if (recv_node == INVALID_NODE_ID) { |
---|
1436 |
debugWarning("operation on invalid node (RECV)\n"); |
---|
1437 |
return -1; |
---|
1438 |
} |
---|
1439 |
|
---|
1440 |
debugOutput(DEBUG_LEVEL_VERBOSE, "Allocating ISO channel using IEC61883 CMP...\n" ); |
---|
1441 |
Util::MutexLockHelper lock(*m_handle_lock); |
---|
1442 |
|
---|
1443 |
struct ChannelInfo cinfo; |
---|
1444 |
|
---|
1445 |
int c = -1; |
---|
1446 |
int bandwidth=1; |
---|
1447 |
#if IEEE1394SERVICE_SKIP_IEC61883_BANDWIDTH_ALLOCATION |
---|
1448 |
bandwidth=0; |
---|
1449 |
#endif |
---|
1450 |
|
---|
1451 |
// do connection management: make connection |
---|
1452 |
c = iec61883_cmp_connect( |
---|
1453 |
m_handle, |
---|
1454 |
xmit_node | 0xffc0, |
---|
1455 |
&xmit_plug, |
---|
1456 |
recv_node | 0xffc0, |
---|
1457 |
&recv_plug, |
---|
1458 |
&bandwidth); |
---|
1459 |
|
---|
1460 |
if((c<0) || (c>63)) { |
---|
1461 |
debugError("Could not do CMP from %04X:%02d to %04X:%02d\n", |
---|
1462 |
xmit_node, xmit_plug, recv_node, recv_plug |
---|
1463 |
); |
---|
1464 |
return -1; |
---|
1465 |
} |
---|
1466 |
|
---|
1467 |
cinfo.channel=c; |
---|
1468 |
cinfo.bandwidth=bandwidth; |
---|
1469 |
cinfo.alloctype=AllocCMP; |
---|
1470 |
|
---|
1471 |
cinfo.xmit_node=xmit_node; |
---|
1472 |
cinfo.xmit_plug=xmit_plug; |
---|
1473 |
cinfo.recv_node=recv_node; |
---|
1474 |
cinfo.recv_plug=recv_plug; |
---|
1475 |
|
---|
1476 |
if (registerIsoChannel(c, cinfo)) { |
---|
1477 |
return c; |
---|
1478 |
} |
---|
1479 |
|
---|
1480 |
return -1; |
---|
1481 |
} |
---|
1482 |
|
---|
1483 |
/** |
---|
1484 |
* Deallocates an iso channel. Silently ignores a request to deallocate |
---|
1485 |
* a negative channel number. |
---|
1486 |
* |
---|
1487 |
* Figures out the method that was used to allocate the channel (generic, cmp, ...) |
---|
1488 |
* and uses the appropriate method to deallocate. Also frees the bandwidth |
---|
1489 |
* that was reserved along with this channel. |
---|
1490 |
* |
---|
1491 |
* @param c channel number |
---|
1492 |
* @return true if successful |
---|
1493 |
*/ |
---|
1494 |
bool Ieee1394Service::freeIsoChannel(signed int c) { |
---|
1495 |
debugOutput(DEBUG_LEVEL_VERBOSE, "Freeing ISO channel %d...\n", c ); |
---|
1496 |
Util::MutexLockHelper lock(*m_handle_lock); |
---|
1497 |
|
---|
1498 |
if (c < 0 || c > 63) { |
---|
1499 |
debugWarning("Invalid channel number: %d\n", c); |
---|
1500 |
return false; |
---|
1501 |
} |
---|
1502 |
|
---|
1503 |
switch (m_channels[c].alloctype) { |
---|
1504 |
default: |
---|
1505 |
debugError(" BUG: invalid allocation type!\n"); |
---|
1506 |
return false; |
---|
1507 |
|
---|
1508 |
case AllocFree: |
---|
1509 |
debugWarning(" Channel %d not registered\n", c); |
---|
1510 |
return false; |
---|
1511 |
|
---|
1512 |
case AllocGeneric: |
---|
1513 |
debugOutput(DEBUG_LEVEL_VERBOSE, " allocated using generic routine...\n" ); |
---|
1514 |
debugOutput(DEBUG_LEVEL_VERBOSE, " freeing %d bandwidth units...\n", m_channels[c].bandwidth ); |
---|
1515 |
if (raw1394_bandwidth_modify(m_handle, m_channels[c].bandwidth, RAW1394_MODIFY_FREE) !=0) { |
---|
1516 |
debugWarning("Failed to deallocate bandwidth\n"); |
---|
1517 |
} |
---|
1518 |
debugOutput(DEBUG_LEVEL_VERBOSE, " freeing channel %d...\n", m_channels[c].channel ); |
---|
1519 |
if (raw1394_channel_modify (m_handle, m_channels[c].channel, RAW1394_MODIFY_FREE) != 0) { |
---|
1520 |
debugWarning("Failed to free channel\n"); |
---|
1521 |
} |
---|
1522 |
if (!unregisterIsoChannel(c)) |
---|
1523 |
return false; |
---|
1524 |
return true; |
---|
1525 |
|
---|
1526 |
case AllocCMP: |
---|
1527 |
debugOutput(DEBUG_LEVEL_VERBOSE, " allocated using IEC61883 CMP...\n" ); |
---|
1528 |
debugOutput(DEBUG_LEVEL_VERBOSE, " performing IEC61883 CMP disconnect...\n" ); |
---|
1529 |
if(iec61883_cmp_disconnect( |
---|
1530 |
m_handle, |
---|
1531 |
m_channels[c].xmit_node | 0xffc0, |
---|
1532 |
m_channels[c].xmit_plug, |
---|
1533 |
m_channels[c].recv_node | 0xffc0, |
---|
1534 |
m_channels[c].recv_plug, |
---|
1535 |
m_channels[c].channel, |
---|
1536 |
m_channels[c].bandwidth) != 0) { |
---|
1537 |
debugWarning("Could not do CMP disconnect for channel %d!\n",c); |
---|
1538 |
} |
---|
1539 |
if (!unregisterIsoChannel(c)) |
---|
1540 |
return false; |
---|
1541 |
return true; |
---|
1542 |
} |
---|
1543 |
|
---|
1544 |
// unreachable |
---|
1545 |
debugError("BUG: unreachable code reached!\n"); |
---|
1546 |
|
---|
1547 |
return false; |
---|
1548 |
} |
---|
1549 |
|
---|
1550 |
/** |
---|
1551 |
* Registers a channel as managed by this ieee1394service |
---|
1552 |
* @param c channel number |
---|
1553 |
* @param cinfo channel info struct |
---|
1554 |
* @return true if successful |
---|
1555 |
*/ |
---|
1556 |
bool Ieee1394Service::registerIsoChannel(unsigned int c, struct ChannelInfo cinfo) { |
---|
1557 |
if (c < 63) { |
---|
1558 |
if (m_channels[c].alloctype != AllocFree) { |
---|
1559 |
debugWarning("Channel %d already registered with bandwidth %d\n", |
---|
1560 |
m_channels[c].channel, m_channels[c].bandwidth); |
---|
1561 |
} |
---|
1562 |
|
---|
1563 |
memcpy(&m_channels[c], &cinfo, sizeof(struct ChannelInfo)); |
---|
1564 |
|
---|
1565 |
} else return false; |
---|
1566 |
return true; |
---|
1567 |
} |
---|
1568 |
|
---|
1569 |
/** |
---|
1570 |
* unegisters a channel from this ieee1394service |
---|
1571 |
* @param c channel number |
---|
1572 |
* @return true if successful |
---|
1573 |
*/ |
---|
1574 |
bool Ieee1394Service::unregisterIsoChannel(unsigned int c) { |
---|
1575 |
if (c < 63) { |
---|
1576 |
if (m_channels[c].alloctype == AllocFree) { |
---|
1577 |
debugWarning("Channel %d not registered\n", c); |
---|
1578 |
return false; |
---|
1579 |
} |
---|
1580 |
|
---|
1581 |
m_channels[c].channel=-1; |
---|
1582 |
m_channels[c].bandwidth=-1; |
---|
1583 |
m_channels[c].alloctype=AllocFree; |
---|
1584 |
m_channels[c].xmit_node=0xFFFF; |
---|
1585 |
m_channels[c].xmit_plug=-1; |
---|
1586 |
m_channels[c].recv_node=0xFFFF; |
---|
1587 |
m_channels[c].recv_plug=-1; |
---|
1588 |
|
---|
1589 |
} else return false; |
---|
1590 |
return true; |
---|
1591 |
} |
---|
1592 |
|
---|
1593 |
/** |
---|
1594 |
* Returns the current value of the `bandwidth available' register on |
---|
1595 |
* the IRM, or -1 on error. |
---|
1596 |
* @return |
---|
1597 |
*/ |
---|
1598 |
signed int Ieee1394Service::getAvailableBandwidth() { |
---|
1599 |
quadlet_t buffer; |
---|
1600 |
Util::MutexLockHelper lock(*m_handle_lock); |
---|
1601 |
signed int result = raw1394_read (m_handle, raw1394_get_irm_id (m_handle), |
---|
1602 |
CSR_REGISTER_BASE + CSR_BANDWIDTH_AVAILABLE, |
---|
1603 |
sizeof (quadlet_t), &buffer); |
---|
1604 |
|
---|
1605 |
if (result < 0) |
---|
1606 |
return -1; |
---|
1607 |
return CondSwapFromBus32(buffer); |
---|
1608 |
} |
---|
1609 |
|
---|
1610 |
void |
---|
1611 |
Ieee1394Service::setVerboseLevel(int l) |
---|
1612 |
{ |
---|
1613 |
if (m_pIsoManager) m_pIsoManager->setVerboseLevel(l); |
---|
1614 |
if (m_pCTRHelper) m_pCTRHelper->setVerboseLevel(l); |
---|
1615 |
if (m_pWatchdog) m_pWatchdog->setVerboseLevel(l); |
---|
1616 |
setDebugLevel(l); |
---|
1617 |
debugOutput( DEBUG_LEVEL_VERBOSE, "Setting verbose level to %d...\n", l ); |
---|
1618 |
} |
---|
1619 |
|
---|
1620 |
void |
---|
1621 |
Ieee1394Service::show() |
---|
1622 |
{ |
---|
1623 |
#ifdef DEBUG |
---|
1624 |
uint32_t cycle_timer; |
---|
1625 |
uint64_t local_time; |
---|
1626 |
if(!readCycleTimerReg(&cycle_timer, &local_time)) { |
---|
1627 |
debugWarning("Could not read cycle timer register\n"); |
---|
1628 |
|
---|
1629 |
} |
---|
1630 |
uint64_t ctr = CYCLE_TIMER_TO_TICKS( cycle_timer ); |
---|
1631 |
|
---|
1632 |
debugOutput( DEBUG_LEVEL_VERBOSE, "Port: %d\n", getPort() ); |
---|
1633 |
debugOutput( DEBUG_LEVEL_VERBOSE, " Name: %s\n", getPortName().c_str() ); |
---|
1634 |
debugOutput( DEBUG_LEVEL_VERBOSE, " CycleTimerHelper: %p, IsoManager: %p, WatchDog: %p\n", |
---|
1635 |
m_pCTRHelper, m_pIsoManager, m_pWatchdog ); |
---|
1636 |
debugOutput( DEBUG_LEVEL_VERBOSE, " Time: %011" PRIu64 " (%03us %04ucy %04uticks)\n", |
---|
1637 |
ctr, |
---|
1638 |
(unsigned int)TICKS_TO_SECS( ctr ), |
---|
1639 |
(unsigned int)TICKS_TO_CYCLES( ctr ), |
---|
1640 |
(unsigned int)TICKS_TO_OFFSET( ctr ) ); |
---|
1641 |
debugOutputShort( DEBUG_LEVEL_NORMAL, "Iso handler info:\n"); |
---|
1642 |
#endif |
---|
1643 |
if (m_pIsoManager) m_pIsoManager->dumpInfo(); |
---|
1644 |
} |
---|
1645 |
|
---|
1646 |
// the helper thread class |
---|
1647 |
Ieee1394Service::HelperThread::HelperThread(Ieee1394Service &parent, std::string name) |
---|
1648 |
: m_parent( parent ) |
---|
1649 |
, m_name( name ) |
---|
1650 |
, m_handle( NULL ) |
---|
1651 |
, m_thread( *(new Util::PosixThread(this, name, false, 0, PTHREAD_CANCEL_DEFERRED)) ) |
---|
1652 |
, m_iterate( false ) |
---|
1653 |
, m_debugModule(parent.m_debugModule) |
---|
1654 |
{ |
---|
1655 |
m_handle = raw1394_new_handle_on_port( parent.m_port ); |
---|
1656 |
if(!m_handle) { |
---|
1657 |
debugError("Could not allocate handle\n"); |
---|
1658 |
// FIXME: better error handling required |
---|
1659 |
} |
---|
1660 |
raw1394_set_userdata( m_handle, this ); |
---|
1661 |
} |
---|
1662 |
|
---|
1663 |
Ieee1394Service::HelperThread::HelperThread(Ieee1394Service &parent, std::string name, bool rt, int prio) |
---|
1664 |
: m_parent( parent ) |
---|
1665 |
, m_name( name ) |
---|
1666 |
, m_handle( NULL ) |
---|
1667 |
, m_thread( *(new Util::PosixThread(this, name, rt, prio, PTHREAD_CANCEL_DEFERRED)) ) |
---|
1668 |
, m_iterate( false ) |
---|
1669 |
, m_debugModule(parent.m_debugModule) |
---|
1670 |
{ |
---|
1671 |
m_handle = raw1394_new_handle_on_port( parent.m_port ); |
---|
1672 |
if(!m_handle) { |
---|
1673 |
debugError("Could not allocate handle\n"); |
---|
1674 |
// FIXME: better error handling required |
---|
1675 |
} |
---|
1676 |
raw1394_set_userdata( m_handle, this ); |
---|
1677 |
} |
---|
1678 |
|
---|
1679 |
Ieee1394Service::HelperThread::~HelperThread() |
---|
1680 |
{ |
---|
1681 |
m_thread.Stop(); |
---|
1682 |
delete &m_thread; |
---|
1683 |
if(m_handle) { |
---|
1684 |
raw1394_destroy_handle(m_handle); |
---|
1685 |
} |
---|
1686 |
} |
---|
1687 |
|
---|
1688 |
bool |
---|
1689 |
Ieee1394Service::HelperThread::Init() |
---|
1690 |
{ |
---|
1691 |
m_iterate = true; |
---|
1692 |
return true; |
---|
1693 |
} |
---|
1694 |
|
---|
1695 |
bool |
---|
1696 |
Ieee1394Service::HelperThread::Execute() |
---|
1697 |
{ |
---|
1698 |
if(m_iterate) { |
---|
1699 |
int err; |
---|
1700 |
err = raw1394_loop_iterate (m_handle); |
---|
1701 |
if(err < 0) { |
---|
1702 |
debugError("Failed to iterate handler\n"); |
---|
1703 |
return false; |
---|
1704 |
} else { |
---|
1705 |
return true; |
---|
1706 |
} |
---|
1707 |
} else { |
---|
1708 |
Util::SystemTimeSource::SleepUsecRelative(1000); |
---|
1709 |
return true; |
---|
1710 |
} |
---|
1711 |
} |
---|
1712 |
|
---|
1713 |
void |
---|
1714 |
Ieee1394Service::HelperThread::setThreadParameters(bool rt, int priority) |
---|
1715 |
{ |
---|
1716 |
debugOutput( DEBUG_LEVEL_VERBOSE, "(%p) switch to: (rt=%d, prio=%d)...\n", this, rt, priority); |
---|
1717 |
if (priority > THREAD_MAX_RTPRIO) priority = THREAD_MAX_RTPRIO; // cap the priority |
---|
1718 |
if (rt) { |
---|
1719 |
m_thread.AcquireRealTime(priority); |
---|
1720 |
} else { |
---|
1721 |
m_thread.DropRealTime(); |
---|
1722 |
} |
---|
1723 |
} |
---|
1724 |
|
---|
1725 |
bool |
---|
1726 |
Ieee1394Service::HelperThread::Start() |
---|
1727 |
{ |
---|
1728 |
return m_thread.Start() == 0; |
---|
1729 |
} |
---|
1730 |
|
---|
1731 |
bool |
---|
1732 |
Ieee1394Service::HelperThread::Stop() |
---|
1733 |
{ |
---|
1734 |
// request to stop iterating |
---|
1735 |
m_iterate = false; |
---|
1736 |
// poke the handler such that the iterate() returns |
---|
1737 |
raw1394_wake_up(m_handle); |
---|
1738 |
// stop the thread |
---|
1739 |
return m_thread.Stop() == 0; |
---|
1740 |
} |
---|