GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: usr.bin/sndiod/dsp.c Lines: 0 307 0.0 %
Date: 2017-11-13 Branches: 0 190 0.0 %

Line Branch Exec Source
1
/*	$OpenBSD: dsp.c,v 1.12 2016/10/27 04:37:47 ratchov Exp $	*/
2
/*
3
 * Copyright (c) 2008-2012 Alexandre Ratchov <alex@caoua.org>
4
 *
5
 * Permission to use, copy, modify, and distribute this software for any
6
 * purpose with or without fee is hereby granted, provided that the above
7
 * copyright notice and this permission notice appear in all copies.
8
 *
9
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
10
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
11
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
12
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
13
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
14
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
15
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
16
 */
17
#include <string.h>
18
#include "dsp.h"
19
#include "utils.h"
20
21
int aparams_ctltovol[128] = {
22
	    0,
23
	  256,	  266,	  276,	  287,	  299,	  310,	  323,	  335,
24
	  348,	  362,	  376,	  391,	  406,	  422,	  439,	  456,
25
	  474,	  493,	  512,	  532,	  553,	  575,	  597,	  621,
26
	  645,	  670,	  697,	  724,	  753,	  782,	  813,	  845,
27
	  878,	  912,	  948,	  985,	 1024,	 1064,	 1106,	 1149,
28
	 1195,	 1241,	 1290,	 1341,	 1393,	 1448,	 1505,	 1564,
29
	 1625,	 1689,	 1756,	 1825,	 1896,	 1971,	 2048,	 2128,
30
	 2212,	 2299,	 2389,	 2483,	 2580,	 2682,	 2787,	 2896,
31
	 3010,	 3128,	 3251,	 3379,	 3511,	 3649,	 3792,	 3941,
32
	 4096,	 4257,	 4424,	 4598,	 4778,	 4966,	 5161,	 5363,
33
	 5574,	 5793,	 6020,	 6256,	 6502,	 6757,	 7023,	 7298,
34
	 7585,	 7883,	 8192,	 8514,	 8848,	 9195,	 9556,	 9931,
35
	10321,	10726,	11148,	11585,	12040,	12513,	13004,	13515,
36
	14045,	14596,	15170,	15765,	16384,	17027,	17696,	18390,
37
	19112,	19863,	20643,	21453,	22295,	23170,	24080,	25025,
38
	26008,	27029,	28090,	29193,	30339,	31530,	32768
39
};
40
41
/*
42
 * Generate a string corresponding to the encoding in par,
43
 * return the length of the resulting string.
44
 */
45
int
46
aparams_enctostr(struct aparams *par, char *ostr)
47
{
48
	char *p = ostr;
49
50
	*p++ = par->sig ? 's' : 'u';
51
	if (par->bits > 9)
52
		*p++ = '0' + par->bits / 10;
53
	*p++ = '0' + par->bits % 10;
54
	if (par->bps > 1) {
55
		*p++ = par->le ? 'l' : 'b';
56
		*p++ = 'e';
57
		if (par->bps != APARAMS_BPS(par->bits) ||
58
		    par->bits < par->bps * 8) {
59
			*p++ = par->bps + '0';
60
			if (par->bits < par->bps * 8) {
61
				*p++ = par->msb ? 'm' : 'l';
62
				*p++ = 's';
63
				*p++ = 'b';
64
			}
65
		}
66
	}
67
	*p++ = '\0';
68
	return p - ostr - 1;
69
}
70
71
/*
72
 * Parse an encoding string, examples: s8, u8, s16, s16le, s24be ...
73
 * set *istr to the char following the encoding. Return the number
74
 * of bytes consumed.
75
 */
