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

Revision 384, 18.1 kB (checked in by pieterpalmers, 17 years ago)

- temporary commit as backup measure
- rewrote synchronisation code
- receive streaming based on SYT works
- transmit streaming synced to received stream sort of works, still

have to iron out some issues.

NOTE: all devices but the bebob's are disabled in this code,

because they still have to be ported to the new sync
mechanism.

Line 
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, &params)) {
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 }
Note: See TracBrowser for help on using the browser.