audio_utils/power.cpp

   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