76
int
77
aparams_strtoenc(struct aparams *par, char *istr)
78
{
79
	char *p = istr;
80
	int i, sig, bits, le, bps, msb;
81
82
#define IS_SEP(c)			\
83
	(((c) < 'a' || (c) > 'z') &&	\
84
	 ((c) < 'A' || (c) > 'Z') &&	\
85
	 ((c) < '0' || (c) > '9'))
86
87
	/*
88
	 * get signedness
89
	 */
90
	if (*p == 's') {
91
		sig = 1;
92
	} else if (*p == 'u') {
93
		sig = 0;
94
	} else
95
		return 0;
96
	p++;
97
98
	/*
99
	 * get number of bits per sample
100
	 */
101
	bits = 0;
102
	for (i = 0; i < 2; i++) {
103
		if (*p < '0' || *p > '9')
104
			break;
105
		bits = (bits * 10) + *p - '0';
106
		p++;
107
	}
108
	if (bits < BITS_MIN || bits > BITS_MAX)
109
		return 0;
110
	bps = APARAMS_BPS(bits);
111
	msb = 1;
112
	le = ADATA_LE;
113
114
	/*
115
	 * get (optional) endianness
116
	 */
117
	if (p[0] == 'l' && p[1] == 'e') {
118
		le = 1;
119
		p += 2;
120
	} else if (p[0] == 'b' && p[1] == 'e') {
121
		le = 0;
122
		p += 2;
123
	} else if (IS_SEP(*p)) {
124
		goto done;
125
	} else
126
		return 0;
127
128
	/*
129
	 * get (optional) number of bytes
130
	 */
131
	if (*p >= '0' && *p <= '9') {
132
		bps = *p - '0';
133
		if (bps < (bits + 7) / 8 ||
134
		    bps > (BITS_MAX + 7) / 8)
135
			return 0;
136
		p++;
137
138
		/*
139
		 * get (optional) alignment
140
		 */
141
		if (p[0] == 'm' && p[1] == 's' && p[2] == 'b') {
142
			msb = 1;
143
			p += 3;
144
		} else if (p[0] == 'l' && p[1] == 's' && p[2] == 'b') {
145
			msb = 0;
146
			p += 3;
147
		} else if (IS_SEP(*p)) {
148
			goto done;
149
		} else
150
			return 0;
151
	} else if (!IS_SEP(*p))
152
		return 0;
153
154
done:
155
	par->msb = msb;
156
	par->sig = sig;
157
	par->bits = bits;
158
	par->bps = bps;
159
	par->le = le;
160
	return p - istr;
161
}
162
163
/*
164
 * Initialise parameters structure with the defaults natively supported
165
 * by the machine.
166
 */
167
void
168
aparams_init(struct aparams *par)
169
{
170
	par->bps = sizeof(adata_t);
171
	par->bits = ADATA_BITS;
172
	par->le = ADATA_LE;
173
	par->sig = 1;
174
	par->msb = 0;
175
}
176
177
/*
178
 * log the given format/channels/encoding
179
 */
180
void
181
aparams_log(struct aparams *par)
182
{
183
	char enc[ENCMAX];
184
185
	aparams_enctostr(par, enc);
186
	log_puts(enc);
187
}
188
189
/*
190
 * return true if encoding corresponds to what we store in adata_t
191
 */
192
int
193
aparams_native(struct aparams *par)
194
{
195
	return par->bps == sizeof(adata_t) && par->bits == ADATA_BITS &&
196
	    (par->bps == 1 || par->le == ADATA_LE) &&
197
	    (par->bits == par->bps * 8 || !par->msb);
198
}
199
200
/*
201
 * resample the given number of frames
202
 */
203
int
204
resamp_do(struct resamp *p, adata_t *in, adata_t *out, int todo)
205
{
206
	unsigned int nch;
207
	adata_t *idata;
208
	unsigned int oblksz;
209
	unsigned int ifr;
210
	int s, ds, diff;
211
	adata_t *odata;
212
	unsigned int iblksz;
213
	unsigned int ofr;
214
	unsigned int c;
215
	adata_t *ctxbuf, *ctx;
216
	unsigned int ctx_start;
217
218
	/*
219
	 * Partially copy structures into local variables, to avoid
220
	 * unnecessary indirections; this also allows the compiler to
221
	 * order local variables more "cache-friendly".
222
	 */
223
	idata = in;
224
	odata = out;
225
	diff = p->diff;
226
	iblksz = p->iblksz;
227
	oblksz = p->oblksz;
228
	ctxbuf = p->ctx;
229
	ctx_start = p->ctx_start;
230
	nch = p->nch;
231
	ifr = todo;
232
	ofr = oblksz;
233
234
	/*
235
	 * Start conversion.
236
	 */
237
#ifdef DEBUG
238
	if (log_level >= 4) {
239
		log_puts("resamp: copying ");
240
		log_puti(todo);
241
		log_puts(" frames, diff = ");
242
		log_putu(diff);
243
		log_puts("\n");
244
	}
245
#endif
246
	for (;;) {
247
		if (diff < 0) {
248
			if (ifr == 0)
249
				break;
250
			ctx_start ^= 1;
251
			ctx = ctxbuf + ctx_start;
252
			for (c = nch; c > 0; c--) {
253
				*ctx = *idata++;
254
				ctx += RESAMP_NCTX;
255
			}
256
			diff += oblksz;
257
			ifr--;
258
		} else if (diff > 0) {
259
			if (ofr == 0)
260
				break;
261
			ctx = ctxbuf;
262
			for (c = nch; c > 0; c--) {
263
				s = ctx[ctx_start];
264
				ds = ctx[ctx_start ^ 1] - s;
265
				ctx += RESAMP_NCTX;
266
				*odata++ = s + ADATA_MULDIV(ds, diff, oblksz);
267
			}
268
			diff -= iblksz;
269
			ofr--;
270
		} else {
271
			if (ifr == 0 || ofr == 0)
272
				break;
273
			ctx = ctxbuf + ctx_start;
274
			for (c = nch; c > 0; c--) {
275
				*odata++ = *ctx;
276
				ctx += RESAMP_NCTX;
277
			}
278
			ctx_start ^= 1;
279
			ctx = ctxbuf + ctx_start;
280
			for (c = nch; c > 0; c--) {
281
				*ctx = *idata++;
282
				ctx += RESAMP_NCTX;
283
			}
284
			diff -= iblksz;
285
			diff += oblksz;
286
			ifr--;
287
			ofr--;
288
		}
289
	}
290
	p->diff = diff;
291
	p->ctx_start = ctx_start;
292
	return oblksz - ofr;
293
}
294
295
/*
296
 * initialize resampler with ibufsz/obufsz factor and "nch" channels
297
 */
