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