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 
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>
49 inline T getPtrPtrValueAndIncrement(const void **data)
50 {
51     return *(*reinterpret_cast<const T **>(data))++;
52 }
53 
54 template <audio_format_t FORMAT>
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>
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>
115 constexpr inline float normalizeEnergy()
116 {
117     const float val = normalizeAmplitude<FORMAT>();
118     return val * val;
119 }
120 
121 template <audio_format_t FORMAT>
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>(&amplitudes);
127         accum += amplitude * amplitude;
128     }
129     return accum;
130 }
131 
132 template <audio_format_t FORMAT>
133 inline 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 
141 template <typename T>
142 float32x4_t convertToFloatVectorAmplitude(T vamplitude) = delete;
143 
144 template <>
145 float32x4_t convertToFloatVectorAmplitude<float32x4_t>(float32x4_t vamplitude) {
146     return vamplitude;
147 }
148 
149 template <>
150 float32x4_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 
155 template <>
156 float32x4_t convertToFloatVectorAmplitude<int32x4_t>(int32x4_t vamplitude) {
157     return vcvtq_f32_s32(vamplitude);
158 }
159 
160 template <typename Vector, typename Scalar>
161 inline 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 
209 template <>
210 inline float energyMono<AUDIO_FORMAT_PCM_FLOAT>(const void *amplitudes, size_t size)
211 {
212     return energyMonoVector<float32x4_t, float>(amplitudes, size);
213 }
214 
215 template <>
216 inline 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
223 template <>
224 inline 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)
231 template <>
232 inline 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 
242 float 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 
268 float 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 
274 bool audio_utils_is_compute_power_format_supported(audio_format_t format)
275 {
276     return isFormatSupported(format);
277 }
278 
279