298
void
299
resamp_init(struct resamp *p, unsigned int iblksz,
300
    unsigned int oblksz, int nch)
301
{
302
	unsigned int i;
303
304
	p->iblksz = iblksz;
305
	p->oblksz = oblksz;
306
	p->diff = 0;
307
	p->nch = nch;
308
	p->ctx_start = 0;
309
	for (i = 0; i < NCHAN_MAX * RESAMP_NCTX; i++)
310
		p->ctx[i] = 0;
311
#ifdef DEBUG
312
	if (log_level >= 3) {
313
		log_puts("resamp: ");
314
		log_putu(iblksz);
315
		log_puts("/");
316
		log_putu(oblksz);
317
		log_puts("\n");
318
	}
319
#endif
320
}
321
322
/*
323
 * encode "todo" frames from native to foreign encoding
324
 */
325
void
326
enc_do(struct conv *p, unsigned char *in, unsigned char *out, int todo)
327
{
328
	unsigned int f;
329
	adata_t *idata;
330
	unsigned int s;
331
	unsigned int oshift;
332
	unsigned int obias;
333
	unsigned int obps;
334
	unsigned int i;
335
	unsigned char *odata;
336
	int obnext;
337
	int osnext;
338
339
#ifdef DEBUG
340
	if (log_level >= 4) {
341
		log_puts("enc: copying ");
342
		log_putu(todo);
343
		log_puts(" frames\n");
344
	}
345
#endif
346
	/*
347
	 * Partially copy structures into local variables, to avoid
348
	 * unnecessary indirections; this also allows the compiler to
349
	 * order local variables more "cache-friendly".
350
	 */
351
	idata = (adata_t *)in;
352
	odata = out;
353
	oshift = p->shift;
354
	obias = p->bias;
355
	obps = p->bps;
356
	obnext = p->bnext;
357
	osnext = p->snext;
358
359
	/*
360
	 * Start conversion.
361
	 */
362
	odata += p->bfirst;
363
	for (f = todo * p->nch; f > 0; f--) {
364
		/* convert adata to u32 */
365
		s = (int)*idata++ + ADATA_UNIT;
366
		s <<= 32 - ADATA_BITS;
367
		/* convert u32 to uN */
368
		s >>= oshift;
369
		/* convert uN to sN */
370
		s -= obias;
371
		/* packetize sN */
372
		for (i = obps; i > 0; i--) {
373
			*odata = (unsigned char)s;
374
			s >>= 8;
375
			odata += obnext;
376
		}
377
		odata += osnext;
378
	}
379
}
380
381
/*
382
 * store "todo" frames of silence in foreign encoding
383
 */
