1 /*
2 * Copyright 2017 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 // #define LOG_NDEBUG 0
18 #define LOG_TAG "audio_utils_power"
19 #include <log/log.h>
20
21 #include <algorithm>
22 #include <math.h>
23
24 #include <audio_utils/power.h>
25 #include <audio_utils/primitives.h>
26
27 #if defined(__aarch64__) || defined(__ARM_NEON__)
28 #include <arm_neon.h>
29 #define USE_NEON
30 #endif
31
32 namespace {
33
isFormatSupported(audio_format_t format)34 constexpr inline bool isFormatSupported(audio_format_t format) {
35 switch (format) {
36 case AUDIO_FORMAT_PCM_8_BIT:
37 case AUDIO_FORMAT_PCM_16_BIT:
38 case AUDIO_FORMAT_PCM_24_BIT_PACKED:
39 case AUDIO_FORMAT_PCM_8_24_BIT:
40 case AUDIO_FORMAT_PCM_32_BIT:
41 case AUDIO_FORMAT_PCM_FLOAT:
42 return true;
43 default:
44 return false;
45 }
46 }
47
48 template <typename T>
getPtrPtrValueAndIncrement(const void ** data)49 inline T getPtrPtrValueAndIncrement(const void **data)
50 {
51 return *(*reinterpret_cast<const T **>(data))++;
52 }
53
54 template <audio_format_t FORMAT>
convertToFloatAndIncrement(const void ** data)55 inline float convertToFloatAndIncrement(const void **data)
56 {
57 switch (FORMAT) {
58 case AUDIO_FORMAT_PCM_8_BIT:
59 return float_from_u8(getPtrPtrValueAndIncrement<uint8_t>(data));
60
61 case AUDIO_FORMAT_PCM_16_BIT:
62 return float_from_i16(getPtrPtrValueAndIncrement<int16_t>(data));
63
64 case AUDIO_FORMAT_PCM_24_BIT_PACKED: {
65 const uint8_t *uptr = reinterpret_cast<const uint8_t *>(*data);
66 *data = uptr + 3;
67 return float_from_p24(uptr);
68 }
69
70 case AUDIO_FORMAT_PCM_8_24_BIT:
71 return float_from_q8_23(getPtrPtrValueAndIncrement<int32_t>(data));
72
73 case AUDIO_FORMAT_PCM_32_BIT:
74 return float_from_i32(getPtrPtrValueAndIncrement<int32_t>(data));
75
76 case AUDIO_FORMAT_PCM_FLOAT:
77 return getPtrPtrValueAndIncrement<float>(data);
78
79 default:
80 // static_assert cannot use false because the compiler may interpret it
81 // even though this code path may never be taken.
82 static_assert(isFormatSupported(FORMAT), "unsupported format");
83 }
84 }
85
86 // used to normalize integer fixed point value to the floating point equivalent.
87 template <audio_format_t FORMAT>
normalizeAmplitude()88 constexpr inline float normalizeAmplitude()
89 {
90 switch (FORMAT) {
91 case AUDIO_FORMAT_PCM_8_BIT:
92 return 1.f / (1 << 7);
93
94 case AUDIO_FORMAT_PCM_16_BIT:
95 return 1.f / (1 << 15);
96
97 case AUDIO_FORMAT_PCM_24_BIT_PACKED: // fall through
98 case AUDIO_FORMAT_PCM_8_24_BIT:
99 return 1.f / (1 << 23);
100
101 case AUDIO_FORMAT_PCM_32_BIT:
102 return 1.f / (1U << 31);
103
104 case AUDIO_FORMAT_PCM_FLOAT:
105 return 1.f;
106
107 default:
108 // static_assert cannot use false because the compiler may interpret it
109 // even though this code path may never be taken.
110 static_assert(isFormatSupported(FORMAT), "unsupported format");
111 }
112 }
113
114 template <audio_format_t FORMAT>
normalizeEnergy()115 constexpr inline float normalizeEnergy()
116 {
117 const float val = normalizeAmplitude<FORMAT>();
118 return val * val;
119 }
120
121 template <audio_format_t FORMAT>
energyMonoRef(const void * amplitudes,size_t size)122 inline float energyMonoRef(const void *amplitudes, size_t size)
123 {
124 float accum(0.f);
125 for (size_t i = 0; i < size; ++i) {
126 const float amplitude = convertToFloatAndIncrement<FORMAT>(&litudes);
127 accum += amplitude * amplitude;
128 }
129 return accum;
130 }
131
132 template <audio_format_t FORMAT>
energyRef(const void * amplitudes,size_t size,size_t numChannels,float * out)133 inline void energyRef(const void *amplitudes, size_t size, size_t numChannels, float* out)
134 {
135 const size_t framesSize = size / numChannels;
136 for (size_t i = 0; i < framesSize; ++i) {
137 for (size_t c = 0; c < numChannels; ++c) {
138 const float amplitude = convertToFloatAndIncrement<FORMAT>(&litudes);
139 out[c] += amplitude * amplitude;
140 }
141 }
142 }
143
144 template <audio_format_t FORMAT>
energyMono(const void * amplitudes,size_t size)145 inline float energyMono(const void *amplitudes, size_t size)
146 {
147 return energyMonoRef<FORMAT>(amplitudes, size);
148 }
149
150 // TODO: optimize with NEON
151 template <audio_format_t FORMAT>
energy(const void * amplitudes,size_t size,size_t numChannels,float * out)152 inline void energy(const void *amplitudes, size_t size, size_t numChannels, float* out)
153 {
154 energyRef<FORMAT>(amplitudes, size, numChannels, out);
155 }
156
157 // fast float power computation for ARM processors that support NEON.
158 #ifdef USE_NEON
159
160 template <typename T>
161 float32x4_t convertToFloatVectorAmplitude(T vamplitude) = delete;
162
163 template <>
convertToFloatVectorAmplitude(float32x4_t vamplitude)164 float32x4_t convertToFloatVectorAmplitude<float32x4_t>(float32x4_t vamplitude) {
165 return vamplitude;
166 }
167
168 template <>
convertToFloatVectorAmplitude(int16x4_t vamplitude)169 float32x4_t convertToFloatVectorAmplitude<int16x4_t>(int16x4_t vamplitude) {
170 const int32x4_t iamplitude = vmovl_s16(vamplitude); // expand s16 to s32 first
171 return vcvtq_f32_s32(iamplitude);
172 }
173
174 template <>
convertToFloatVectorAmplitude(int32x4_t vamplitude)175 float32x4_t convertToFloatVectorAmplitude<int32x4_t>(int32x4_t vamplitude) {
176 return vcvtq_f32_s32(vamplitude);
177 }
178
179 template <typename Vector, typename Scalar>
energyMonoVector(const void * amplitudes,size_t size)180 inline float energyMonoVector(const void *amplitudes, size_t size)
181 {
182 static_assert(sizeof(Vector) % sizeof(Scalar) == 0,
183 "Vector size must be a multiple of scalar size");
184 const size_t vectorLength = sizeof(Vector) / sizeof(Scalar); // typically 4 (a const)
185
186 // check pointer validity, must be aligned with scalar type.
187 const Scalar *samplitudes = reinterpret_cast<const Scalar *>(amplitudes);
188 LOG_ALWAYS_FATAL_IF((uintptr_t)samplitudes % alignof(Scalar) != 0,
189 "Non-element aligned address: %p %zu", samplitudes, alignof(Scalar));
190
191 float accumulator = 0;
192
193 // handle pointer unaligned to vector type.
194 while ((uintptr_t)samplitudes % alignof(Vector) != 0 /* compiler optimized */ && size > 0) {
195 const float amp = (float)*samplitudes++;
196 accumulator += amp * amp;
197 --size;
198 }
199
200 // samplitudes is now adjusted for proper vector alignment, cast to Vector *
201 const Vector *vamplitudes = reinterpret_cast<const Vector *>(samplitudes);
202
203 // clear vector accumulator
204 float32x4_t accum = vdupq_n_f32(0);
205
206 // iterate over array getting sum of squares in vectorLength lanes.
207 size_t i;
208 for (i = 0; i < size - size % vectorLength /* compiler optimized */; i += vectorLength) {
209 const float32x4_t famplitude = convertToFloatVectorAmplitude(*vamplitudes++);
210 accum = vmlaq_f32(accum, famplitude, famplitude);
211 }
212
213 // narrow vectorLength lanes of floats
214 float32x2_t accum2 = vadd_f32(vget_low_f32(accum), vget_high_f32(accum)); // get stereo volume
215 accum2 = vpadd_f32(accum2, accum2); // combine to mono
216
217 // accumulate vector
218 accumulator += vget_lane_f32(accum2, 0);
219
220 // accumulate any trailing elements too small for vector size
221 for (; i < size; ++i) {
222 const float amp = (float)samplitudes[i];
223 accumulator += amp * amp;
224 }
225 return accumulator;
226 }
227
228 template <>
energyMono(const void * amplitudes,size_t size)229 inline float energyMono<AUDIO_FORMAT_PCM_FLOAT>(const void *amplitudes, size_t size)
230 {
231 return energyMonoVector<float32x4_t, float>(amplitudes, size);
232 }
233
234 template <>
energyMono(const void * amplitudes,size_t size)235 inline float energyMono<AUDIO_FORMAT_PCM_16_BIT>(const void *amplitudes, size_t size)
236 {
237 return energyMonoVector<int16x4_t, int16_t>(amplitudes, size)
238 * normalizeEnergy<AUDIO_FORMAT_PCM_16_BIT>();
239 }
240
241 // fast int32_t power computation for PCM_32
242 template <>
energyMono(const void * amplitudes,size_t size)243 inline float energyMono<AUDIO_FORMAT_PCM_32_BIT>(const void *amplitudes, size_t size)
244 {
245 return energyMonoVector<int32x4_t, int32_t>(amplitudes, size)
246 * normalizeEnergy<AUDIO_FORMAT_PCM_32_BIT>();
247 }
248
249 // fast int32_t power computation for PCM_8_24 (essentially identical to PCM_32 above)
250 template <>
energyMono(const void * amplitudes,size_t size)251 inline float energyMono<AUDIO_FORMAT_PCM_8_24_BIT>(const void *amplitudes, size_t size)
252 {
253 return energyMonoVector<int32x4_t, int32_t>(amplitudes, size)
254 * normalizeEnergy<AUDIO_FORMAT_PCM_8_24_BIT>();
255 }
256
257 #endif // USE_NEON
258
259 } // namespace
260
audio_utils_compute_energy_mono(const void * buffer,audio_format_t format,size_t samples)261 float audio_utils_compute_energy_mono(const void *buffer, audio_format_t format, size_t samples)
262 {
263 switch (format) {
264 case AUDIO_FORMAT_PCM_8_BIT:
265 return energyMono<AUDIO_FORMAT_PCM_8_BIT>(buffer, samples);
266
267 case AUDIO_FORMAT_PCM_16_BIT:
268 return energyMono<AUDIO_FORMAT_PCM_16_BIT>(buffer, samples);
269
270 case AUDIO_FORMAT_PCM_24_BIT_PACKED:
271 return energyMono<AUDIO_FORMAT_PCM_24_BIT_PACKED>(buffer, samples);
272
273 case AUDIO_FORMAT_PCM_8_24_BIT:
274 return energyMono<AUDIO_FORMAT_PCM_8_24_BIT>(buffer, samples);
275
276 case AUDIO_FORMAT_PCM_32_BIT:
277 return energyMono<AUDIO_FORMAT_PCM_32_BIT>(buffer, samples);
278
279 case AUDIO_FORMAT_PCM_FLOAT:
280 return energyMono<AUDIO_FORMAT_PCM_FLOAT>(buffer, samples);
281
282 default:
283 LOG_ALWAYS_FATAL("invalid format: %#x", format);
284 }
285 }
286
audio_utils_accumulate_energy(const void * buffer,audio_format_t format,size_t samples,size_t numChannels,float * out)287 void audio_utils_accumulate_energy(const void* buffer,
288 audio_format_t format,
289 size_t samples,
290 size_t numChannels,
291 float* out)
292 {
293 switch (format) {
294 case AUDIO_FORMAT_PCM_8_BIT:
295 energy<AUDIO_FORMAT_PCM_8_BIT>(buffer, samples, numChannels, out);
296 break;
297
298 case AUDIO_FORMAT_PCM_16_BIT:
299 energy<AUDIO_FORMAT_PCM_16_BIT>(buffer, samples, numChannels, out);
300 break;
301
302 case AUDIO_FORMAT_PCM_24_BIT_PACKED:
303 energy<AUDIO_FORMAT_PCM_24_BIT_PACKED>(buffer, samples, numChannels, out);
304 break;
305
306 case AUDIO_FORMAT_PCM_8_24_BIT:
307 energy<AUDIO_FORMAT_PCM_8_24_BIT>(buffer, samples, numChannels, out);
308 break;
309
310 case AUDIO_FORMAT_PCM_32_BIT:
311 energy<AUDIO_FORMAT_PCM_32_BIT>(buffer, samples, numChannels, out);
312 break;
313
314 case AUDIO_FORMAT_PCM_FLOAT:
315 energy<AUDIO_FORMAT_PCM_FLOAT>(buffer, samples, numChannels, out);
316 break;
317
318 default:
319 LOG_ALWAYS_FATAL("invalid format: %#x", format);
320 }
321 }
322
audio_utils_compute_power_mono(const void * buffer,audio_format_t format,size_t samples)323 float audio_utils_compute_power_mono(const void *buffer, audio_format_t format, size_t samples)
324 {
325 return audio_utils_power_from_energy(
326 audio_utils_compute_energy_mono(buffer, format, samples) / samples);
327 }
328
audio_utils_is_compute_power_format_supported(audio_format_t format)329 bool audio_utils_is_compute_power_format_supported(audio_format_t format)
330 {
331 return isFormatSupported(format);
332 }
333