1 /*-
2 * Copyright (c) 2021 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 <string.h>
30 #include <stdlib.h>
31 #include <errno.h>
32
33 #include "int.h"
34
35 void
vclient_tx_equalizer(struct virtual_client * pvc,int64_t * src,size_t total)36 vclient_tx_equalizer(struct virtual_client *pvc,
37 int64_t *src, size_t total)
38 {
39 double *f_data;
40 size_t channels;
41 size_t f_size;
42 size_t x;
43
44 f_size = pvc->profile->tx_filter_size;
45 if (f_size == 0 || total == 0)
46 return;
47
48 channels = pvc->channels;
49 total /= channels;
50
51 while (1) {
52 size_t delta;
53 size_t offset;
54 size_t y;
55
56 offset = pvc->tx_filter_offset;
57 delta = f_size - offset;
58
59 if (delta > total)
60 delta = total;
61
62 for (x = 0; x != channels; x++) {
63 f_data = pvc->profile->tx_filter_data[x];
64 if (f_data == NULL)
65 continue;
66
67 for (y = 0; y != delta; y++) {
68 pvc->tx_filter_in[x][y + offset] = src[x + y * channels];
69 src[x + y * channels] = pvc->tx_filter_out[x][y + offset];
70 }
71 }
72
73 pvc->tx_filter_offset += delta;
74 total -= delta;
75 src += delta * channels;
76
77 /* check if there is enough data for a new transform */
78 if (pvc->tx_filter_offset == f_size) {
79 for (x = 0; x != channels; x++) {
80 f_data = pvc->profile->tx_filter_data[x];
81 if (f_data == NULL)
82 continue;
83
84 /* shift down output */
85 for (y = 0; y != f_size; y++) {
86 pvc->tx_filter_out[x][y] = pvc->tx_filter_out[x][y + f_size];
87 pvc->tx_filter_out[x][y + f_size] = 0;
88 }
89 /* perform transform */
90 voss_x3_multiply_double(pvc->tx_filter_in[x],
91 f_data, pvc->tx_filter_out[x], f_size);
92 }
93 pvc->tx_filter_offset = 0;
94 }
95 if (total == 0)
96 break;
97 }
98 }
99
100 void
vclient_rx_equalizer(struct virtual_client * pvc,int64_t * src,size_t total)101 vclient_rx_equalizer(struct virtual_client *pvc,
102 int64_t *src, size_t total)
103 {
104 double *f_data;
105 size_t channels;
106 size_t f_size;
107 size_t x;
108
109 f_size = pvc->profile->rx_filter_size;
110
111 if (f_size == 0 || total == 0)
112 return;
113
114 channels = pvc->channels;
115 total /= channels;
116
117 while (1) {
118 size_t delta;
119 size_t offset;
120 size_t y;
121
122 offset = pvc->rx_filter_offset;
123 delta = f_size - offset;
124
125 if (delta > total)
126 delta = total;
127
128 for (x = 0; x != channels; x++) {
129 f_data = pvc->profile->rx_filter_data[x];
130 if (f_data == NULL)
131 continue;
132
133 for (y = 0; y != delta; y++) {
134 pvc->rx_filter_in[x][y + offset] = src[x + y * channels];
135 src[x + y * channels] = pvc->rx_filter_out[x][y + offset];
136 }
137 }
138
139 pvc->rx_filter_offset += delta;
140 total -= delta;
141 src += delta * channels;
142
143 /* check if there is enough data for a new transform */
144 if (pvc->rx_filter_offset == f_size) {
145 for (x = 0; x != channels; x++) {
146 f_data = pvc->profile->rx_filter_data[x];
147 if (f_data == NULL)
148 continue;
149
150 /* shift output down */
151 for (y = 0; y != f_size; y++) {
152 pvc->rx_filter_out[x][y] = pvc->rx_filter_out[x][y + f_size];
153 pvc->rx_filter_out[x][y + f_size] = 0;
154 }
155 /* perform transform */
156 voss_x3_multiply_double(pvc->rx_filter_in[x],
157 f_data, pvc->rx_filter_out[x], f_size);
158 }
159 pvc->rx_filter_offset = 0;
160 }
161 if (total == 0)
162 break;
163 }
164 }
165
166 int
vclient_eq_alloc(struct virtual_client * pvc)167 vclient_eq_alloc(struct virtual_client *pvc)
168 {
169 uint8_t x;
170
171 pvc->tx_filter_offset = 0;
172 pvc->rx_filter_offset = 0;
173
174 for (x = 0; x != pvc->channels; x++) {
175 uint32_t size;
176
177 size = pvc->profile->tx_filter_size;
178 if (size != 0) {
179 pvc->tx_filter_in[x] =
180 malloc(sizeof(pvc->tx_filter_in[x][0]) * size);
181 pvc->tx_filter_out[x] =
182 calloc(2 * size, sizeof(pvc->tx_filter_out[x][0]));
183 if (pvc->tx_filter_in[x] == NULL ||
184 pvc->tx_filter_out[x] == NULL)
185 goto error;
186 }
187 size = pvc->profile->rx_filter_size;
188 if (size != 0) {
189 pvc->rx_filter_in[x] =
190 malloc(sizeof(pvc->rx_filter_in[x][0]) * size);
191 pvc->rx_filter_out[x] =
192 calloc(2 * size, sizeof(pvc->rx_filter_out[x][0]));
193 if (pvc->rx_filter_in[x] == NULL ||
194 pvc->rx_filter_out[x] == NULL)
195 goto error;
196 }
197 }
198 return (0);
199
200 error:
201 vclient_eq_free(pvc);
202 return (ENOMEM);
203 }
204
205 void
vclient_eq_free(struct virtual_client * pvc)206 vclient_eq_free(struct virtual_client *pvc)
207 {
208 uint8_t x;
209
210 pvc->tx_filter_offset = 0;
211 pvc->rx_filter_offset = 0;
212
213 for (x = 0; x != VMAX_CHAN; x++) {
214 free(pvc->tx_filter_in[x]);
215 pvc->tx_filter_in[x] = NULL;
216
217 free(pvc->rx_filter_in[x]);
218 pvc->rx_filter_in[x] = NULL;
219
220 free(pvc->tx_filter_out[x]);
221 pvc->tx_filter_out[x] = NULL;
222
223 free(pvc->rx_filter_out[x]);
224 pvc->rx_filter_out[x] = NULL;
225 }
226 }
227