384
void
385
enc_sil_do(struct conv *p, unsigned char *out, int todo)
386
{
387
	unsigned int f;
388
	unsigned int s;
389
	unsigned int oshift;
390
	int obias;
391
	unsigned int obps;
392
	unsigned int i;
393
	unsigned char *odata;
394
	int obnext;
395
	int osnext;
396
397
#ifdef DEBUG
398
	if (log_level >= 4) {
399
		log_puts("enc: silence ");
400
		log_putu(todo);
401
		log_puts(" frames\n");
402
	}
403
#endif
404
	/*
405
	 * Partially copy structures into local variables, to avoid
406
	 * unnecessary indirections; this also allows the compiler to
407
	 * order local variables more "cache-friendly".
408
	 */
409
	odata = out;
410
	oshift = p->shift;
411
	obias = p->bias;
412
	obps = p->bps;
413
	obnext = p->bnext;
414
	osnext = p->snext;
415
416
	/*
417
	 * Start conversion.
418
	 */
419
	odata += p->bfirst;
420
	for (f = todo * p->nch; f > 0; f--) {
421
		s = ((1U << 31) >> oshift) - obias;
422
		for (i = obps; i > 0; i--) {
423
			*odata = (unsigned char)s;
424
			s >>= 8;
425
			odata += obnext;
426
		}
427
		odata += osnext;
428
	}
429
}
430
431
/*
432
 * initialize encoder from native to foreign encoding
433
 */
434
void
435
enc_init(struct conv *p, struct aparams *par, int nch)
436
{
437
	p->nch = nch;
438
	p->bps = par->bps;
439
	if (par->msb) {
440
		p->shift = 32 - par->bps * 8;
441
	} else {
442
		p->shift = 32 - par->bits;
443
	}
444
	if (par->sig) {
445
		p->bias = (1U << 31) >> p->shift;
446
	} else {
447
		p->bias = 0;
448
	}
449
	if (!par->le) {
450
		p->bfirst = par->bps - 1;
451
		p->bnext = -1;
452
		p->snext = 2 * par->bps;
453
	} else {
454
		p->bfirst = 0;
455
		p->bnext = 1;
456
		p->snext = 0;
457
	}
458
#ifdef DEBUG
459
	if (log_level >= 3) {
460
		log_puts("enc: ");
461
		aparams_log(par);
462
		log_puts(", ");
463
		log_puti(p->nch);
464
		log_puts(" channels\n");
465
	}
466
#endif
467
}
468
469
/*
470
 * decode "todo" frames from foreign to native encoding
471
 */
472
void
473
dec_do(struct conv *p, unsigned char *in, unsigned char *out, int todo)
474
{
475
	unsigned int f;
476
	unsigned int ibps;
477
	unsigned int i;
478
	unsigned int s = 0xdeadbeef;
479
	unsigned char *idata;
480
	int ibnext;
481
	int isnext;
482
	unsigned int ibias;
483
	unsigned int ishift;
484
	adata_t *odata;
485
486
#ifdef DEBUG
487
	if (log_level >= 4) {
488
		log_puts("dec: copying ");
489
		log_putu(todo);
490
		log_puts(" frames\n");
491
	}
492
#endif
493
	/*
494
	 * Partially copy structures into local variables, to avoid
495
	 * unnecessary indirections; this also allows the compiler to
496
	 * order local variables more "cache-friendly".
497
	 */
498
	idata = in;
499
	odata = (adata_t *)out;
500
	ibps = p->bps;
501
	ibnext = p->bnext;
502
	ibias = p->bias;
503
	ishift = p->shift;
504
	isnext = p->snext;
505
506
	/*
507
	 * Start conversion.
508
	 */
509
	idata += p->bfirst;
510
	for (f = todo * p->nch; f > 0; f--) {
511
		for (i = ibps; i > 0; i--) {
512
			s <<= 8;
513
			s |= *idata;
514
			idata += ibnext;
515
		}
516
		idata += isnext;
517
		s += ibias;
518
		s <<= ishift;
519
		s >>= 32 - ADATA_BITS;
520
		*odata++ = s - ADATA_UNIT;
521
	}
522
}
523
524
/*
525
 * initialize decoder from foreign to native encoding
526
 */
527
void
528
dec_init(struct conv *p, struct aparams *par, int nch)
529
{
530
	p->bps = par->bps;
531
	p->nch = nch;
532
	if (par->msb) {
533
		p->shift = 32 - par->bps * 8;
534
	} else {
535
		p->shift = 32 - par->bits;
536
	}
537
	if (par->sig) {
538
		p->bias = (1U << 31) >> p->shift;
539
	} else {
540
		p->bias = 0;
541
	}
542
	if (par->le) {
543
		p->bfirst = par->bps - 1;
544
		p->bnext = -1;
545
		p->snext = 2 * par->bps;
546
	} else {
547
		p->bfirst = 0;
548
		p->bnext = 1;
549
		p->snext = 0;
550
	}
551
#ifdef DEBUG
552
	if (log_level >= 3) {
553
		log_puts("dec: ");
554
		aparams_log(par);
555
		log_puts(", ");
556
		log_puti(p->nch);
557
		log_puts(" channels\n");
558
	}
559
#endif
560
}
561
562
/*
563
 * mix "todo" input frames on the output with the given volume
564
 */
