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

Revision 719, 16.1 kB (checked in by ppalmers, 16 years ago)

backup commit

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