1 // SPDX-License-Identifier: BSD-3-Clause
2 //
3 // Copyright(c) 2022 Intel Corporation. All rights reserved.
4 //
5 // Author: Andrula Song <andrula.song@intel.com>
6 
7 /**
8  * \file
9  * \brief Volume generic processing implementation with peak volume detection
10  * \authors Andrula Song <andrula.song@intel.com>
11  */
12 
13 #include <sof/audio/buffer.h>
14 #include <sof/audio/component.h>
15 #include <sof/audio/format.h>
16 #include <sof/common.h>
17 #include <ipc/stream.h>
18 #include <stddef.h>
19 #include <stdint.h>
20 #include <stdlib.h>
21 
22 LOG_MODULE_DECLARE(volume_generic, CONFIG_SOF_LOG_LEVEL);
23 
24 #include <sof/audio/volume.h>
25 
26 #ifdef CONFIG_GENERIC
27 
28 #if CONFIG_COMP_PEAK_VOL
29 
30 #if CONFIG_FORMAT_S24LE
31 /**
32  * \brief Volume s24 to s24 multiply function
33  * \param[in] x   input sample.
34  * \param[in] vol gain.
35  * \return output sample.
36  *
37  * Volume multiply for 24 bit input and 24 bit output.
38  */
vol_mult_s24_to_s24(int32_t x,int32_t vol)39 static inline int32_t vol_mult_s24_to_s24(int32_t x, int32_t vol)
40 {
41 	return q_multsr_sat_32x32_24(sign_extend_s24(x), vol, Q_SHIFT_BITS_64(23, VOL_QXY_Y, 23));
42 }
43 
44 /**
45  * \brief Volume processing from 24/32 bit to 24/32 bit.
46  * \param[in,out] dev Volume base component device.
47  * \param[in,out] sink Destination buffer.
48  * \param[in,out] source Input buffer.
49  * \param[in] frames Number of frames to process.
50  * \param[in] attenuation factor for peakmeter adjustment
51  *
52  * Copy and scale volume from 24/32 bit source buffer
53  * to 24/32 bit destination buffer.
54  */
vol_s24_to_s24(struct processing_module * mod,struct input_stream_buffer * bsource,struct output_stream_buffer * bsink,uint32_t frames,uint32_t attenuation)55 static void vol_s24_to_s24(struct processing_module *mod, struct input_stream_buffer *bsource,
56 			   struct output_stream_buffer *bsink, uint32_t frames,
57 			   uint32_t attenuation)
58 {
59 	struct vol_data *cd = module_get_private_data(mod);
60 	struct audio_stream __sparse_cache *source = bsource->data;
61 	struct audio_stream __sparse_cache *sink = bsink->data;
62 	int32_t vol;
63 	int32_t *x, *x0;
64 	int32_t *y, *y0;
65 	int nmax, n, i, j;
66 	const int nch = source->channels;
67 	int remaining_samples = frames * nch;
68 	int32_t tmp;
69 
70 	memset(cd->peak_regs.peak_meter, 0, sizeof(uint32_t) * cd->channels);
71 	x = audio_stream_wrap(source, (char *)source->r_ptr + bsource->consumed);
72 	y = audio_stream_wrap(sink, (char *)sink->w_ptr + bsink->size);
73 
74 	bsource->consumed += VOL_S32_SAMPLES_TO_BYTES(remaining_samples);
75 	bsink->size += VOL_S32_SAMPLES_TO_BYTES(remaining_samples);
76 	while (remaining_samples) {
77 		nmax = audio_stream_samples_without_wrap_s24(source, x);
78 		n = MIN(remaining_samples, nmax);
79 		nmax = audio_stream_samples_without_wrap_s24(sink, y);
80 		n = MIN(n, nmax);
81 		for (j = 0; j < nch; j++) {
82 			x0 = x + j;
83 			y0 = y + j;
84 			vol = cd->volume[j];
85 			tmp = 0;
86 			for (i = 0; i < n; i += nch) {
87 				y0[i] = vol_mult_s24_to_s24(x0[i], vol);
88 				tmp = MAX(abs(x0[i]), tmp);
89 			}
90 			tmp = tmp << attenuation;
91 			cd->peak_regs.peak_meter[j] = MAX(tmp, cd->peak_regs.peak_meter[j]);
92 		}
93 		remaining_samples -= n;
94 		x = audio_stream_wrap(source, x + n);
95 		y = audio_stream_wrap(sink, y + n);
96 	}
97 	/* update peak vol */
98 	peak_vol_update(cd);
99 }
100 #endif /* CONFIG_FORMAT_S24LE */
101 
102 #if CONFIG_FORMAT_S32LE
103 /**
104  * \brief Volume processing from 32 bit to 32 bit.
105  * \param[in,out] dev Volume base component device.
106  * \param[in,out] sink Destination buffer.
107  * \param[in,out] source Input buffer.
108  * \param[in] frames Number of frames to process.
109  * \param[in] attenuation factor for peakmeter adjustment
110  *
111  * Copy and scale volume from 32 bit source buffer
112  * to 32 bit destination buffer.
113  */
vol_s32_to_s32(struct processing_module * mod,struct input_stream_buffer * bsource,struct output_stream_buffer * bsink,uint32_t frames,uint32_t attenuation)114 static void vol_s32_to_s32(struct processing_module *mod, struct input_stream_buffer *bsource,
115 			   struct output_stream_buffer *bsink, uint32_t frames,
116 			   uint32_t attenuation)
117 {
118 	struct vol_data *cd = module_get_private_data(mod);
119 	struct audio_stream __sparse_cache *source = bsource->data;
120 	struct audio_stream __sparse_cache *sink = bsink->data;
121 	int32_t vol;
122 	int32_t *x, *x0;
123 	int32_t *y, *y0;
124 	int nmax, n, i, j;
125 	const int nch = source->channels;
126 	int remaining_samples = frames * nch;
127 	int32_t tmp;
128 
129 	memset(cd->peak_regs.peak_meter, 0, sizeof(uint32_t) * cd->channels);
130 	x = audio_stream_wrap(source, (char *)source->r_ptr + bsource->consumed);
131 	y = audio_stream_wrap(sink, (char *)sink->w_ptr + bsink->size);
132 	bsource->consumed += VOL_S32_SAMPLES_TO_BYTES(remaining_samples);
133 	bsink->size += VOL_S32_SAMPLES_TO_BYTES(remaining_samples);
134 	while (remaining_samples) {
135 		nmax = audio_stream_samples_without_wrap_s32(source, x);
136 		n = MIN(remaining_samples, nmax);
137 		nmax = audio_stream_samples_without_wrap_s32(sink, y);
138 		n = MIN(n, nmax);
139 		/* Note: on Xtensa processing one channel volume at time performed slightly
140 		 * better than simpler interleaved code version (average 19 us vs. 20 us).
141 		 */
142 		for (j = 0; j < nch; j++) {
143 			x0 = x + j;
144 			y0 = y + j;
145 			vol = cd->volume[j];
146 			tmp = 0;
147 			for (i = 0; i < n; i += nch) {
148 				y0[i] = q_multsr_sat_32x32(x0[i], vol,
149 							   Q_SHIFT_BITS_64(31, VOL_QXY_Y, 31));
150 				tmp = MAX(abs(x0[i]), tmp);
151 			}
152 			tmp = tmp << attenuation;
153 			cd->peak_regs.peak_meter[j] = MAX(tmp, cd->peak_regs.peak_meter[j]);
154 		}
155 		remaining_samples -= n;
156 		x = audio_stream_wrap(source, x + n);
157 		y = audio_stream_wrap(sink, y + n);
158 	}
159 
160 	/* update peak vol */
161 	peak_vol_update(cd);
162 }
163 #endif /* CONFIG_FORMAT_S32LE */
164 
165 #if CONFIG_FORMAT_S16LE
166 /**
167  * \brief Volume processing from 16 bit to 16 bit.
168  * \param[in,out] dev Volume base component device.
169  * \param[in,out] sink Destination buffer.
170  * \param[in,out] source Input buffer.
171  * \param[in] frames Number of frames to process.
172  * \param[in] attenuation factor for peakmeter adjustment (unused for 16bit)
173  *
174  * Copy and scale volume from 16 bit source buffer
175  * to 16 bit destination buffer.
176  */
vol_s16_to_s16(struct processing_module * mod,struct input_stream_buffer * bsource,struct output_stream_buffer * bsink,uint32_t frames,uint32_t attenuation)177 static void vol_s16_to_s16(struct processing_module *mod, struct input_stream_buffer *bsource,
178 			   struct output_stream_buffer *bsink, uint32_t frames,
179 			   uint32_t attenuation)
180 {
181 	struct vol_data *cd = module_get_private_data(mod);
182 	struct audio_stream __sparse_cache *source = bsource->data;
183 	struct audio_stream __sparse_cache *sink = bsink->data;
184 	int32_t vol;
185 	int16_t *x, *x0;
186 	int16_t *y, *y0;
187 	int nmax, n, i, j;
188 	const int nch = source->channels;
189 	int remaining_samples = frames * nch;
190 	int32_t tmp;
191 
192 	memset(cd->peak_regs.peak_meter, 0, sizeof(uint32_t) * cd->channels);
193 	x = audio_stream_wrap(source, (char *)source->r_ptr + bsource->consumed);
194 	y = audio_stream_wrap(sink, (char *)sink->w_ptr + bsink->size);
195 
196 	bsource->consumed += VOL_S16_SAMPLES_TO_BYTES(remaining_samples);
197 	bsink->size += VOL_S16_SAMPLES_TO_BYTES(remaining_samples);
198 	while (remaining_samples) {
199 		nmax = audio_stream_samples_without_wrap_s16(source, x);
200 		n = MIN(remaining_samples, nmax);
201 		nmax = audio_stream_samples_without_wrap_s16(sink, y);
202 		n = MIN(n, nmax);
203 		for (j = 0; j < nch; j++) {
204 			x0 = x + j;
205 			y0 = y + j;
206 			vol = cd->volume[j];
207 			tmp = 0;
208 			for (i = 0; i < n; i += nch) {
209 				y0[i] = q_multsr_sat_32x32_16(x0[i], vol,
210 							      Q_SHIFT_BITS_32(15, VOL_QXY_Y, 15));
211 				tmp = MAX(abs(x0[i]), tmp);
212 			}
213 			cd->peak_regs.peak_meter[j] = MAX(tmp, cd->peak_regs.peak_meter[j]);
214 		}
215 		remaining_samples -= n;
216 		x = audio_stream_wrap(source, x + n);
217 		y = audio_stream_wrap(sink, y + n);
218 	}
219 
220 	/* update peak vol */
221 	peak_vol_update(cd);
222 }
223 #endif
224 
225 const struct comp_func_map volume_func_map[] = {
226 #if CONFIG_FORMAT_S16LE
227 	{ SOF_IPC_FRAME_S16_LE, vol_s16_to_s16 },
228 #endif /* CONFIG_FORMAT_S16LE */
229 #if CONFIG_FORMAT_S24LE
230 	{ SOF_IPC_FRAME_S24_4LE, vol_s24_to_s24 },
231 #endif /* CONFIG_FORMAT_S24LE */
232 #if CONFIG_FORMAT_S32LE
233 	{ SOF_IPC_FRAME_S32_LE, vol_s32_to_s32 },
234 #endif /* CONFIG_FORMAT_S32LE */
235 };
236 
237 const size_t volume_func_count = ARRAY_SIZE(volume_func_map);
238 #endif
239 #endif
240