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
32namespace {
33
34constexpr 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
48template <typename T>
49inline T getPtrPtrValueAndIncrement(const void **data)
50{
51    return *(*reinterpret_cast<const T **>(data))++;
52}
53
54template <audio_format_t FORMAT>
55inline 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.
87template <audio_format_t FORMAT>
88constexpr 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
114template <audio_format_t FORMAT>
115constexpr inline float normalizeEnergy()
116{
117    const float val = normalizeAmplitude<FORMAT>();
118    return val * val;
119}
120
121template <audio_format_t FORMAT>
122inline 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>(&amplitudes);
127        accum += amplitude * amplitude;
128    }
129    return accum;
130}
131
132template <audio_format_t FORMAT>
133inline float energyMono(const void *amplitudes, size_t size)
134{
135    return energyMonoRef<FORMAT>(amplitudes, size);
136}
137
138// fast float power computation for ARM processors that support NEON.
139#ifdef USE_NEON
140
141template <typename T>
142float32x4_t convertToFloatVectorAmplitude(T vamplitude) = delete;
143
144template <>
145float32x4_t convertToFloatVectorAmplitude<float32x4_t>(float32x4_t vamplitude) {
146    return vamplitude;
147}
148
149template <>
150float32x4_t convertToFloatVectorAmplitude<int16x4_t>(int16x4_t vamplitude) {
151    const int32x4_t iamplitude = vmovl_s16(vamplitude); // expand s16 to s32 first
152    return vcvtq_f32_s32(iamplitude);
153}
154
155template <>
156float32x4_t convertToFloatVectorAmplitude<int32x4_t>(int32x4_t vamplitude) {
157    return vcvtq_f32_s32(vamplitude);
158}
159
160template <typename Vector, typename Scalar>
161inline float energyMonoVector(const void *amplitudes, size_t size)
162{
163    static_assert(sizeof(Vector) % sizeof(Scalar) == 0,
164             "Vector size must be a multiple of scalar size");
165    const size_t vectorLength = sizeof(Vector) / sizeof(Scalar); // typically 4 (a const)
166
167    // check pointer validity, must be aligned with scalar type.
168    const Scalar *samplitudes = reinterpret_cast<const Scalar *>(amplitudes);
169    LOG_ALWAYS_FATAL_IF((uintptr_t)samplitudes % alignof(Scalar) != 0,
170            "Non-element aligned address: %p %zu", samplitudes, alignof(Scalar));
171
172    float accumulator = 0;
173
174    // handle pointer unaligned to vector type.
175    while ((uintptr_t)samplitudes % alignof(Vector) != 0 /* compiler optimized */ && size > 0) {
176        const float amp = (float)*samplitudes++;
177        accumulator += amp * amp;
178        --size;
179    }
180
181    // samplitudes is now adjusted for proper vector alignment, cast to Vector *
182    const Vector *vamplitudes = reinterpret_cast<const Vector *>(samplitudes);
183
184    // clear vector accumulator
185    float32x4_t accum = vdupq_n_f32(0);
186
187    // iterate over array getting sum of squares in vectorLength lanes.
188    size_t i;
189    for (i = 0; i < size - size % vectorLength /* compiler optimized */; i += vectorLength) {
190        const float32x4_t famplitude = convertToFloatVectorAmplitude(*vamplitudes++);
191        accum = vmlaq_f32(accum, famplitude, famplitude);
192    }
193
194    // narrow vectorLength lanes of floats
195    float32x2_t accum2 = vadd_f32(vget_low_f32(accum), vget_high_f32(accum)); // get stereo volume
196    accum2 = vpadd_f32(accum2, accum2); // combine to mono
197
198    // accumulate vector
199    accumulator += vget_lane_f32(accum2, 0);
200
201    // accumulate any trailing elements too small for vector size
202    for (; i < size; ++i) {
203        const float amp = (float)samplitudes[i];
204        accumulator += amp * amp;
205    }
206    return accumulator;
207}
208
209template <>
210inline float energyMono<AUDIO_FORMAT_PCM_FLOAT>(const void *amplitudes, size_t size)
211{
212    return energyMonoVector<float32x4_t, float>(amplitudes, size);
213}
214
215template <>
216inline float energyMono<AUDIO_FORMAT_PCM_16_BIT>(const void *amplitudes, size_t size)
217{
218    return energyMonoVector<int16x4_t, int16_t>(amplitudes, size)
219            * normalizeEnergy<AUDIO_FORMAT_PCM_16_BIT>();
220}
221
222// fast int32_t power computation for PCM_32
223template <>
224inline float energyMono<AUDIO_FORMAT_PCM_32_BIT>(const void *amplitudes, size_t size)
225{
226    return energyMonoVector<int32x4_t, int32_t>(amplitudes, size)
227            * normalizeEnergy<AUDIO_FORMAT_PCM_32_BIT>();
228}
229
230// fast int32_t power computation for PCM_8_24 (essentially identical to PCM_32 above)
231template <>
232inline float energyMono<AUDIO_FORMAT_PCM_8_24_BIT>(const void *amplitudes, size_t size)
233{
234    return energyMonoVector<int32x4_t, int32_t>(amplitudes, size)
235            * normalizeEnergy<AUDIO_FORMAT_PCM_8_24_BIT>();
236}
237
238#endif // USE_NEON
239
240} // namespace
241
242float audio_utils_compute_energy_mono(const void *buffer, audio_format_t format, size_t samples)
243{
244    switch (format) {
245    case AUDIO_FORMAT_PCM_8_BIT:
246        return energyMono<AUDIO_FORMAT_PCM_8_BIT>(buffer, samples);
247
248    case AUDIO_FORMAT_PCM_16_BIT:
249        return energyMono<AUDIO_FORMAT_PCM_16_BIT>(buffer, samples);
250
251    case AUDIO_FORMAT_PCM_24_BIT_PACKED:
252        return energyMono<AUDIO_FORMAT_PCM_24_BIT_PACKED>(buffer, samples);
253
254    case AUDIO_FORMAT_PCM_8_24_BIT:
255        return energyMono<AUDIO_FORMAT_PCM_8_24_BIT>(buffer, samples);
256
257    case AUDIO_FORMAT_PCM_32_BIT:
258        return energyMono<AUDIO_FORMAT_PCM_32_BIT>(buffer, samples);
259
260    case AUDIO_FORMAT_PCM_FLOAT:
261        return energyMono<AUDIO_FORMAT_PCM_FLOAT>(buffer, samples);
262
263    default:
264        LOG_ALWAYS_FATAL("invalid format: %#x", format);
265    }
266}
267
268float audio_utils_compute_power_mono(const void *buffer, audio_format_t format, size_t samples)
269{
270    return audio_utils_power_from_energy(
271            audio_utils_compute_energy_mono(buffer, format, samples) / samples);
272}
273
274bool audio_utils_is_compute_power_format_supported(audio_format_t format)
275{
276    return isFormatSupported(format);
277}
278
279