root/branches/streaming-rework/tests/test-sytmonitor.cpp

Revision 419, 17.5 kB (checked in by pieterpalmers, 16 years ago)

namespace simplification

Line 
1 /***************************************************************************
2 Copyright (C) 2007 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 Streaming;
46 using namespace Util;
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    
173     SytMonitor *monitors[128];
174     int64_t stream_offset_ticks[128];
175
176     struct arguments arguments;
177
178     // Default values.
179     arguments.port        = 0;
180     arguments.node_id     = 0;
181     arguments.rtprio      = 20;
182     arguments.realtime    = false;
183     arguments.nb_combos   = 0;
184
185     // Parse our arguments; every option seen by `parse_opt' will
186     // be reflected in `arguments'.
187     if ( argp_parse ( &argp, argc, argv, 0, 0, &arguments ) ) {
188         fprintf( stderr, "Could not parse command line\n" );
189         goto finish;
190     }
191    
192     memset(&stream_offset_ticks, 0, sizeof(int64_t) * 128);
193    
194     run=1;
195    
196     run_realtime=arguments.realtime;
197     realtime_prio=arguments.rtprio;
198
199     signal (SIGINT, sighandler);
200     signal (SIGPIPE, sighandler);
201
202     debugOutput(DEBUG_LEVEL_NORMAL, "Freebob SYT monitor\n");
203    
204     m_isoManager=new IsoHandlerManager();
205    
206     if(!m_isoManager) {
207         debugOutput(DEBUG_LEVEL_NORMAL, "Could not create IsoHandlerManager\n");
208         goto finish;
209     }
210        
211     m_isoManager->setVerboseLevel(DEBUG_LEVEL_VERBOSE);
212            
213     if(!m_isoManager->init()) {
214         debugOutput(DEBUG_LEVEL_NORMAL, "Could not init() IsoHandlerManager\n");
215         goto finish;
216     }
217
218         // register monitors
219         for (i=0;i<arguments.nb_combos;i++) {
220             debugOutput(DEBUG_LEVEL_NORMAL, "Registering SytMonitor %d\n",i);
221        
222             // add a stream to the manager so that it has something to do
223             monitors[i]=new SytMonitor(arguments.args[i].port);
224            
225             if (!monitors[i]) {
226                 debugOutput(DEBUG_LEVEL_NORMAL, "Could not create SytMonitor %d\n", i);
227                 goto finish;
228             }   
229        
230             monitors[i]->setVerboseLevel(DEBUG_LEVEL_VERBOSE);
231        
232             if (!monitors[i]->init()) {
233                 debugOutput(DEBUG_LEVEL_NORMAL, "Could not init SytMonitor %d\n", i);
234                 goto finish;
235             }
236        
237             monitors[i]->setChannel(arguments.args[i].channel);
238            
239             if(!m_isoManager->registerStream(monitors[i])) {
240                 debugOutput(DEBUG_LEVEL_NORMAL, "Could not register SytMonitor %d with isoManager\n", i);
241                 goto finish;
242             }
243
244         }
245
246         debugOutput(DEBUG_LEVEL_NORMAL,   "Preparing IsoHandlerManager...\n");
247         if (!m_isoManager->prepare()) {
248                 debugOutput(DEBUG_LEVEL_NORMAL, "Could not prepare isoManager\n");
249                 goto finish;
250         }
251
252         debugOutput(DEBUG_LEVEL_NORMAL,   "Starting ISO manager sync update thread...\n");
253
254
255         debugOutput(DEBUG_LEVEL_NORMAL,   "Starting IsoHandlers...\n");
256         if (!m_isoManager->startHandlers(0)) {
257                 debugOutput(DEBUG_LEVEL_NORMAL, "Could not start handlers...\n");
258                 goto finish;
259         }
260        
261         if (arguments.realtime) {
262             // get rt priority for this thread too.
263             params.sched_priority = arguments.rtprio + 1;
264             if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &params)) {
265                 debugWarning("Couldn't set realtime prio for main thread...");
266             }
267         }
268    
269         // do the actual work
270         nb_iter=0;
271        
272         while(run) {
273         debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"--- Iterate ---\n");
274        
275 //         if(!m_isoManager->iterate()) {
276 //             debugFatal("Could not iterate the isoManager\n");
277 //             return false;
278 //         }
279        
280         if(!masterTimeSource.updateTimeSource()) {
281             debugFatal("Could not update the masterTimeSource\n");
282             return false;
283         }
284        
285         debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"--- Process ---\n");
286         // process the cycle info's
287         if (arguments.nb_combos>0) {
288             int master_guard=0;
289             struct cycle_info master_cif;
290            
291             bool advance_master=true;
292            
293             while (advance_master && monitors[0]->readNextCycleInfo(&master_cif)) {
294                 advance_master=true;
295            
296                 master_guard++;
297                 if(master_guard>1000) {
298                     debugWarning("Guard passed on master sync!\n");
299                     break;
300                 }           
301                 // we try to match the packets received on equal cycles
302                
303                 // unwrap the seconds counter
304                 if (master_cif.seconds==0) master_cif.seconds+=128;
305                
306                 debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"MASTER: [%2d: %04us %04uc, %04X]\n",
307                     0,master_cif.seconds,master_cif.cycle,master_cif.syt);
308                
309                 for (i=1;i<arguments.nb_combos;i++) {
310                     struct cycle_info cif;
311                     int slave_guard=0;
312                     bool read_slave=monitors[i]->readNextCycleInfo(&cif);
313                    
314                     while(read_slave) {
315                         slave_guard++;
316                         if(slave_guard>1000) {
317                             debugWarning("Guard passed on slave sync %d!\n",i);
318                             break;
319                         }
320                         // unwrap the seconds counter
321                         if (cif.seconds==0) cif.seconds+=128;
322
323                         if(cif.cycle==master_cif.cycle
324                         && cif.seconds==master_cif.seconds) { // this is the one
325                             debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"  GOOD : [%2d: %04us %04uc, %04X]\n",
326                                 i,cif.seconds, cif.cycle,cif.syt);
327                             monitors[i]->consumeNextCycleInfo();
328                            
329                             // we have a match ;)
330                             if ((cif.syt != 0xFFFF) && (master_cif.syt != 0xFFFF)) {
331                                 // detect seconds wraparound
332                                 if ((master_cif.pres_seconds==0) && (cif.pres_seconds==127)) {
333                                     master_cif.pres_ticks += TICKS_PER_SECOND*128LL;
334                                 }
335                                 if ((master_cif.pres_seconds==127) && (cif.pres_seconds==0)) {
336                                     cif.pres_ticks += TICKS_PER_SECOND*128LL;
337                                 }
338                             // average out the offset
339                                 int64_t err=(((uint64_t)master_cif.pres_ticks) - ((uint64_t)cif.pres_ticks));
340                                
341                                 debugOutput(DEBUG_LEVEL_NORMAL,"Diff for %d at cycle %04d: %6lld (MTS: %11llu | STS: %11llu\n",
342                                     i,cif.cycle,err, master_cif.pres_ticks, cif.pres_ticks);
343                                
344                                 err = err - stream_offset_ticks[i];
345                                
346                                 if(err>50000 || err < -50000) {
347                                     debugOutput(DEBUG_LEVEL_NORMAL,
348                                         " Large Diff: %dticks, delta=%d; avg=%d\n",
349                                         err,(((int)master_cif.pres_ticks) - ((int)cif.pres_ticks)),stream_offset_ticks[i]);
350                                    
351                                     debugOutput(DEBUG_LEVEL_NORMAL,
352                                         "  Master   : %04X -> %10u (%04us %04uc %04ut)\n",
353                                         master_cif.syt, master_cif.pres_ticks,
354                                         master_cif.pres_seconds, master_cif.pres_cycle, master_cif.pres_offset
355                                         );
356                                     debugOutput(DEBUG_LEVEL_NORMAL,
357                                         "             [%04us %04uc, %04X (%02uc %04ut)]\n",
358                                         master_cif.seconds,master_cif.cycle,
359                                         master_cif.syt, CYCLE_TIMER_GET_CYCLES(master_cif.syt),
360                                         CYCLE_TIMER_GET_OFFSET(master_cif.syt));
361                                        
362                                     debugOutput(DEBUG_LEVEL_NORMAL,
363                                         "  Current  : %04X -> %10u (%04us %04uc %04ut)\n",
364                                         cif.syt, cif.pres_ticks,
365                                         cif.pres_seconds, cif.pres_cycle, cif.pres_offset
366                                         );
367                                     debugOutput(DEBUG_LEVEL_NORMAL,
368                                         "             [%04us %04uc, %04X (%02uc %04ut)]\n",
369                                         cif.seconds,cif.cycle,
370                                         cif.syt, CYCLE_TIMER_GET_CYCLES(cif.syt),
371                                         CYCLE_TIMER_GET_OFFSET(cif.syt));
372                                     debugOutput(DEBUG_LEVEL_NORMAL,"\n");
373                                 }
374                                
375                                 stream_offset_ticks[i] += err/1000;
376                                    
377                                 debugOutput(DEBUG_LEVEL_VERY_VERBOSE," Difference: %dticks\n",
378                                         (((int)master_cif.pres_ticks) - ((int)cif.pres_ticks)));
379                             }
380                            
381                             break;
382                         } else {
383                             if ((cif.seconds < master_cif.seconds) ||
384                                 ((cif.seconds == master_cif.seconds)
385                                 && (cif.cycle < master_cif.cycle))) {
386                                
387                                 debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"  LAGS : [%2d: %04us %04uc, %04X]\n",
388                                     i,cif.seconds, cif.cycle,cif.syt);
389                                 // the stream lags behind
390                                
391                                 // advance the read pointer
392                                 // this will always succeed, otherwise we wouldn't be
393                                 // in this while()
394                                 monitors[i]->consumeNextCycleInfo();
395                             } else {
396                                 debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"  LEADS: [%2d: %04us %04uc, %04X]\n",
397                                     i,cif.seconds, cif.cycle,cif.syt);
398                            
399                                 // the stream is too far ahead,
400                                 // don't do anything
401                                 break;
402                             }
403                         }
404                         read_slave=monitors[i]->readNextCycleInfo(&cif);
405                     }
406
407                     // detect an empty buffer
408                     if (!read_slave) {
409                         debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"    > No more info's (1)\n");
410                         advance_master=false;
411                     }
412
413                 }
414                 debugOutputShort(DEBUG_LEVEL_VERY_VERBOSE,"\n");
415                 if (advance_master) {
416                     monitors[0]->consumeNextCycleInfo();
417                 }
418             }
419         }
420        
421             // show info every x iterations
422             if (masterTimeSource.getCurrentTimeAsUsecs()
423                 - last_print_time > 1000000L) {
424                
425                 m_isoManager->dumpInfo();
426                 for (i=0;i<arguments.nb_combos;i++) {
427                     monitors[i]->dumpInfo();
428                     debugOutputShort(DEBUG_LEVEL_NORMAL,"    ==> Stream offset: %10lld ticks (%6.4f ms)\n",
429                     stream_offset_ticks[i], (1.0*((double)stream_offset_ticks[i]))/24576.0);
430                 }
431                 masterTimeSource.printTimeSourceInfo();
432                 last_print_time=masterTimeSource.getCurrentTimeAsUsecs();
433             }
434            
435             // 125us/packet, so sleep for a while
436             usleep(100);
437         }
438
439         debugOutput(DEBUG_LEVEL_NORMAL,   "Stopping handlers...\n");
440         if(!m_isoManager->stopHandlers()) {
441             debugOutput(DEBUG_LEVEL_NORMAL, "Could not stop ISO handlers\n");
442             goto finish;
443         }
444        
445         // unregister monitors
446         for (i=0;i<arguments.nb_combos;i++) {
447         debugOutput(DEBUG_LEVEL_NORMAL, "Unregistering SytMonitor %d\n",i);
448        
449         if(!m_isoManager->unregisterStream(monitors[i])) {
450             debugOutput(DEBUG_LEVEL_NORMAL, "Could not unregister SytMonitor %d\n",i);
451             goto finish;
452             }
453             delete monitors[i];
454     }
455        
456     delete m_isoManager;
457
458 finish:
459         debugOutput(DEBUG_LEVEL_NORMAL, "Bye...\n");
460
461 return EXIT_SUCCESS;
462 }
Note: See TracBrowser for help on using the browser.