iodelay.cpp 7.09 KB
Newer Older
sletz's avatar
sletz committed
1
/*
sletz's avatar
sletz committed
2
    Copyright (C) 2003-2008 Fons Adriaensen <fons@kokkinizita.net>
3
    
sletz's avatar
sletz committed
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

// --------------------------------------------------------------------------------

#include <stdlib.h>
#include <stdio.h>
23
#include <stdint.h>
24
#include <math.h>
sletz's avatar
sletz committed
25
#include <unistd.h>
sletz's avatar
sletz committed
26
#include <jack/jack.h>
sletz's avatar
sletz committed
27

28
struct Freq
sletz's avatar
sletz committed
29
30
31
32
33
{
    int   p;
    int   f;
    float xa;
    float ya;
34
35
36
37
    float x1;
    float y1;
    float x2;
    float y2;
sletz's avatar
sletz committed
38
39
40
};


41
42
struct MTDM
{
sletz's avatar
sletz committed
43
44
    double  _del;
    double  _err;
45
    float   _wlp;
sletz's avatar
sletz committed
46
47
    int     _cnt;
    int     _inv;
48
49

    struct Freq _freq [13];
sletz's avatar
sletz committed
50
51
};

52
53

struct MTDM * mtdm_new (double fsamp)
sletz's avatar
sletz committed
54
55
{
    int   i;
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
    struct Freq  *F;

    struct MTDM *retval = (MTDM *)malloc( sizeof(struct MTDM) );

    if (retval==NULL)
	    return NULL;

    retval->_cnt = 0;
    retval->_inv = 0;

    retval->_freq [0].f  = 4096;
    retval->_freq [1].f  = 2048;
    retval->_freq [2].f  = 3072;
    retval->_freq [3].f  = 2560;
    retval->_freq [4].f  = 2304;
    retval->_freq [5].f  = 2176; 
    retval->_freq [6].f  = 1088;
    retval->_freq [7].f  = 1312;
    retval->_freq [8].f  = 1552;
    retval->_freq [9].f  = 1800;
    retval->_freq [10].f = 3332;
    retval->_freq [11].f = 3586;
    retval->_freq [12].f = 3841;
    retval->_wlp = 200.0f / fsamp;
    for (i = 0, F = retval->_freq; i < 13; i++, F++) {
Stephane Letz's avatar
Stephane Letz committed
81
82
        F->p = 128;
        F->xa = F->ya = 0.0f;
83
84
        F->x1 = F->y1 = 0.0f;
        F->x2 = F->y2 = 0.0f;
sletz's avatar
sletz committed
85
    }
86
87

    return retval;
sletz's avatar
sletz committed
88
89
}

90
int mtdm_process (struct MTDM *self, size_t len, float *ip, float *op)
sletz's avatar
sletz committed
91
92
93
{
    int    i;
    float  vip, vop, a, c, s;
94
    struct Freq   *F;
sletz's avatar
sletz committed
95
96
97
98

    while (len--)
    {
        vop = 0.0f;
Stephane Letz's avatar
Stephane Letz committed
99
        vip = *ip++;
100
        for (i = 0, F = self->_freq; i < 13; i++, F++)
Stephane Letz's avatar
Stephane Letz committed
101
        {
102
            a = 2 * (float) M_PI * (F->p & 65535) / 65536.0; 
Stephane Letz's avatar
Stephane Letz committed
103
            F->p += F->f;
104
105
106
            c =  cosf (a); 
            s = -sinf (a); 
            vop += (i ? 0.01f : 0.20f) * s;
Stephane Letz's avatar
Stephane Letz committed
107
108
            F->xa += s * vip;
            F->ya += c * vip;
109
        } 
Stephane Letz's avatar
Stephane Letz committed
110
        *op++ = vop;
111
        if (++self->_cnt == 16)
Stephane Letz's avatar
Stephane Letz committed
112
        {
113
            for (i = 0, F = self->_freq; i < 13; i++, F++)
Stephane Letz's avatar
Stephane Letz committed
114
            {
115
116
117
118
                F->x1 += self->_wlp * (F->xa - F->x1 + 1e-20);
                F->y1 += self->_wlp * (F->ya - F->y1 + 1e-20);
                F->x2 += self->_wlp * (F->x1 - F->x2 + 1e-20);
                F->y2 += self->_wlp * (F->y1 - F->y2 + 1e-20);
Stephane Letz's avatar
Stephane Letz committed
119
120
                F->xa = F->ya = 0.0f;
            }
121
            self->_cnt = 0;
Stephane Letz's avatar
Stephane Letz committed
122
        }
sletz's avatar
sletz committed
123
124
125
126
127
    }

    return 0;
}

128
int mtdm_resolve (struct MTDM *self)
sletz's avatar
sletz committed
129
130
131
{
    int     i, k, m;
    double  d, e, f0, p;
132
    struct Freq *F = self->_freq;
sletz's avatar
sletz committed
133

134
135
136
137
138
    if (hypot (F->x2, F->y2) < 0.001) return -1;
    d = atan2 (F->y2, F->x2) / (2 * M_PI);
    if (self->_inv) d += 0.5;
    if (d > 0.5) d -= 1.0;
    f0 = self->_freq [0].f;
sletz's avatar
sletz committed
139
    m = 1;
140
141
    self->_err = 0.0;
    for (i = 0; i < 12; i++)
sletz's avatar
sletz committed
142
    {
Stephane Letz's avatar
Stephane Letz committed
143
        F++;
144
145
        p = atan2 (F->y2, F->x2) / (2 * M_PI) - d * F->f / f0;
        if (self->_inv) p += 0.5;
Stephane Letz's avatar
Stephane Letz committed
146
        p -= floor (p);
147
        p *= 2;
Stephane Letz's avatar
Stephane Letz committed
148
149
        k = (int)(floor (p + 0.5));
        e = fabs (p - k);
150
151
152
153
154
155
        if (e > self->_err) self->_err = e;
        if (e > 0.4) return 1; 
        d += m * (k & 1);
        m *= 2;
    }  
    self->_del = 16 * d;
sletz's avatar
sletz committed
156
157
158
159

    return 0;
}

160
161
162
163
void mtdm_invert (struct MTDM *self)
{
	self->_inv ^= 1;
}
sletz's avatar
sletz committed
164
// --------------------------------------------------------------------------------
sletz's avatar
sletz committed
165

166
static struct MTDM    *mtdm;
sletz's avatar
sletz committed
167
168
169
170
static jack_client_t  *jack_handle;
static jack_port_t    *jack_capt;
static jack_port_t    *jack_play;

171
172
jack_latency_range_t   capture_latency = {UINT32_MAX, UINT32_MAX};
jack_latency_range_t   playback_latency = {UINT32_MAX, UINT32_MAX};
sletz's avatar
sletz committed
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198

void
latency_cb (jack_latency_callback_mode_t mode, void *arg)
{
	jack_latency_range_t range;

	range.min = range.max = 0;

	if (mode == JackCaptureLatency) {
		jack_port_set_latency_range (jack_play, mode, &range);
		jack_port_get_latency_range (jack_capt, mode, &range);
		if ((range.min != capture_latency.min) || (range.max != capture_latency.max)) {
			capture_latency = range;
			printf ("new capture latency: [%d, %d]\n", range.min, range.max);
		}
	} else {
		jack_port_set_latency_range (jack_capt, mode, &range);
		jack_port_get_latency_range (jack_play, mode, &range);
		if ((range.min != playback_latency.min) || (range.max != playback_latency.max)) {
			playback_latency = range;
			printf ("new playback latency: [%d, %d]\n", range.min, range.max);
		}
	}

}

sletz's avatar
sletz committed
199
int jack_callback (jack_nframes_t nframes, void *arg)
sletz's avatar
sletz committed
200
201
202
203
204
{
    float *ip, *op;

    ip = (float *)(jack_port_get_buffer (jack_capt, nframes));
    op = (float *)(jack_port_get_buffer (jack_play, nframes));
205
    mtdm_process (mtdm, nframes, ip, op);
sletz's avatar
sletz committed
206
    return 0;
sletz's avatar
sletz committed
207
208
209
210
}

int main (int ac, char *av [])
{
sletz's avatar
sletz committed
211
212
    float          t;
    jack_status_t  s;
sletz's avatar
sletz committed
213

sletz's avatar
sletz committed
214
215
    jack_handle = jack_client_open ("jack_delay", JackNoStartServer, &s);
    if (jack_handle == 0)
sletz's avatar
sletz committed
216
    {
sletz's avatar
sletz committed
217
218
        fprintf (stderr, "Can't connect to Jack, is the server running ?\n");
        exit (1);
sletz's avatar
sletz committed
219
220
    }

221
222
    mtdm = mtdm_new(jack_get_sample_rate(jack_handle));

sletz's avatar
sletz committed
223
    jack_set_process_callback (jack_handle, jack_callback, 0);
sletz's avatar
sletz committed
224

sletz's avatar
sletz committed
225
226
227
    if (jack_set_latency_callback)
	    jack_set_latency_callback (jack_handle, latency_cb, 0);

sletz's avatar
sletz committed
228
229
230
    jack_capt = jack_port_register (jack_handle, "in",  JACK_DEFAULT_AUDIO_TYPE, JackPortIsInput, 0);
    jack_play = jack_port_register (jack_handle, "out", JACK_DEFAULT_AUDIO_TYPE, JackPortIsOutput, 0);

sletz's avatar
sletz committed
231
232
    t = 1000.0f / jack_get_sample_rate (jack_handle);

sletz's avatar
sletz committed
233
234
    if (jack_activate (jack_handle))
    {
sletz's avatar
sletz committed
235
        fprintf(stderr, "Can't activate Jack");
sletz's avatar
sletz committed
236
237
238
239
240
        return 1;
    }

    while (1)
    {
241
242
243
244
245
246
247
248
 
    #ifdef WIN32 
        Sleep (250); 
    #else 
        usleep (250000); 
 	#endif        
        if (mtdm_resolve (mtdm) < 0) printf ("Signal below threshold...\n");
        else 
sletz's avatar
sletz committed
249
        {
250
            jack_nframes_t systemic_latency;
251
252

            if (mtdm->_err > 0.3) 
sletz's avatar
sletz committed
253
            {
254
255
                mtdm_invert ( mtdm );
                mtdm_resolve ( mtdm );
sletz's avatar
sletz committed
256
            }
257
            systemic_latency = (jack_nframes_t) floor (mtdm->_del - (capture_latency.max + playback_latency.max));
258

259
            printf ("%10.3lf frames %10.3lf ms total roundtrip latency\n\textra loopback latency: %u frames\n\tuse %u for the backend arguments -I and -O", mtdm->_del, mtdm->_del * t, 
260
                    systemic_latency, systemic_latency/2);
261
262
            if (mtdm->_err > 0.2) printf (" ??");
                if (mtdm->_inv) printf (" Inv");
sletz's avatar
sletz committed
263
264
            printf ("\n");
        }
sletz's avatar
sletz committed
265
266
267
268
269
    }

    return 0;
}

270
// --------------------------------------------------------------------------------