1/*-
2 * Copyright (c) 2005-2009 Ariff Abdullah <ariff@FreeBSD.org>
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
7 * are met:
8 * 1. Redistributions of source code must retain the above copyright
9 *    notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 *    notice, this list of conditions and the following disclaimer in the
12 *    documentation and/or other materials provided with the distribution.
13 *
14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
24 * SUCH DAMAGE.
25 */
26
27/* feeder_volume, a long 'Lost Technology' rather than a new feature. */
28
29#ifdef _KERNEL
30#ifdef HAVE_KERNEL_OPTION_HEADERS
31#include "opt_snd.h"
32#endif
33#include <dev/sound/pcm/sound.h>
34#include <dev/sound/pcm/pcm.h>
35#include "feeder_if.h"
36
37#define SND_USE_FXDIV
38#include "snd_fxdiv_gen.h"
39
40SND_DECLARE_FILE("$FreeBSD$");
41#endif
42
43typedef void (*feed_volume_t)(int *, int *, uint32_t, uint8_t *, uint32_t);
44
45#define FEEDVOLUME_CALC8(s, v)	(SND_VOL_CALC_SAMPLE((intpcm_t)		\
46				 (s) << 8, v) >> 8)
47#define FEEDVOLUME_CALC16(s, v)	SND_VOL_CALC_SAMPLE((intpcm_t)(s), v)
48#define FEEDVOLUME_CALC24(s, v)	SND_VOL_CALC_SAMPLE((intpcm64_t)(s), v)
49#define FEEDVOLUME_CALC32(s, v)	SND_VOL_CALC_SAMPLE((intpcm64_t)(s), v)
50
51#define FEEDVOLUME_DECLARE(SIGN, BIT, ENDIAN)				\
52static void								\
53feed_volume_##SIGN##BIT##ENDIAN(int *vol, int *matrix,			\
54    uint32_t channels, uint8_t *dst, uint32_t count)			\
55{									\
56	intpcm##BIT##_t v;						\
57	intpcm_t x;							\
58	uint32_t i;							\
59									\
60	dst += count * PCM_##BIT##_BPS * channels;			\
61	do {								\
62		i = channels;						\
63		do {							\
64			dst -= PCM_##BIT##_BPS;				\
65			i--;						\
66			x = PCM_READ_##SIGN##BIT##_##ENDIAN(dst);	\
67			v = FEEDVOLUME_CALC##BIT(x, vol[matrix[i]]);	\
68			x = PCM_CLAMP_##SIGN##BIT(v);			\
69			_PCM_WRITE_##SIGN##BIT##_##ENDIAN(dst, x);	\
70		} while (i != 0);					\
71	} while (--count != 0);						\
72}
73
74#if BYTE_ORDER == LITTLE_ENDIAN || defined(SND_FEEDER_MULTIFORMAT)
75FEEDVOLUME_DECLARE(S, 16, LE)
76FEEDVOLUME_DECLARE(S, 32, LE)
77#endif
78#if BYTE_ORDER == BIG_ENDIAN || defined(SND_FEEDER_MULTIFORMAT)
79FEEDVOLUME_DECLARE(S, 16, BE)
80FEEDVOLUME_DECLARE(S, 32, BE)
81#endif
82#ifdef SND_FEEDER_MULTIFORMAT
83FEEDVOLUME_DECLARE(S,  8, NE)
84FEEDVOLUME_DECLARE(S, 24, LE)
85FEEDVOLUME_DECLARE(S, 24, BE)
86FEEDVOLUME_DECLARE(U,  8, NE)
87FEEDVOLUME_DECLARE(U, 16, LE)
88FEEDVOLUME_DECLARE(U, 24, LE)
89FEEDVOLUME_DECLARE(U, 32, LE)
90FEEDVOLUME_DECLARE(U, 16, BE)
91FEEDVOLUME_DECLARE(U, 24, BE)
92FEEDVOLUME_DECLARE(U, 32, BE)
93#endif
94
95struct feed_volume_info {
96	uint32_t bps, channels;
97	feed_volume_t apply;
98	int volume_class;
99	int state;
100	int matrix[SND_CHN_MAX];
101};
102
103#define FEEDVOLUME_ENTRY(SIGN, BIT, ENDIAN)				\
104	{								\
105		AFMT_##SIGN##BIT##_##ENDIAN,				\
106		feed_volume_##SIGN##BIT##ENDIAN				\
107	}
108
109static const struct {
110	uint32_t format;
111	feed_volume_t apply;
112} feed_volume_info_tab[] = {
113#if BYTE_ORDER == LITTLE_ENDIAN || defined(SND_FEEDER_MULTIFORMAT)
114	FEEDVOLUME_ENTRY(S, 16, LE),
115	FEEDVOLUME_ENTRY(S, 32, LE),
116#endif
117#if BYTE_ORDER == BIG_ENDIAN || defined(SND_FEEDER_MULTIFORMAT)
118	FEEDVOLUME_ENTRY(S, 16, BE),
119	FEEDVOLUME_ENTRY(S, 32, BE),
120#endif
121#ifdef SND_FEEDER_MULTIFORMAT
122	FEEDVOLUME_ENTRY(S,  8, NE),
123	FEEDVOLUME_ENTRY(S, 24, LE),
124	FEEDVOLUME_ENTRY(S, 24, BE),
125	FEEDVOLUME_ENTRY(U,  8, NE),
126	FEEDVOLUME_ENTRY(U, 16, LE),
127	FEEDVOLUME_ENTRY(U, 24, LE),
128	FEEDVOLUME_ENTRY(U, 32, LE),
129	FEEDVOLUME_ENTRY(U, 16, BE),
130	FEEDVOLUME_ENTRY(U, 24, BE),
131	FEEDVOLUME_ENTRY(U, 32, BE)
132#endif
133};
134
135#define FEEDVOLUME_TAB_SIZE	((int32_t)				\
136				 (sizeof(feed_volume_info_tab) /	\
137				  sizeof(feed_volume_info_tab[0])))
138
139static int
140feed_volume_init(struct pcm_feeder *f)
141{
142	struct feed_volume_info *info;
143	struct pcmchan_matrix *m;
144	uint32_t i;
145	int ret;
146
147	if (f->desc->in != f->desc->out ||
148	    AFMT_CHANNEL(f->desc->in) > SND_CHN_MAX)
149		return (EINVAL);
150
151	for (i = 0; i < FEEDVOLUME_TAB_SIZE; i++) {
152		if (AFMT_ENCODING(f->desc->in) ==
153		    feed_volume_info_tab[i].format) {
154			info = malloc(sizeof(*info), M_DEVBUF,
155			    M_NOWAIT | M_ZERO);
156			if (info == NULL)
157				return (ENOMEM);
158
159			info->bps = AFMT_BPS(f->desc->in);
160			info->channels = AFMT_CHANNEL(f->desc->in);
161			info->apply = feed_volume_info_tab[i].apply;
162			info->volume_class = SND_VOL_C_PCM;
163			info->state = FEEDVOLUME_ENABLE;
164
165			f->data = info;
166			m = feeder_matrix_default_channel_map(info->channels);
167			if (m == NULL) {
168				free(info, M_DEVBUF);
169				return (EINVAL);
170			}
171
172			ret = feeder_volume_apply_matrix(f, m);
173			if (ret != 0)
174				free(info, M_DEVBUF);
175
176			return (ret);
177		}
178	}
179
180	return (EINVAL);
181}
182
183static int
184feed_volume_free(struct pcm_feeder *f)
185{
186	struct feed_volume_info *info;
187
188	info = f->data;
189	if (info != NULL)
190		free(info, M_DEVBUF);
191
192	f->data = NULL;
193
194	return (0);
195}
196
197static int
198feed_volume_set(struct pcm_feeder *f, int what, int value)
199{
200	struct feed_volume_info *info;
201	struct pcmchan_matrix *m;
202	int ret;
203
204	info = f->data;
205	ret = 0;
206
207	switch (what) {
208	case FEEDVOLUME_CLASS:
209		if (value < SND_VOL_C_BEGIN || value > SND_VOL_C_END)
210			return (EINVAL);
211		info->volume_class = value;
212		break;
213	case FEEDVOLUME_CHANNELS:
214		if (value < SND_CHN_MIN || value > SND_CHN_MAX)
215			return (EINVAL);
216		m = feeder_matrix_default_channel_map(value);
217		if (m == NULL)
218			return (EINVAL);
219		ret = feeder_volume_apply_matrix(f, m);
220		break;
221	case FEEDVOLUME_STATE:
222		if (!(value == FEEDVOLUME_ENABLE || value == FEEDVOLUME_BYPASS))
223			return (EINVAL);
224		info->state = value;
225		break;
226	default:
227		return (EINVAL);
228		break;
229	}
230
231	return (ret);
232}
233
234static int
235feed_volume_feed(struct pcm_feeder *f, struct pcm_channel *c, uint8_t *b,
236    uint32_t count, void *source)
237{
238	struct feed_volume_info *info;
239	uint32_t j, align;
240	int i, *vol, *matrix;
241	uint8_t *dst;
242
243	/*
244	 * Fetch filter data operation.
245	 */
246	info = f->data;
247
248	if (info->state == FEEDVOLUME_BYPASS)
249		return (FEEDER_FEED(f->source, c, b, count, source));
250
251	vol = c->volume[SND_VOL_C_VAL(info->volume_class)];
252	matrix = info->matrix;
253
254	/*
255	 * First, let see if we really need to apply gain at all.
256	 */
257	j = 0;
258	i = info->channels;
259	do {
260		if (vol[matrix[--i]] != SND_VOL_FLAT) {
261			j = 1;
262			break;
263		}
264	} while (i != 0);
265
266	/* Nope, just bypass entirely. */
267	if (j == 0)
268		return (FEEDER_FEED(f->source, c, b, count, source));
269
270	dst = b;
271	align = info->bps * info->channels;
272
273	do {
274		if (count < align)
275			break;
276
277		j = SND_FXDIV(FEEDER_FEED(f->source, c, dst, count, source),
278		    align);
279		if (j == 0)
280			break;
281
282		info->apply(vol, matrix, info->channels, dst, j);
283
284		j *= align;
285		dst += j;
286		count -= j;
287
288	} while (count != 0);
289
290	return (dst - b);
291}
292
293static struct pcm_feederdesc feeder_volume_desc[] = {
294	{ FEEDER_VOLUME, 0, 0, 0, 0 },
295	{ 0, 0, 0, 0, 0 }
296};
297
298static kobj_method_t feeder_volume_methods[] = {
299	KOBJMETHOD(feeder_init,		feed_volume_init),
300	KOBJMETHOD(feeder_free,		feed_volume_free),
301	KOBJMETHOD(feeder_set,		feed_volume_set),
302	KOBJMETHOD(feeder_feed,		feed_volume_feed),
303	KOBJMETHOD_END
304};
305
306FEEDER_DECLARE(feeder_volume, NULL);
307
308/* Extern */
309
310/*
311 * feeder_volume_apply_matrix(): For given matrix map, apply its configuration
312 *                               to feeder_volume matrix structure. There are
313 *                               possibilites that feeder_volume be inserted
314 *                               before or after feeder_matrix, which in this
315 *                               case feeder_volume must be in a good terms
316 *                               with _current_ matrix.
317 */
318int
319feeder_volume_apply_matrix(struct pcm_feeder *f, struct pcmchan_matrix *m)
320{
321	struct feed_volume_info *info;
322	uint32_t i;
323
324	if (f == NULL || f->desc == NULL || f->desc->type != FEEDER_VOLUME ||
325	    f->data == NULL || m == NULL || m->channels < SND_CHN_MIN ||
326	    m->channels > SND_CHN_MAX)
327		return (EINVAL);
328
329	info = f->data;
330
331	for (i = 0; i < (sizeof(info->matrix) / sizeof(info->matrix[0])); i++) {
332		if (i < m->channels)
333			info->matrix[i] = m->map[i].type;
334		else
335			info->matrix[i] = SND_CHN_T_FL;
336	}
337
338	info->channels = m->channels;
339
340	return (0);
341}
342