root/trunk/libffado/tests/test-sytmonitor.cpp

Revision 445, 16.3 kB (checked in by pieterpalmers, 17 years ago)

* name change from FreeBoB to FFADO
* replaced tabs by 4 spaces
* got rid of end-of-line spaces
* made all license and copyrights conform

library becomes LGPL, apps become GPL
explicitly state LGPL v2.1 and GPL v2 (don't like v3 draft)

copyrights are 2005-2007 Daniel & Pieter
except for the MotU stuff (C) Jonathan, Pieter

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