1 |
/*************************************************************************** |
---|
2 |
Copyright (C) 2005 by Pieter Palmers * |
---|
3 |
* |
---|
4 |
This program is free software; you can redistribute it and/or modify * |
---|
5 |
it under the terms of the GNU General Public License as published by * |
---|
6 |
the Free Software Foundation; either version 2 of the License, or * |
---|
7 |
(at your option) any later version. * |
---|
8 |
* |
---|
9 |
This program is distributed in the hope that it will be useful, * |
---|
10 |
but WITHOUT ANY WARRANTY; without even the implied warranty of * |
---|
11 |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
---|
12 |
GNU General Public License for more details. * |
---|
13 |
* |
---|
14 |
You should have received a copy of the GNU General Public License * |
---|
15 |
along with this program; if not, write to the * |
---|
16 |
Free Software Foundation, Inc., * |
---|
17 |
59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
---|
18 |
***************************************************************************/ |
---|
19 |
|
---|
20 |
#ifdef HAVE_CONFIG_H |
---|
21 |
#include <config.h> |
---|
22 |
#endif |
---|
23 |
|
---|
24 |
#include <argp.h> |
---|
25 |
#include <stdio.h> |
---|
26 |
#include <stdlib.h> |
---|
27 |
#include <string.h> |
---|
28 |
#include <endian.h> |
---|
29 |
|
---|
30 |
#include <signal.h> |
---|
31 |
#include "src/debugmodule/debugmodule.h" |
---|
32 |
|
---|
33 |
#include <netinet/in.h> |
---|
34 |
|
---|
35 |
#include "src/libstreaming/cycletimer.h" |
---|
36 |
|
---|
37 |
#include "src/libstreaming/IsoHandlerManager.h" |
---|
38 |
#include "SytMonitor.h" |
---|
39 |
|
---|
40 |
#include "src/libutil/PosixThread.h" |
---|
41 |
#include "src/libutil/SystemTimeSource.h" |
---|
42 |
|
---|
43 |
#include "pthread.h" |
---|
44 |
|
---|
45 |
using namespace FreebobStreaming; |
---|
46 |
using namespace FreebobUtil; |
---|
47 |
|
---|
48 |
|
---|
49 |
DECLARE_GLOBAL_DEBUG_MODULE; |
---|
50 |
|
---|
51 |
int run; |
---|
52 |
// Program documentation. |
---|
53 |
static char doc[] = "FreeBoB -- SYT monitor application\n\n"; |
---|
54 |
|
---|
55 |
// A description of the arguments we accept. |
---|
56 |
static char args_doc[] = "PORT1,CHANNEL1 [PORT2,CHANNEL2 [... up to 128 combo's]]"; |
---|
57 |
|
---|
58 |
struct port_channel_combo { |
---|
59 |
int channel; |
---|
60 |
int port; |
---|
61 |
}; |
---|
62 |
|
---|
63 |
struct arguments |
---|
64 |
{ |
---|
65 |
short silent; |
---|
66 |
short verbose; |
---|
67 |
int port; |
---|
68 |
int node_id; |
---|
69 |
bool realtime; |
---|
70 |
int rtprio; |
---|
71 |
struct port_channel_combo args[128]; |
---|
72 |
int nb_combos; |
---|
73 |
}; |
---|
74 |
|
---|
75 |
// The options we understand. |
---|
76 |
static struct argp_option options[] = { |
---|
77 |
{"port", 'p', "nr", 0, "IEEE1394 Port to use" }, |
---|
78 |
{"realtime", 'R', 0, 0, "Run with realtime scheduling?" }, |
---|
79 |
{"realtime-priority", 'r', "nr", 0, "Realtime scheduling priority" }, |
---|
80 |
{ 0 } |
---|
81 |
}; |
---|
82 |
|
---|
83 |
//------------------------------------------------------------- |
---|
84 |
|
---|
85 |
// Parse a single option. |
---|
86 |
static error_t |
---|
87 |
parse_opt( int key, char* arg, struct argp_state* state ) |
---|
88 |
{ |
---|
89 |
// Get the input argument from `argp_parse', which we |
---|
90 |
// know is a pointer to our arguments structure. |
---|
91 |
struct arguments* arguments = ( struct arguments* ) state->input; |
---|
92 |
char* tail; |
---|
93 |
|
---|
94 |
switch (key) { |
---|
95 |
case 'p': |
---|
96 |
if (arg) { |
---|
97 |
arguments->port = strtol( arg, &tail, 0 ); |
---|
98 |
if ( errno ) { |
---|
99 |
fprintf( stderr, "Could not parse 'port' argument\n" ); |
---|
100 |
return ARGP_ERR_UNKNOWN; |
---|
101 |
} |
---|
102 |
} else { |
---|
103 |
if ( errno ) { |
---|
104 |
fprintf( stderr, "Could not parse 'port' argumen\n" ); |
---|
105 |
return ARGP_ERR_UNKNOWN; |
---|
106 |
} |
---|
107 |
} |
---|
108 |
break; |
---|
109 |
case 'R': |
---|
110 |
arguments->realtime = true; |
---|
111 |
break; |
---|
112 |
case 'r': |
---|
113 |
if (arg) { |
---|
114 |
arguments->rtprio = strtol( arg, &tail, 0 ); |
---|
115 |
if ( errno ) { |
---|
116 |
fprintf( stderr, "Could not parse 'realtime-priority' argument\n" ); |
---|
117 |
return ARGP_ERR_UNKNOWN; |
---|
118 |
} |
---|
119 |
} |
---|
120 |
break; |
---|
121 |
case ARGP_KEY_ARG: |
---|
122 |
if (state->arg_num >= 128) { |
---|
123 |
// Too many arguments. |
---|
124 |
argp_usage( state ); |
---|
125 |
} |
---|
126 |
|
---|
127 |
if(sscanf( arg, "%d,%d", |
---|
128 |
&arguments->args[state->arg_num].port, |
---|
129 |
&arguments->args[state->arg_num].channel) != 2) { |
---|
130 |
fprintf( stderr, "Could not parse port-channel specification ('%s')\n", arg); |
---|
131 |
|
---|
132 |
} else { |
---|
133 |
printf("Adding Port %d, Channel %d to list...\n", |
---|
134 |
arguments->args[state->arg_num].port, |
---|
135 |
arguments->args[state->arg_num].channel); |
---|
136 |
arguments->nb_combos++; |
---|
137 |
} |
---|
138 |
break; |
---|
139 |
case ARGP_KEY_END: |
---|
140 |
if (state->arg_num < 1) { |
---|
141 |
// Not enough arguments. |
---|
142 |
argp_usage( state ); |
---|
143 |
} |
---|
144 |
break; |
---|
145 |
default: |
---|
146 |
return ARGP_ERR_UNKNOWN; |
---|
147 |
} |
---|
148 |
return 0; |
---|
149 |
} |
---|
150 |
|
---|
151 |
// Our argp parser. |
---|
152 |
static struct argp argp = { options, parse_opt, args_doc, doc }; |
---|
153 |
|
---|
154 |
|
---|
155 |
static void sighandler (int sig) |
---|
156 |
{ |
---|
157 |
run = 0; |
---|
158 |
} |
---|
159 |
|
---|
160 |
int main(int argc, char *argv[]) |
---|
161 |
{ |
---|
162 |
bool run_realtime=false; |
---|
163 |
int realtime_prio=20; |
---|
164 |
int nb_iter; |
---|
165 |
int i; |
---|
166 |
struct sched_param params; |
---|
167 |
uint64_t last_print_time=0; |
---|
168 |
|
---|
169 |
SystemTimeSource masterTimeSource; |
---|
170 |
|
---|
171 |
IsoHandlerManager *m_isoManager=NULL; |
---|
172 |
PosixThread * m_isoManagerThread=NULL; |
---|
173 |
|
---|
174 |
SytMonitor *monitors[128]; |
---|
175 |
int64_t stream_offset_ticks[128]; |
---|
176 |
|
---|
177 |
struct arguments arguments; |
---|
178 |
|
---|
179 |
// Default values. |
---|
180 |
arguments.port = 0; |
---|
181 |
arguments.node_id = 0; |
---|
182 |
arguments.rtprio = 20; |
---|
183 |
arguments.realtime = false; |
---|
184 |
arguments.nb_combos = 0; |
---|
185 |
|
---|
186 |
// Parse our arguments; every option seen by `parse_opt' will |
---|
187 |
// be reflected in `arguments'. |
---|
188 |
if ( argp_parse ( &argp, argc, argv, 0, 0, &arguments ) ) { |
---|
189 |
fprintf( stderr, "Could not parse command line\n" ); |
---|
190 |
goto finish; |
---|
191 |
} |
---|
192 |
|
---|
193 |
memset(&stream_offset_ticks,0,sizeof(unsigned int) * 128); |
---|
194 |
|
---|
195 |
|
---|
196 |
run=1; |
---|
197 |
|
---|
198 |
run_realtime=arguments.realtime; |
---|
199 |
realtime_prio=arguments.rtprio; |
---|
200 |
|
---|
201 |
signal (SIGINT, sighandler); |
---|
202 |
signal (SIGPIPE, sighandler); |
---|
203 |
|
---|
204 |
debugOutput(DEBUG_LEVEL_NORMAL, "Freebob SYT monitor\n"); |
---|
205 |
|
---|
206 |
m_isoManager=new IsoHandlerManager(); |
---|
207 |
|
---|
208 |
if(!m_isoManager) { |
---|
209 |
debugOutput(DEBUG_LEVEL_NORMAL, "Could not create IsoHandlerManager\n"); |
---|
210 |
goto finish; |
---|
211 |
} |
---|
212 |
|
---|
213 |
m_isoManager->setVerboseLevel(DEBUG_LEVEL_VERBOSE); |
---|
214 |
|
---|
215 |
// the thread to execute the manager |
---|
216 |
m_isoManagerThread=new PosixThread( |
---|
217 |
m_isoManager, |
---|
218 |
run_realtime, realtime_prio, |
---|
219 |
PTHREAD_CANCEL_DEFERRED); |
---|
220 |
|
---|
221 |
if(!m_isoManagerThread) { |
---|
222 |
debugOutput(DEBUG_LEVEL_NORMAL, "Could not create iso manager thread\n"); |
---|
223 |
goto finish; |
---|
224 |
} |
---|
225 |
|
---|
226 |
// register monitors |
---|
227 |
for (i=0;i<arguments.nb_combos;i++) { |
---|
228 |
debugOutput(DEBUG_LEVEL_NORMAL, "Registering SytMonitor %d\n",i); |
---|
229 |
|
---|
230 |
// add a stream to the manager so that it has something to do |
---|
231 |
monitors[i]=new SytMonitor(arguments.args[i].port); |
---|
232 |
|
---|
233 |
if (!monitors[i]) { |
---|
234 |
debugOutput(DEBUG_LEVEL_NORMAL, "Could not create SytMonitor %d\n", i); |
---|
235 |
goto finish; |
---|
236 |
} |
---|
237 |
|
---|
238 |
monitors[i]->setVerboseLevel(DEBUG_LEVEL_VERBOSE); |
---|
239 |
|
---|
240 |
if (!monitors[i]->init()) { |
---|
241 |
debugOutput(DEBUG_LEVEL_NORMAL, "Could not init SytMonitor %d\n", i); |
---|
242 |
goto finish; |
---|
243 |
} |
---|
244 |
|
---|
245 |
monitors[i]->setChannel(arguments.args[i].channel); |
---|
246 |
|
---|
247 |
if(!m_isoManager->registerStream(monitors[i])) { |
---|
248 |
debugOutput(DEBUG_LEVEL_NORMAL, "Could not register SytMonitor %d with isoManager\n", i); |
---|
249 |
goto finish; |
---|
250 |
} |
---|
251 |
|
---|
252 |
if (!masterTimeSource.registerSlave(monitors[i]->getHandler())) { |
---|
253 |
debugOutput(DEBUG_LEVEL_NORMAL, "Could not register SytMonitor %d's IsoHandler with masterTimeSource\n", i); |
---|
254 |
goto finish; |
---|
255 |
|
---|
256 |
} |
---|
257 |
} |
---|
258 |
|
---|
259 |
debugOutput(DEBUG_LEVEL_NORMAL, "Preparing IsoHandlerManager...\n"); |
---|
260 |
if (!m_isoManager->prepare()) { |
---|
261 |
debugOutput(DEBUG_LEVEL_NORMAL, "Could not prepare isoManager\n"); |
---|
262 |
goto finish; |
---|
263 |
} |
---|
264 |
|
---|
265 |
debugOutput(DEBUG_LEVEL_NORMAL, "Starting ISO manager sync update thread...\n"); |
---|
266 |
|
---|
267 |
|
---|
268 |
debugOutput(DEBUG_LEVEL_NORMAL, "Starting IsoHandlers...\n"); |
---|
269 |
if (!m_isoManager->startHandlers(0)) { |
---|
270 |
debugOutput(DEBUG_LEVEL_NORMAL, "Could not start handlers...\n"); |
---|
271 |
goto finish; |
---|
272 |
} |
---|
273 |
|
---|
274 |
// start the runner thread |
---|
275 |
m_isoManagerThread->Start(); |
---|
276 |
|
---|
277 |
if (arguments.realtime) { |
---|
278 |
// get rt priority for this thread too. |
---|
279 |
params.sched_priority = arguments.rtprio + 1; |
---|
280 |
if (pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶ms)) { |
---|
281 |
debugWarning("Couldn't set realtime prio for main thread..."); |
---|
282 |
} |
---|
283 |
} |
---|
284 |
|
---|
285 |
// do the actual work |
---|
286 |
nb_iter=0; |
---|
287 |
|
---|
288 |
while(run) { |
---|
289 |
debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"--- Iterate ---\n"); |
---|
290 |
|
---|
291 |
// if(!m_isoManager->iterate()) { |
---|
292 |
// debugFatal("Could not iterate the isoManager\n"); |
---|
293 |
// return false; |
---|
294 |
// } |
---|
295 |
|
---|
296 |
if(!masterTimeSource.updateTimeSource()) { |
---|
297 |
debugFatal("Could not update the masterTimeSource\n"); |
---|
298 |
return false; |
---|
299 |
} |
---|
300 |
|
---|
301 |
debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"--- Process ---\n"); |
---|
302 |
// process the cycle info's |
---|
303 |
if (arguments.nb_combos>0) { |
---|
304 |
int master_guard=0; |
---|
305 |
struct cycle_info master_cif; |
---|
306 |
|
---|
307 |
bool advance_master=true; |
---|
308 |
|
---|
309 |
while (advance_master && monitors[0]->readNextCycleInfo(&master_cif)) { |
---|
310 |
advance_master=true; |
---|
311 |
|
---|
312 |
master_guard++; |
---|
313 |
if(master_guard>1000) { |
---|
314 |
debugWarning("Guard passed on master sync!\n"); |
---|
315 |
break; |
---|
316 |
} |
---|
317 |
// we try to match the packets received on equal cycles |
---|
318 |
|
---|
319 |
// unwrap the seconds counter |
---|
320 |
if (master_cif.seconds==0) master_cif.seconds+=128; |
---|
321 |
|
---|
322 |
debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"MASTER: [%2d: %04us %04uc, %04X]\n", |
---|
323 |
0,master_cif.seconds,master_cif.cycle,master_cif.syt); |
---|
324 |
|
---|
325 |
for (i=1;i<arguments.nb_combos;i++) { |
---|
326 |
struct cycle_info cif; |
---|
327 |
int slave_guard=0; |
---|
328 |
bool read_slave=monitors[i]->readNextCycleInfo(&cif); |
---|
329 |
|
---|
330 |
while(read_slave) { |
---|
331 |
slave_guard++; |
---|
332 |
if(slave_guard>1000) { |
---|
333 |
debugWarning("Guard passed on slave sync %d!\n",i); |
---|
334 |
break; |
---|
335 |
} |
---|
336 |
// unwrap the seconds counter |
---|
337 |
if (cif.seconds==0) cif.seconds+=128; |
---|
338 |
|
---|
339 |
if(cif.cycle==master_cif.cycle |
---|
340 |
&& cif.seconds==master_cif.seconds) { // this is the one |
---|
341 |
debugOutput(DEBUG_LEVEL_VERY_VERBOSE," GOOD : [%2d: %04us %04uc, %04X]\n", |
---|
342 |
i,cif.seconds, cif.cycle,cif.syt); |
---|
343 |
monitors[i]->consumeNextCycleInfo(); |
---|
344 |
|
---|
345 |
// we have a match ;) |
---|
346 |
if ((cif.syt != 0xFFFF) && (master_cif.syt != 0xFFFF)) { |
---|
347 |
// detect seconds wraparound |
---|
348 |
if ((master_cif.pres_seconds==0) && (cif.pres_seconds==127)) { |
---|
349 |
master_cif.pres_ticks += TICKS_PER_SECOND*128LL; |
---|
350 |
} |
---|
351 |
if ((master_cif.pres_seconds==127) && (cif.pres_seconds==0)) { |
---|
352 |
cif.pres_ticks += TICKS_PER_SECOND*128LL; |
---|
353 |
} |
---|
354 |
// average out the offset |
---|
355 |
int64_t err=(((uint64_t)master_cif.pres_ticks) - ((uint64_t)cif.pres_ticks)); |
---|
356 |
|
---|
357 |
err = err - stream_offset_ticks[i]; |
---|
358 |
|
---|
359 |
if(err>50000 || err < -50000) { |
---|
360 |
debugOutput(DEBUG_LEVEL_NORMAL, |
---|
361 |
" Large Diff: %dticks, delta=%d; avg=%d\n", |
---|
362 |
err,(((int)master_cif.pres_ticks) - ((int)cif.pres_ticks)),stream_offset_ticks[i]); |
---|
363 |
|
---|
364 |
debugOutput(DEBUG_LEVEL_NORMAL, |
---|
365 |
" Master : %04X -> %10u (%04us %04uc %04ut)\n", |
---|
366 |
master_cif.syt, master_cif.pres_ticks, |
---|
367 |
master_cif.pres_seconds, master_cif.pres_cycle, master_cif.pres_offset |
---|
368 |
); |
---|
369 |
debugOutput(DEBUG_LEVEL_NORMAL, |
---|
370 |
" [%04us %04uc, %04X (%02uc %04ut)]\n", |
---|
371 |
master_cif.seconds,master_cif.cycle, |
---|
372 |
master_cif.syt, CYCLE_TIMER_GET_CYCLES(master_cif.syt), |
---|
373 |
CYCLE_TIMER_GET_OFFSET(master_cif.syt)); |
---|
374 |
|
---|
375 |
debugOutput(DEBUG_LEVEL_NORMAL, |
---|
376 |
" Current : %04X -> %10u (%04us %04uc %04ut)\n", |
---|
377 |
cif.syt, cif.pres_ticks, |
---|
378 |
cif.pres_seconds, cif.pres_cycle, cif.pres_offset |
---|
379 |
); |
---|
380 |
debugOutput(DEBUG_LEVEL_NORMAL, |
---|
381 |
" [%04us %04uc, %04X (%02uc %04ut)]\n", |
---|
382 |
cif.seconds,cif.cycle, |
---|
383 |
cif.syt, CYCLE_TIMER_GET_CYCLES(cif.syt), |
---|
384 |
CYCLE_TIMER_GET_OFFSET(cif.syt)); |
---|
385 |
debugOutput(DEBUG_LEVEL_NORMAL,"\n"); |
---|
386 |
} |
---|
387 |
|
---|
388 |
stream_offset_ticks[i] += err/1000; |
---|
389 |
|
---|
390 |
debugOutput(DEBUG_LEVEL_VERY_VERBOSE," Difference: %dticks\n", |
---|
391 |
(((int)master_cif.pres_ticks) - ((int)cif.pres_ticks))); |
---|
392 |
} |
---|
393 |
|
---|
394 |
break; |
---|
395 |
} else { |
---|
396 |
if ((cif.seconds < master_cif.seconds) || |
---|
397 |
((cif.seconds == master_cif.seconds) |
---|
398 |
&& (cif.cycle < master_cif.cycle))) { |
---|
399 |
|
---|
400 |
debugOutput(DEBUG_LEVEL_VERY_VERBOSE," LAGS : [%2d: %04us %04uc, %04X]\n", |
---|
401 |
i,cif.seconds, cif.cycle,cif.syt); |
---|
402 |
// the stream lags behind |
---|
403 |
|
---|
404 |
// advance the read pointer |
---|
405 |
// this will always succeed, otherwise we wouldn't be |
---|
406 |
// in this while() |
---|
407 |
monitors[i]->consumeNextCycleInfo(); |
---|
408 |
} else { |
---|
409 |
debugOutput(DEBUG_LEVEL_VERY_VERBOSE," LEADS: [%2d: %04us %04uc, %04X]\n", |
---|
410 |
i,cif.seconds, cif.cycle,cif.syt); |
---|
411 |
|
---|
412 |
// the stream is too far ahead, |
---|
413 |
// don't do anything |
---|
414 |
break; |
---|
415 |
} |
---|
416 |
} |
---|
417 |
read_slave=monitors[i]->readNextCycleInfo(&cif); |
---|
418 |
} |
---|
419 |
|
---|
420 |
// detect an empty buffer |
---|
421 |
if (!read_slave) { |
---|
422 |
debugOutput(DEBUG_LEVEL_VERY_VERBOSE," > No more info's (1)\n"); |
---|
423 |
advance_master=false; |
---|
424 |
} |
---|
425 |
|
---|
426 |
} |
---|
427 |
debugOutputShort(DEBUG_LEVEL_VERY_VERBOSE,"\n"); |
---|
428 |
if (advance_master) { |
---|
429 |
monitors[0]->consumeNextCycleInfo(); |
---|
430 |
} |
---|
431 |
} |
---|
432 |
} |
---|
433 |
|
---|
434 |
// show info every x iterations |
---|
435 |
if (masterTimeSource.getCurrentTimeAsUsecs() |
---|
436 |
- last_print_time > 1000000L) { |
---|
437 |
|
---|
438 |
m_isoManager->dumpInfo(); |
---|
439 |
for (i=0;i<arguments.nb_combos;i++) { |
---|
440 |
monitors[i]->dumpInfo(); |
---|
441 |
debugOutputShort(DEBUG_LEVEL_NORMAL," ==> Stream offset: %10lld ticks (%6.4f ms)\n", |
---|
442 |
stream_offset_ticks[i], (1.0*((double)stream_offset_ticks[i]))/24576.0); |
---|
443 |
} |
---|
444 |
masterTimeSource.printTimeSourceInfo(); |
---|
445 |
last_print_time=masterTimeSource.getCurrentTimeAsUsecs(); |
---|
446 |
} |
---|
447 |
|
---|
448 |
// 125us/packet, so sleep for a while |
---|
449 |
usleep(100); |
---|
450 |
} |
---|
451 |
|
---|
452 |
debugOutput(DEBUG_LEVEL_NORMAL, "Stopping handlers...\n"); |
---|
453 |
if(!m_isoManager->stopHandlers()) { |
---|
454 |
debugOutput(DEBUG_LEVEL_NORMAL, "Could not stop ISO handlers\n"); |
---|
455 |
goto finish; |
---|
456 |
} |
---|
457 |
|
---|
458 |
// stop the sync thread |
---|
459 |
debugOutput(DEBUG_LEVEL_NORMAL, "Stopping ISO manager sync update thread...\n"); |
---|
460 |
m_isoManagerThread->Stop(); |
---|
461 |
|
---|
462 |
// unregister monitors |
---|
463 |
for (i=0;i<arguments.nb_combos;i++) { |
---|
464 |
debugOutput(DEBUG_LEVEL_NORMAL, "Unregistering SytMonitor %d\n",i); |
---|
465 |
|
---|
466 |
if(!m_isoManager->unregisterStream(monitors[i])) { |
---|
467 |
debugOutput(DEBUG_LEVEL_NORMAL, "Could not unregister SytMonitor %d\n",i); |
---|
468 |
goto finish; |
---|
469 |
} |
---|
470 |
delete monitors[i]; |
---|
471 |
} |
---|
472 |
|
---|
473 |
delete m_isoManagerThread; |
---|
474 |
delete m_isoManager; |
---|
475 |
|
---|
476 |
finish: |
---|
477 |
debugOutput(DEBUG_LEVEL_NORMAL, "Bye...\n"); |
---|
478 |
|
---|
479 |
return EXIT_SUCCESS; |
---|
480 |
} |
---|