1 /*-
2 * Copyright (c) 2014 Hans Petter Selasky
3 *
4 * Redistribution and use in source and binary forms, with or without
5 * modification, are permitted provided that the following conditions
6 * are met:
7 * 1. Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * 2. Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 *
13 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
14 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
15 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
16 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
17 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
18 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
19 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
20 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
21 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
22 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
23 * SUCH DAMAGE.
24 */
25
26 #include <sys/queue.h>
27
28 #include <stdint.h>
29 #include <stdlib.h>
30 #include <string.h>
31 #include <fcntl.h>
32 #include <unistd.h>
33 #include <err.h>
34 #include <math.h>
35 #include <sysexits.h>
36
37 #include "int.h"
38
39 #define REF_FREQ 500 /* HZ */
40
41 uint32_t voss_ad_last_delay;
42 uint8_t voss_ad_enabled;
43 uint8_t voss_ad_output_signal;
44 uint8_t voss_ad_input_channel;
45 uint8_t voss_ad_output_channel;
46
47 static struct voss_ad {
48 double *wave;
49
50 double *sin_a;
51 double *cos_a;
52
53 double *sin_b;
54 double *cos_b;
55
56 double *buf_a;
57 double *buf_b;
58
59 double sum_sin_a;
60 double sum_cos_a;
61
62 double sum_sin_b;
63 double sum_cos_b;
64
65 uint32_t len_a;
66 uint32_t len_b;
67
68 uint32_t offset_a;
69 uint32_t offset_b;
70 } voss_ad;
71
72 void
voss_ad_reset(void)73 voss_ad_reset(void)
74 {
75 uint32_t x;
76
77 for (x = 0; x != voss_ad.len_a; x++)
78 voss_ad.buf_a[x] = 0;
79
80 for (x = 0; x != voss_ad.len_b; x++)
81 voss_ad.buf_b[x] = 0;
82
83 voss_ad.sum_sin_a = 0;
84 voss_ad.sum_cos_a = 0;
85 voss_ad.sum_sin_b = 0;
86 voss_ad.sum_cos_b = 0;
87
88 voss_ad.offset_a = 0;
89 voss_ad.offset_b = 0;
90
91 voss_ad_last_delay = 0;
92 }
93
94 void
voss_ad_init(uint32_t rate)95 voss_ad_init(uint32_t rate)
96 {
97 double freq;
98 int samples;
99 int len;
100 int x;
101
102 len = sqrt(rate);
103
104 samples = len * len;
105
106 voss_ad.wave = malloc(sizeof(voss_ad.wave[0]) * samples);
107
108 voss_ad.sin_a = malloc(sizeof(voss_ad.sin_a[0]) * len);
109 voss_ad.cos_a = malloc(sizeof(voss_ad.cos_a[0]) * len);
110 voss_ad.buf_a = malloc(sizeof(voss_ad.buf_a[0]) * len);
111 voss_ad.len_a = len;
112
113 voss_ad.sin_b = malloc(sizeof(voss_ad.sin_b[0]) * samples);
114 voss_ad.cos_b = malloc(sizeof(voss_ad.cos_b[0]) * samples);
115 voss_ad.buf_b = malloc(sizeof(voss_ad.buf_b[0]) * samples);
116 voss_ad.len_b = samples;
117
118 if (voss_ad.sin_a == NULL || voss_ad.cos_a == NULL ||
119 voss_ad.sin_b == NULL || voss_ad.cos_b == NULL ||
120 voss_ad.buf_a == NULL || voss_ad.buf_b == NULL)
121 errx(EX_SOFTWARE, "Out of memory");
122
123 freq = 1.0;
124
125 while (1) {
126 double temp = freq * ((double)rate) / ((double)len);
127 if (temp >= REF_FREQ)
128 break;
129 freq += 1.0;
130 }
131
132 for (x = 0; x != len; x++) {
133 voss_ad.sin_a[x] = sin(freq * 2.0 * M_PI * ((double)x) / ((double)len));
134 voss_ad.cos_a[x] = cos(freq * 2.0 * M_PI * ((double)x) / ((double)len));
135 voss_ad.buf_a[x] = 0;
136 }
137
138 for (x = 0; x != samples; x++) {
139
140 voss_ad.wave[x] = sin(freq * 2.0 * M_PI * ((double)x) / ((double)len)) *
141 (1.0 + sin(2.0 * M_PI * ((double)x) / ((double)samples))) / 2.0;
142
143 voss_ad.sin_b[x] = sin(2.0 * M_PI * ((double)x) / ((double)samples));
144 voss_ad.cos_b[x] = cos(2.0 * M_PI * ((double)x) / ((double)samples));
145 voss_ad.buf_b[x] = 0;
146 }
147 }
148
149 static double
voss_add_decode_offset(double x,double y)150 voss_add_decode_offset(double x /* cos */, double y /* sin */)
151 {
152 uint32_t v;
153 double r;
154
155 r = sqrt((x * x) + (y * y));
156
157 if (r == 0.0)
158 return (0);
159
160 x /= r;
161 y /= r;
162
163 v = 0;
164
165 if (y < 0) {
166 v |= 1;
167 y = -y;
168 }
169 if (x < 0) {
170 v |= 2;
171 x = -x;
172 }
173
174 if (y < x) {
175 r = acos(y);
176 } else {
177 r = asin(x);
178 }
179
180 switch (v) {
181 case 0:
182 r = (2.0 * M_PI) - r;
183 break;
184 case 1:
185 r = M_PI + r;
186 break;
187 case 3:
188 r = M_PI - r;
189 break;
190 default:
191 break;
192 }
193 return (r);
194 }
195
196 double
voss_ad_getput_sample(double sample)197 voss_ad_getput_sample(double sample)
198 {
199 double retval;
200 double phase;
201 uint32_t xa;
202 uint32_t xb;
203
204 xa = voss_ad.offset_a;
205 xb = voss_ad.offset_b;
206 retval = voss_ad.wave[xb];
207
208 sample -= voss_ad.buf_a[xa];
209 voss_ad.sum_sin_a += voss_ad.sin_a[xa] * sample;
210 voss_ad.sum_cos_a += voss_ad.cos_a[xa] * sample;
211 voss_ad.buf_a[xa] += sample;
212
213 sample = sqrt((voss_ad.sum_sin_a * voss_ad.sum_sin_a) +
214 (voss_ad.sum_cos_a * voss_ad.sum_cos_a));
215
216 sample -= voss_ad.buf_b[xb];
217 voss_ad.sum_sin_b += voss_ad.sin_b[xb] * sample;
218 voss_ad.sum_cos_b += voss_ad.cos_b[xb] * sample;
219 voss_ad.buf_b[xb] += sample;
220
221 if (++xa == voss_ad.len_a)
222 xa = 0;
223
224 if (++xb == voss_ad.len_b) {
225 xb = 0;
226
227 phase = voss_add_decode_offset(
228 voss_ad.sum_cos_b, voss_ad.sum_sin_b);
229
230 voss_ad_last_delay = (uint32_t)(phase * (double)(voss_ad.len_b) / (2.0 * M_PI)) - (voss_ad.len_a / 2);
231 if (voss_ad_last_delay > voss_ad.len_b)
232 voss_ad_last_delay = voss_ad.len_b;
233 }
234 voss_ad.offset_a = xa;
235 voss_ad.offset_b = xb;
236
237 return (retval * (1LL << voss_ad_output_signal));
238 }
239