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

Revision 392, 18.2 kB (checked in by pieterpalmers, 16 years ago)

- document TimestampedBuffer? class
- partially ported timestamp handling to TimestampedBuffer?
- introduced test for TimestampedBuffer? class

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