565
void
566
cmap_add(struct cmap *p, void *in, void *out, int vol, int todo)
567
{
568
	adata_t *idata, *odata;
569
	int i, j, nch, istart, inext, onext, ostart, y, v;
570
571
#ifdef DEBUG
572
	if (log_level >= 4) {
573
		log_puts("cmap: adding ");
574
		log_puti(todo);
575
		log_puts(" frames\n");
576
	}
577
#endif
578
	idata = in;
579
	odata = out;
580
	ostart = p->ostart;
581
	onext = p->onext;
582
	istart = p->istart;
583
	inext = p->inext;
584
	nch = p->nch;
585
	v = vol;
586
587
	/*
588
	 * map/mix input on the output
589
	 */
590
	for (i = todo; i > 0; i--) {
591
		odata += ostart;
592
		idata += istart;
593
		for (j = nch; j > 0; j--) {
594
			y = *odata + ADATA_MUL(*idata, v);
595
			if (y >= ADATA_UNIT)
596
				y = ADATA_UNIT - 1;
597
			else if (y < -ADATA_UNIT)
598
				y = -ADATA_UNIT;
599
			*odata = y;
600
			idata++;
601
			odata++;
602
		}
603
		odata += onext;
604
		idata += inext;
605
	}
606
}
607
608
/*
609
 * overwrite output with "todo" input frames with the given volume
610
 */
611
void
612
cmap_copy(struct cmap *p, void *in, void *out, int vol, int todo)
613
{
614
	adata_t *idata, *odata;
615
	int i, j, nch, istart, inext, onext, ostart, v;
616
617
#ifdef DEBUG
618
	if (log_level >= 4) {
619
		log_puts("cmap: copying ");
620
		log_puti(todo);
621
		log_puts(" frames\n");
622
	}
623
#endif
624
	idata = in;
625
	odata = out;
626
	ostart = p->ostart;
627
	onext = p->onext;
628
	istart = p->istart;
629
	inext = p->inext;
630
	nch = p->nch;
631
	v = vol;
632
633
	/*
634
	 * copy to the output buffer
635
	 */
636
	for (i = todo; i > 0; i--) {
637
		idata += istart;
638
		odata += ostart;
639
		for (j = nch; j > 0; j--) {
640
			*odata = ADATA_MUL(*idata, v);
641
			odata++;
642
			idata++;
643
		}
644
		odata += onext;
645
		idata += inext;
646
	}
647
}
648
649
/*
650
 * initialize channel mapper, to map a subset of input channel range
651
 * into a subset of the output channel range
652
 */
653
void
654
cmap_init(struct cmap *p,
655
    int imin, int imax, int isubmin, int isubmax,
656
    int omin, int omax, int osubmin, int osubmax)
657
{
658
	int cmin, cmax;
659
660
	cmin = -NCHAN_MAX;
661
	if (osubmin > cmin)
662
		cmin = osubmin;
663
	if (omin > cmin)
664
		cmin = omin;
665
	if (isubmin > cmin)
666
		cmin = isubmin;
667
	if (imin > cmin)
668
		cmin = imin;
669
670
	cmax = NCHAN_MAX;
671
	if (osubmax < cmax)
672
		cmax = osubmax;
673
	if (omax < cmax)
674
		cmax = omax;
675
	if (isubmax < cmax)
676
		cmax = isubmax;
677
	if (imax < cmax)
678
		cmax = imax;
679
680
	p->ostart = cmin - omin;
681
	p->onext = omax - cmax;
682
	p->istart = cmin - imin;
683
	p->inext = imax - cmax;
684
	p->nch = cmax - cmin + 1;
685
#ifdef DEBUG
686
	if (log_level >= 3) {
687
		log_puts("cmap: nch = ");
688
		log_puti(p->nch);
689
		log_puts(", ostart = ");
690
		log_puti(p->ostart);
691
		log_puts(", onext = ");
692
		log_puti(p->onext);
693
		log_puts(", istart = ");
694
		log_puti(p->istart);
695
		log_puts(", inext = ");
696
		log_puti(p->inext);
697
		log_puts("\n");
698
	}
699
#endif
700
}