1/*-
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2005-2009 Ariff Abdullah <ariff@FreeBSD.org>
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 * 1. Redistributions of source code must retain the above copyright
11 *    notice, this list of conditions and the following disclaimer.
12 * 2. Redistributions in binary form must reproduce the above copyright
13 *    notice, this list of conditions and the following disclaimer in the
14 *    documentation and/or other materials provided with the distribution.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
17 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
20 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
21 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
22 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
24 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
25 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
26 * SUCH DAMAGE.
27 */
28
29/* feeder_volume, a long 'Lost Technology' rather than a new feature. */
30
31#ifdef _KERNEL
32#ifdef HAVE_KERNEL_OPTION_HEADERS
33#include "opt_snd.h"
34#endif
35#include <dev/sound/pcm/sound.h>
36#include <dev/sound/pcm/pcm.h>
37#include "feeder_if.h"
38
39#define SND_USE_FXDIV
40#include "snd_fxdiv_gen.h"
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	int temp_vol[SND_CHN_T_VOL_MAX];
239	struct feed_volume_info *info;
240	uint32_t j, align;
241	int i, *matrix;
242	uint8_t *dst;
243	const int16_t *vol;
244	const int8_t *muted;
245
246	/*
247	 * Fetch filter data operation.
248	 */
249	info = f->data;
250
251	if (info->state == FEEDVOLUME_BYPASS)
252		return (FEEDER_FEED(f->source, c, b, count, source));
253
254	vol = c->volume[SND_VOL_C_VAL(info->volume_class)];
255	muted = c->muted[SND_VOL_C_VAL(info->volume_class)];
256	matrix = info->matrix;
257
258	/*
259	 * First, let see if we really need to apply gain at all.
260	 */
261	j = 0;
262	i = info->channels;
263	while (i--) {
264		if (vol[matrix[i]] != SND_VOL_FLAT ||
265		    muted[matrix[i]] != 0) {
266			j = 1;
267			break;
268		}
269	}
270
271	/* Nope, just bypass entirely. */
272	if (j == 0)
273		return (FEEDER_FEED(f->source, c, b, count, source));
274
275	/* Check if any controls are muted. */
276	for (j = 0; j != SND_CHN_T_VOL_MAX; j++)
277		temp_vol[j] = muted[j] ? 0 : vol[j];
278
279	dst = b;
280	align = info->bps * info->channels;
281
282	do {
283		if (count < align)
284			break;
285
286		j = SND_FXDIV(FEEDER_FEED(f->source, c, dst, count, source),
287		    align);
288		if (j == 0)
289			break;
290
291		info->apply(temp_vol, matrix, info->channels, dst, j);
292
293		j *= align;
294		dst += j;
295		count -= j;
296
297	} while (count != 0);
298
299	return (dst - b);
300}
301
302static struct pcm_feederdesc feeder_volume_desc[] = {
303	{ FEEDER_VOLUME, 0, 0, 0, 0 },
304	{ 0, 0, 0, 0, 0 }
305};
306
307static kobj_method_t feeder_volume_methods[] = {
308	KOBJMETHOD(feeder_init,		feed_volume_init),
309	KOBJMETHOD(feeder_free,		feed_volume_free),
310	KOBJMETHOD(feeder_set,		feed_volume_set),
311	KOBJMETHOD(feeder_feed,		feed_volume_feed),
312	KOBJMETHOD_END
313};
314
315FEEDER_DECLARE(feeder_volume, NULL);
316
317/* Extern */
318
319/*
320 * feeder_volume_apply_matrix(): For given matrix map, apply its configuration
321 *                               to feeder_volume matrix structure. There are
322 *                               possibilites that feeder_volume be inserted
323 *                               before or after feeder_matrix, which in this
324 *                               case feeder_volume must be in a good terms
325 *                               with _current_ matrix.
326 */
327int
328feeder_volume_apply_matrix(struct pcm_feeder *f, struct pcmchan_matrix *m)
329{
330	struct feed_volume_info *info;
331	uint32_t i;
332
333	if (f == NULL || f->desc == NULL || f->desc->type != FEEDER_VOLUME ||
334	    f->data == NULL || m == NULL || m->channels < SND_CHN_MIN ||
335	    m->channels > SND_CHN_MAX)
336		return (EINVAL);
337
338	info = f->data;
339
340	for (i = 0; i < nitems(info->matrix); i++) {
341		if (i < m->channels)
342			info->matrix[i] = m->map[i].type;
343		else
344			info->matrix[i] = SND_CHN_T_FL;
345	}
346
347	info->channels = m->channels;
348
349	return (0);
350}
351