?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Merge remote-tracking branch 'origin/master'
[unleashed/lotheac.git] / usr / src / uts / common / io / audio / impl / audio_grc3.c
blob3d42ab84252d925c6299ca2475603be7816dfd91
1 /*
2 * CDDL HEADER START
3 *
4 * The contents of this file are subject to the terms of the
5 * Common Development and Distribution License (the "License").
6 * You may not use this file except in compliance with the License.
7 *
8 * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
9 * or http://www.opensolaris.org/os/licensing.
10 * See the License for the specific language governing permissions
11 * and limitations under the License.
12 *
13 * When distributing Covered Code, include this CDDL HEADER in each
14 * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
15 * If applicable, add the following below this CDDL HEADER, with the
16 * fields enclosed by brackets "[]" replaced with your own identifying
17 * information: Portions Copyright [yyyy] [name of copyright owner]
18 *
19 * CDDL HEADER END
20 */
21 /*
22 * Copyright (C) 4Front Technologies 1996-2008.
23 *
24 * Copyright 2009 Sun Microsystems, Inc. All rights reserved.
25 * Use is subject to license terms.
26 */
27
28 /*
29 * Purpose: GRC3 Sample Rate Converter
30 *
31 * GRC library version 3.1
32 */
33
34 #include <sys/types.h>
35 #include "audio_grc3.h"
36
37 extern const int32_t filter_data_L[];
38 extern const int32_t filter_data_M[];
39 extern const int32_t filter_data_H[];
40 extern const int32_t filter_data_P[];
41
42 #define filter_data_HX filter_data_H
43 #define filter_data_PX filter_data_P
44
45 static int32_t
46 _muldivu64(uint32_t a, uint32_t val1, uint32_t val2)
47 {
48 uint64_t v = ((uint64_t)a) * val1 / val2;
49 return ((uint32_t)(v));
50 }
51
52
53 static int32_t
54 _grc_sat6(int32_t a, int32_t b)
55 {
56 int64_t v = ((int64_t)a) * b + (1 << 5);
57 return ((int32_t)(v >> 6));
58 }
59
60 static int32_t
61 _grc_sat31(int32_t a, int32_t b)
62 {
63 int64_t v = ((int64_t)a) * b + (1 << 30);
64 return ((int32_t)(v >> 31));
65 }
66
67
68 #define DEFINE_FILTER(T) \
69 static int32_t \
70 _filt31_##T(int32_t a, int32_t idx) \
71 { \
72 int64_t v = ((int64_t)a) * filter_data_##T[idx >> 15]; \
73 return ((int32_t)(v >> 31)); \
74 }
75
76 #define DEFINE_FILTER_HQ(T) \
77 static int32_t \
78 _filt31_##T(int32_t a, int32_t idx) \
79 { \
80 int32_t idx2 = idx>>15; \
81 int64_t v = ((int64_t)a) * \
82 \
83 (filter_data_##T[idx2] + \
84 (((int64_t)(idx & 32767)) * (filter_data_##T[idx2 + 1] - \
85 filter_data_##T[idx2]) >> 15)); \
86 return ((int32_t)(v>>31)); \
87 }
88
89
90 DEFINE_FILTER(L)
91 DEFINE_FILTER(M)
92 DEFINE_FILTER(H)
93 DEFINE_FILTER_HQ(HX)
94 DEFINE_FILTER(P)
95 DEFINE_FILTER_HQ(PX)
96
97 #define DEFINE_CONVD(T, SZ) \
98 static int32_t \
99 _conv31d_##T(int32_t *history, uint32_t filter, uint32_t incv) \
100 { \
101 int32_t accum = 0; \
102 \
103 filter = (1024 << 15) - filter; \
104 \
105 while (filter < ((uint32_t)(SZ << 15))) { \
106 accum += _filt31_##T(*history, filter); \
107 filter += incv; \
108 history--; \
109 } \
110 \
111 return (accum); \
112 }
113
114 DEFINE_CONVD(L, 4096)
115 DEFINE_CONVD(M, 8192)
116 DEFINE_CONVD(H, 16384)
117 DEFINE_CONVD(HX, 16384)
118 DEFINE_CONVD(P, 32768)
119 DEFINE_CONVD(PX, 32768)
120
121 static int32_t
122 _conv31_L(int32_t *history, uint32_t filter)
123 {
124 int32_t accum = 0;
125
126 #define ITERATION(p) \
127 accum += _filt31_##p(*history, filter); \
128 filter += (1024 << 15); \
129 history--
130
131 ITERATION(L); ITERATION(L); ITERATION(L); ITERATION(L);
132 return (accum);
133 }
134
135
136 static int32_t
137 _conv31_M(int32_t *history, uint32_t filter)
138 {
139 int32_t accum = 0;
140
141 ITERATION(M); ITERATION(M); ITERATION(M); ITERATION(M);
142 ITERATION(M); ITERATION(M); ITERATION(M); ITERATION(M);
143 return (accum);
144 }
145
146 static int32_t
147 _conv31_H(int32_t *history, uint32_t filter)
148 {
149 int32_t accum = 0;
150
151 ITERATION(H); ITERATION(H); ITERATION(H); ITERATION(H);
152 ITERATION(H); ITERATION(H); ITERATION(H); ITERATION(H);
153 ITERATION(H); ITERATION(H); ITERATION(H); ITERATION(H);
154 ITERATION(H); ITERATION(H); ITERATION(H); ITERATION(H);
155 return (accum);
156 }
157
158 static int32_t
159 _conv31_HX(int32_t *history, uint32_t filter)
160 {
161 int32_t accum = 0;
162
163 ITERATION(HX); ITERATION(HX); ITERATION(HX); ITERATION(HX);
164 ITERATION(HX); ITERATION(HX); ITERATION(HX); ITERATION(HX);
165 ITERATION(HX); ITERATION(HX); ITERATION(HX); ITERATION(HX);
166 ITERATION(HX); ITERATION(HX); ITERATION(HX); ITERATION(HX);
167 return (accum);
168 }
169
170 static int32_t
171 _conv31_P(int32_t *history, uint32_t filter)
172 {
173 int32_t accum = 0;
174
175 ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
176 ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
177 ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
178 ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
179 ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
180 ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
181 ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
182 ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
183 return (accum);
184 }
185
186 static int32_t
187 _conv31_PX(int32_t *history, uint32_t filter)
188 {
189 int32_t accum = 0;
190
191 ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
192 ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
193 ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
194 ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
195 ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
196 ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
197 ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
198 ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
199 return (accum);
200 }
201
202 #define GRC3_RESAMPLE(QUAL) \
203 static void \
204 grc3_upsample_##QUAL(grc3state_t *grc, const int32_t *src, \
205 int32_t *dst, uint32_t sz, uint32_t bufsz, int inc, int offset) \
206 { \
207 int32_t ptr = grc->ptr; \
208 int32_t srcrate = grc->srcrate; \
209 int32_t dstrate = grc->dstrate; \
210 int32_t *history = grc->historyptr; \
211 int32_t filtfactor = grc->filtfactor; \
212 uint32_t dstsz = 0; \
213 \
214 src += offset; \
215 dst += offset; \
216 \
217 while (sz > 0) { \
218 while (ptr < dstrate) { \
219 if (dstsz >= bufsz) \
220 goto endloop; \
221 dst[0] = (_conv31_##QUAL(history, \
222 _grc_sat6(ptr, filtfactor))); \
223 ptr += srcrate; \
224 dst += inc; \
225 dstsz++; \
226 } \
227 \
228 history++; \
229 if (history >= (grc->history + GRC3_MAXHISTORY * 2)) \
230 history -= GRC3_MAXHISTORY; \
231 \
232 history[0] = history[-GRC3_MAXHISTORY] = (*src); \
233 \
234 ptr -= dstrate; \
235 \
236 sz--; \
237 src += inc; \
238 } \
239 endloop: \
240 \
241 grc->ptr = ptr; \
242 grc->historyptr = history; \
243 grc->outsz = dstsz; \
244 } \
245 \
246 static void \
247 grc3_dnsample_##QUAL(grc3state_t *grc, const int32_t *src, \
248 int32_t *dst, uint32_t sz, uint32_t bufsz, int inc, int offset) \
249 { \
250 int32_t ptr = grc->ptr; \
251 int32_t srcrate = grc->srcrate; \
252 int32_t dstrate = grc->dstrate; \
253 int32_t sat = grc->sat; \
254 int32_t *history = grc->historyptr; \
255 int32_t filtfactor = grc->filtfactor; \
256 uint32_t dstsz = 0; \
257 \
258 src += offset; \
259 dst += offset; \
260 \
261 while (sz > 0) { \
262 while (ptr >= srcrate) { \
263 if (dstsz >= bufsz) \
264 goto endloop; \
265 ptr -= srcrate; \
266 dst[0] = (_conv31d_##QUAL(history, \
267 _grc_sat6(ptr, filtfactor), \
268 grc->ptr_incv)); \
269 dst += inc; \
270 dstsz++; \
271 } \
272 \
273 history++; \
274 if (history >= (grc->history + GRC3_MAXHISTORY * 2)) \
275 history -= GRC3_MAXHISTORY; \
276 \
277 /* \
278 * TODO: for better quality multiplier is worth moving \
279 * to output cascade \
280 */ \
281 history[0] = history[-GRC3_MAXHISTORY] = \
282 _grc_sat31((*src), sat); \
283 \
284 ptr += dstrate; \
285 \
286 sz--; \
287 src += inc; \
288 } \
289 endloop: \
290 \
291 grc->ptr = ptr; \
292 grc->historyptr = history; \
293 grc->outsz = dstsz; \
294 } \
295 \
296 static void \
297 grc3_resample_##QUAL(grc3state_t *grc, const void *src, void *dst, \
298 uint32_t sz, uint32_t bufsz, int inc, int offset) \
299 { \
300 if (grc->srcrate <= grc->dstrate) \
301 grc3_upsample_##QUAL(grc, src, dst, sz, \
302 bufsz, inc, offset); \
303 else \
304 grc3_dnsample_##QUAL(grc, src, dst, sz, \
305 bufsz, inc, offset); \
306 }
307
308 GRC3_RESAMPLE(L)
309 GRC3_RESAMPLE(M)
310 GRC3_RESAMPLE(H)
311 GRC3_RESAMPLE(HX)
312 GRC3_RESAMPLE(P)
313 GRC3_RESAMPLE(PX)
314
315 /*
316 * For performance reasons, we only support 24-bit SRC.
317 */
318 void
319 grc3_convert(grc3state_t *grc, int quality, const void *src,
320 void *dst, int sz, int bufsz, int inc, int offset)
321 {
322
323 switch (quality) {
324 default:
325 case 0:
326 case 1:
327 grc3_resample_L(grc, src, dst, sz, bufsz, inc, offset);
328 break;
329 case 2:
330 grc3_resample_M(grc, src, dst, sz, bufsz, inc, offset);
331 break;
332 case 3:
333 grc3_resample_H(grc, src, dst, sz, bufsz, inc, offset);
334 break;
335 case 4:
336 grc3_resample_HX(grc, src, dst, sz, bufsz, inc, offset);
337 break;
338 case 5:
339 grc3_resample_P(grc, src, dst, sz, bufsz, inc, offset);
340 break;
341 case 6:
342 grc3_resample_PX(grc, src, dst, sz, bufsz, inc, offset);
343 break;
344 }
345 }
346
347 void
348 grc3_reset(grc3state_t *grc)
349 {
350 int32_t t;
351 grc->ptr = 0;
352 grc->historyptr = grc->history + GRC3_MAXHISTORY;
353
354 for (t = 0; t < GRC3_MAXHISTORY * 2; t++)
355 grc->history[t] = 0;
356 }
357
358 static void
359 grc3_setup_up(grc3state_t *grc, uint32_t fromRate, uint32_t toRate)
360 {
361 grc->srcrate = fromRate;
362 grc->dstrate = toRate;
363 grc->filtfactor = 0x80000000U / toRate;
364 }
365
366 static void
367 grc3_setup_dn(grc3state_t *grc, uint32_t fromRate, uint32_t toRate)
368 {
369 grc->srcrate = fromRate;
370 grc->dstrate = toRate;
371 grc->filtfactor = 0x80000000U / fromRate;
372 grc->ptr_incv = _muldivu64(1024 << 15, toRate, fromRate);
373 grc->sat = _muldivu64(0x80000000U, toRate, fromRate);
374 }
375
376 void
377 grc3_setup(grc3state_t *grc, uint32_t fromRate, uint32_t toRate)
378 {
379 while ((!(fromRate & 1)) && (!(toRate & 1)) && (fromRate > 0)) {
380 fromRate >>= 1;
381 toRate >>= 1;
382 }
383
384 if (fromRate <= toRate)
385 grc3_setup_up(grc, fromRate, toRate);
386 else
387 grc3_setup_dn(grc, fromRate, toRate);
388 }