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

Revision 393, 17.7 kB (checked in by pieterpalmers, 16 years ago)

- fixed some bugs in the timestampedbuffer
- cleaned up the amdtpstreamprocessor
- updated test-sytmonitor and test-cycletimer

to the new threading structure

- implemented test for timestampedbuffer

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