1 /*
2  * Copyright 2022 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_MelProcessor"
19 // #define VERY_VERY_VERBOSE_LOGGING
20 #ifdef VERY_VERY_VERBOSE_LOGGING
21 #define ALOGVV ALOGV
22 #else
23 #define ALOGVV(a...) do { } while(0)
24 #endif
25 
26 #include <audio_utils/MelProcessor.h>
27 
28 #include <audio_utils/format.h>
29 #include <audio_utils/power.h>
30 #include <log/log.h>
31 #include <sstream>
32 #include <unordered_map>
33 #include <utils/threads.h>
34 
35 namespace android::audio_utils {
36 
37 constexpr int32_t kSecondsPerMelValue = 1;
38 constexpr float kMelAdjustmentDb = -3.f;
39 
40 // Estimated offset defined in Table39 of IEC62368-1 3rd edition
41 // -30dBFS, -10dBFS should correspond to 80dBSPL, 100dBSPL
42 constexpr float kMeldBFSTodBSPLOffset = 110.f;
43 
44 constexpr float kRs1OutputdBFS = 80.f;  // dBA
45 
46 constexpr float kRs2LowerBound = 80.f;  // dBA
47 constexpr float kRs2UpperBound = 100.f;  // dBA
48 
49 // The following arrays contain the IIR biquad filter coefficients for performing A-weighting as
50 // described in IEC 61672:2003 for multiple sample rates. The format is b0, b1, b2, a1, a2
51 constexpr std::array<std::array<float, kBiquadNumCoefs>, MelProcessor::kCascadeBiquadNumber>
52     kBqCoeffs8000 = {{{0.630301, 0.000000, -0.630301, 0.103818, -0.360417},
53                       {1.000000, 0.000000, -1.000000, -0.264382, -0.601403},
54                       {1.000000, -2.000000, 1.000000, -1.967903, 0.968160}}};
55 constexpr std::array<std::array<float, kBiquadNumCoefs>, MelProcessor::kCascadeBiquadNumber>
56     kBqCoeffs11025 = {{{0.601164, 1.202327, 0.601164, 1.106098, 0.305863},
57                        {1.000000, -2.000000, 1.000000, -1.593019, 0.613701},
58                        {1.000000, -2.000000, 1.000000, -1.976658, 0.976794}}};
59 constexpr std::array<std::array<float, kBiquadNumCoefs>, MelProcessor::kCascadeBiquadNumber>
60     kBqCoeffs12000 = {{{0.588344, 1.176688, 0.588344, 1.045901, 0.273477},
61                        {1.000000, -2.000000, 1.000000, -1.621383, 0.639134},
62                        {1.000000, -2.000000, 1.000000, -1.978544, 0.978660}}};
63 constexpr std::array<std::array<float, kBiquadNumCoefs>, MelProcessor::kCascadeBiquadNumber>
64     kBqCoeffs16000 = {{{0.531220, 1.062441, 0.531220, 0.821564, 0.168742},
65                        {1.000000, -2.000000, 1.000000, -1.705510, 0.715988},
66                        {1.000000, -2.000000, 1.000000, -1.983887, 0.983952}}};
67 constexpr std::array<std::array<float, kBiquadNumCoefs>, MelProcessor::kCascadeBiquadNumber>
68     kBqCoeffs22050 = {{{0.449072, 0.898144, 0.449072, 0.538750, 0.072563},
69                        {1.000000, -2.000000, 1.000000, -1.779533, 0.785281},
70                        {1.000000, -2.000000, 1.000000, -1.988295, 0.988329}}};
71 constexpr std::array<std::array<float, kBiquadNumCoefs>, MelProcessor::kCascadeBiquadNumber>
72     kBqCoeffs24000 = {{{0.425411, 0.850821, 0.425411, 0.459298, 0.052739},
73                        {1.000000, -2.000000, 1.000000, -1.796051, 0.800946},
74                        {1.000000, -2.000000, 1.000000, -1.989243, 0.989272}}};
75 constexpr std::array<std::array<float, kBiquadNumCoefs>, MelProcessor::kCascadeBiquadNumber>
76     kBqCoeffs32000 = {{{0.343284, 0.686569, 0.343284, 0.179472, 0.008053},
77                        {1.000000, -2.000000, 1.000000, -1.843991, 0.846816},
78                        {1.000000, -2.000000, 1.000000, -1.991927, 0.991943}}};
79 constexpr std::array<std::array<float, kBiquadNumCoefs>, MelProcessor::kCascadeBiquadNumber>
80     kBqCoeffs44100 = {{{0.255612, 0.511223, 0.255612, -0.140536, 0.004938},
81                        {1.000000, -2.000000, 1.000000, -1.884901, 0.886421},
82                        {1.000000, -2.000000, 1.000000, -1.994139, 0.994147}}};
83 constexpr std::array<std::array<float, kBiquadNumCoefs>, MelProcessor::kCascadeBiquadNumber>
84     kBqCoeffs48000 = {{{0.234183, 0.468366, 0.234183, -0.224558, 0.012607},
85                        {1.000000, -2.000000, 1.000000, -1.893870, 0.895160},
86                        {1.000000, -2.000000, 1.000000, -1.994614, 0.994622}}};
87 constexpr std::array<std::array<float, kBiquadNumCoefs>, MelProcessor::kCascadeBiquadNumber>
88     kBqCoeffs64000 = {{{0.169014, 0.338029, 0.169014, -0.502217, 0.063056},
89                        {1.000000, -2.000000, 1.000000, -1.919579, 0.920314},
90                        {1.000000, -2.000000, 1.000000, -1.995959, 0.995964}}};
91 constexpr std::array<std::array<float, kBiquadNumCoefs>, MelProcessor::kCascadeBiquadNumber>
92     kBqCoeffs88200 = {{{0.111831, 0.223662, 0.111831, -0.788729, 0.155523},
93                        {1.000000, -2.000000, 1.000000, -1.941143, 0.941534},
94                        {1.000000, -2.000000, 1.000000, -1.997067, 0.997069}}};
95 constexpr std::array<std::array<float, kBiquadNumCoefs>, MelProcessor::kCascadeBiquadNumber>
96     kBqCoeffs96000 = {{{0.099469, 0.198937, 0.099469, -0.859073, 0.184502},
97                        {1.000000, -2.000000, 1.000000, -1.945825, 0.946156},
98                        {1.000000, -2.000000, 1.000000, -1.997305, 0.997307}}};
99 constexpr std::array<std::array<float, kBiquadNumCoefs>, MelProcessor::kCascadeBiquadNumber>
100     kBqCoeffs128000 = {{{0.065337, 0.130674, 0.065337, -1.078602, 0.290845},
101                         {1.000000, -2.000000, 1.000000, -1.959154, 0.959342},
102                         {1.000000, -2.000000, 1.000000, -1.997979, 0.997980}}};
103 constexpr std::array<std::array<float, kBiquadNumCoefs>, MelProcessor::kCascadeBiquadNumber>
104     kBqCoeffs176400 = {{{0.039432, 0.078864, 0.039432, -1.286304, 0.413645},
105                         {1.000000, -2.000000, 1.000000, -1.970232, 0.970331},
106                         {1.000000, -2.000000, 1.000000, -1.998533, 0.998534}}};
107 constexpr std::array<std::array<float, kBiquadNumCoefs>, MelProcessor::kCascadeBiquadNumber>
108     kBqCoeffs192000 = {{{0.034315, 0.068629, 0.034315, -1.334647, 0.445320},
109                         {1.000000, -2.000000, 1.000000, -1.972625, 0.972709},
110                         {1.000000, -2.000000, 1.000000, -1.998652, 0.998653}}};
111 
MelProcessor(uint32_t sampleRate,uint32_t channelCount,audio_format_t format,const sp<MelCallback> & callback,audio_port_handle_t deviceId,float rs2Value,size_t maxMelsCallback)112 MelProcessor::MelProcessor(uint32_t sampleRate,
113         uint32_t channelCount,
114         audio_format_t format,
115         const sp<MelCallback>& callback,
116         audio_port_handle_t deviceId,
117         float rs2Value,
118         size_t maxMelsCallback)
119     : mCallback(callback),
120       mMelWorker("MelWorker#" + pointerString(), mCallback),
121       mSampleRate(sampleRate),
122       mFramesPerMelValue(sampleRate * kSecondsPerMelValue),
123       mChannelCount(channelCount),
124       mFormat(format),
125       mAWeightSamples(mFramesPerMelValue * mChannelCount),
126       mFloatSamples(mFramesPerMelValue * mChannelCount),
127       mCurrentChannelEnergy(channelCount, 0.0f),
128       mMelValues(maxMelsCallback),
129       mCurrentIndex(0),
130       mDeviceId(deviceId),
131       mRs2UpperBound(rs2Value),
132       mCurrentSamples(0)
133 {
134     createBiquads_l();
135 
136     mMelWorker.run();
137 }
138 
139 static const std::unordered_map<uint32_t, const std::array<std::array<float, kBiquadNumCoefs>,
getSampleRateBiquadCoeffs()140         MelProcessor::kCascadeBiquadNumber>*>& getSampleRateBiquadCoeffs() {
141     static const std::unordered_map<uint32_t, const std::array<std::array<float, kBiquadNumCoefs>,
142                              MelProcessor::kCascadeBiquadNumber>*> sampleRateBiquadCoeffs = {
143             {8000, &kBqCoeffs8000},
144             {11025, &kBqCoeffs11025},
145             {12000, &kBqCoeffs12000},
146             {16000, &kBqCoeffs16000},
147             {22050, &kBqCoeffs22050},
148             {24000, &kBqCoeffs24000},
149             {32000, &kBqCoeffs32000},
150             {44100, &kBqCoeffs44100},
151             {48000, &kBqCoeffs48000},
152             {64000, &kBqCoeffs64000},
153             {88200, &kBqCoeffs88200},
154             {96000, &kBqCoeffs96000},
155             {128000, &kBqCoeffs128000},
156             {176400, &kBqCoeffs176400},
157             {192000, &kBqCoeffs192000},
158         };
159     return sampleRateBiquadCoeffs;
160 }
161 
isSampleRateSupported_l() const162 bool MelProcessor::isSampleRateSupported_l() const {
163     return getSampleRateBiquadCoeffs().count(mSampleRate) != 0;
164 }
165 
createBiquads_l()166 void MelProcessor::createBiquads_l() {
167     if (!isSampleRateSupported_l()) {
168         return;
169     }
170 
171     const auto& biquadCoeffs = getSampleRateBiquadCoeffs().at(mSampleRate); // checked above
172     mCascadedBiquads =
173               {std::make_unique<DefaultBiquadFilter>(mChannelCount, biquadCoeffs->at(0)),
174                std::make_unique<DefaultBiquadFilter>(mChannelCount, biquadCoeffs->at(1)),
175                std::make_unique<DefaultBiquadFilter>(mChannelCount, biquadCoeffs->at(2))};
176 }
177 
setOutputRs2UpperBound(float rs2Value)178 status_t MelProcessor::setOutputRs2UpperBound(float rs2Value)
179 {
180     if (rs2Value < kRs2LowerBound || rs2Value > kRs2UpperBound) {
181         return BAD_VALUE;
182     }
183 
184     mRs2UpperBound = rs2Value;
185 
186     return NO_ERROR;
187 }
188 
getOutputRs2UpperBound() const189 float MelProcessor::getOutputRs2UpperBound() const
190 {
191     return mRs2UpperBound;
192 }
193 
setDeviceId(audio_port_handle_t deviceId)194 void MelProcessor::setDeviceId(audio_port_handle_t deviceId)
195 {
196     mDeviceId = deviceId;
197 }
198 
getDeviceId()199 audio_port_handle_t MelProcessor::getDeviceId() {
200     return mDeviceId;
201 }
202 
pause()203 void MelProcessor::pause()
204 {
205     ALOGV("%s", __func__);
206     mPaused = true;
207 }
208 
resume()209 void MelProcessor::resume()
210 {
211     ALOGV("%s", __func__);
212     mPaused = false;
213 }
214 
updateAudioFormat(uint32_t sampleRate,uint32_t channelCount,audio_format_t format)215 void MelProcessor::updateAudioFormat(uint32_t sampleRate,
216                                      uint32_t channelCount,
217                                      audio_format_t format) {
218     ALOGV("%s: update audio format %u, %u, %d", __func__, sampleRate, channelCount, format);
219 
220     std::lock_guard l(mLock);
221 
222     bool differentSampleRate = (mSampleRate != sampleRate);
223     bool differentChannelCount = (mChannelCount != channelCount);
224 
225     mSampleRate = sampleRate;
226     mFramesPerMelValue = sampleRate * kSecondsPerMelValue;
227     mChannelCount = channelCount;
228     mFormat = format;
229 
230     if (differentSampleRate || differentChannelCount) {
231         mAWeightSamples.resize(mFramesPerMelValue * mChannelCount);
232         mFloatSamples.resize(mFramesPerMelValue * mChannelCount);
233     }
234     if (differentChannelCount) {
235         mCurrentChannelEnergy.resize(channelCount);
236     }
237 
238     createBiquads_l();
239 }
240 
applyAWeight_l(const void * buffer,size_t samples)241 void MelProcessor::applyAWeight_l(const void* buffer, size_t samples)
242 {
243     memcpy_by_audio_format(mFloatSamples.data(), AUDIO_FORMAT_PCM_FLOAT, buffer, mFormat, samples);
244 
245     float* tempFloat[2] = { mFloatSamples.data(), mAWeightSamples.data() };
246     int inIdx = 1, outIdx = 0;
247     const size_t frames = samples / mChannelCount;
248     for (const auto& biquad : mCascadedBiquads) {
249         outIdx ^= 1;
250         inIdx ^= 1;
251         biquad->process(tempFloat[outIdx], tempFloat[inIdx], frames);
252     }
253 
254     // should not be the case since the size is odd
255     if (!(mCascadedBiquads.size() & 1)) {
256         std::swap(mFloatSamples, mAWeightSamples);
257     }
258 }
259 
getCombinedChannelEnergy_l()260 float MelProcessor::getCombinedChannelEnergy_l() {
261     float combinedEnergy = 0.0f;
262     for (auto& energy: mCurrentChannelEnergy) {
263         combinedEnergy += energy;
264         energy = 0;
265     }
266 
267     combinedEnergy /= (float) mFramesPerMelValue;
268     return combinedEnergy;
269 }
270 
addMelValue_l(float mel)271 void MelProcessor::addMelValue_l(float mel) {
272     mMelValues[mCurrentIndex] = mel;
273     ALOGV("%s: writing MEL %f at index %d for device %d",
274           __func__,
275           mel,
276           mCurrentIndex,
277           mDeviceId.load());
278 
279     bool notifyWorker = false;
280 
281     if (mel > mRs2UpperBound) {
282         mMelWorker.momentaryExposure(mel, mDeviceId);
283         notifyWorker = true;
284     }
285 
286     bool reportContinuousValues = false;
287     if ((mMelValues[mCurrentIndex] < kRs1OutputdBFS && mCurrentIndex > 0)) {
288         reportContinuousValues = true;
289     } else if (mMelValues[mCurrentIndex] >= kRs1OutputdBFS) {
290         // only store MEL that are above RS1
291         ++mCurrentIndex;
292     }
293 
294     if (reportContinuousValues || (mCurrentIndex > mMelValues.size() - 1)) {
295         mMelWorker.newMelValues(mMelValues, mCurrentIndex, mDeviceId);
296         notifyWorker = true;
297         mCurrentIndex = 0;
298     }
299 
300     if (notifyWorker) {
301         mMelWorker.mCondVar.notify_one();
302     }
303 }
304 
process(const void * buffer,size_t bytes)305 int32_t MelProcessor::process(const void* buffer, size_t bytes) {
306     if (mPaused) {
307         return 0;
308     }
309 
310     // should be uncontested and not block if process method is called from a single thread
311     std::lock_guard<std::mutex> guard(mLock);
312 
313     if (!isSampleRateSupported_l()) {
314         return 0;
315     }
316 
317     const size_t bytes_per_sample = audio_bytes_per_sample(mFormat);
318     size_t samples = bytes_per_sample > 0 ? bytes / bytes_per_sample : 0;
319     while (samples > 0) {
320         const size_t requiredSamples =
321             mFramesPerMelValue * mChannelCount - mCurrentSamples;
322         size_t processSamples = std::min(requiredSamples, samples);
323         processSamples -= processSamples % mChannelCount;
324 
325         applyAWeight_l(buffer, processSamples);
326 
327         audio_utils_accumulate_energy(mAWeightSamples.data(),
328                                       AUDIO_FORMAT_PCM_FLOAT,
329                                       processSamples,
330                                       mChannelCount,
331                                       mCurrentChannelEnergy.data());
332         mCurrentSamples += processSamples;
333 
334         ALOGVV(
335             "required:%zu, process:%zu, mCurrentChannelEnergy[0]:%f, mCurrentSamples:%zu",
336             requiredSamples,
337             processSamples,
338             mCurrentChannelEnergy[0],
339             mCurrentSamples.load());
340         if (processSamples < requiredSamples) {
341             return static_cast<int32_t>(bytes);
342         }
343 
344         addMelValue_l(fmaxf(
345             audio_utils_power_from_energy(getCombinedChannelEnergy_l())
346                 + kMelAdjustmentDb
347                 + kMeldBFSTodBSPLOffset
348                 - mAttenuationDB, 0.0f));
349 
350         samples -= processSamples;
351         buffer =
352             (const uint8_t*) buffer + processSamples * bytes_per_sample;
353         mCurrentSamples = 0;
354     }
355 
356     return static_cast<int32_t>(bytes);
357 }
358 
setAttenuation(float attenuationDB)359 void MelProcessor::setAttenuation(float attenuationDB) {
360     ALOGV("%s: setting the attenuation %f", __func__, attenuationDB);
361     mAttenuationDB = attenuationDB;
362 }
363 
onLastStrongRef(const void * id)364 void MelProcessor::onLastStrongRef(const void* id __attribute__((unused))) {
365    mMelWorker.stop();
366    ALOGV("%s: Stopped thread: %s for device %d", __func__, mMelWorker.mThreadName.c_str(),
367          mDeviceId.load());
368 }
369 
pointerString() const370 std::string MelProcessor::pointerString() const {
371     const void * address = static_cast<const void*>(this);
372     std::stringstream aStream;
373     aStream << address;
374     return aStream.str();
375 }
376 
run()377 void MelProcessor::MelWorker::run() {
378     mThread = std::thread([&]{
379         // name the thread to help identification
380         androidSetThreadName(mThreadName.c_str());
381         ALOGV("%s::run(): Started thread", mThreadName.c_str());
382 
383         while (true) {
384             std::unique_lock l(mCondVarMutex);
385             if (mStopRequested) {
386                 return;
387             }
388             mCondVar.wait(l, [&] { return (mRbReadPtr != mRbWritePtr) || mStopRequested; });
389 
390             while (mRbReadPtr != mRbWritePtr && !mStopRequested) {
391                 ALOGV("%s::run(): new callbacks, rb idx read=%zu, write=%zu",
392                       mThreadName.c_str(),
393                       mRbReadPtr.load(),
394                       mRbWritePtr.load());
395                 auto callback = mCallback.promote();
396                 if (callback == nullptr) {
397                     ALOGW("%s::run(): MelCallback is null, quitting MelWorker",
398                           mThreadName.c_str());
399                     return;
400                 }
401 
402                 MelCallbackData data = mCallbackRingBuffer[mRbReadPtr];
403                 if (data.mMel != 0.f) {
404                     callback->onMomentaryExposure(data.mMel, data.mPort);
405                 } else if (data.mMelsSize != 0) {
406                     callback->onNewMelValues(data.mMels, 0, data.mMelsSize,
407                                              data.mPort, /*attenuated=*/true);
408                 } else {
409                     ALOGE("%s::run(): Invalid MEL data. Skipping callback", mThreadName.c_str());
410                 }
411                 incRingBufferIndex(mRbReadPtr);
412             }
413         }
414     });
415 }
416 
stop()417 void MelProcessor::MelWorker::stop() {
418     bool oldValue;
419     {
420         std::lock_guard l(mCondVarMutex);
421         oldValue = mStopRequested;
422         mStopRequested = true;
423     }
424     if (!oldValue) {
425         mCondVar.notify_one();
426         mThread.join();
427     }
428 }
429 
momentaryExposure(float mel,audio_port_handle_t port)430 void MelProcessor::MelWorker::momentaryExposure(float mel, audio_port_handle_t port) {
431     ALOGV("%s", __func__);
432 
433     if (ringBufferIsFull()) {
434         ALOGW("%s: cannot add momentary exposure for port %d, MelWorker buffer is full", __func__,
435               port);
436         return;
437     }
438 
439     // worker is thread-safe, no lock since there is only one writer and we take into account
440     // spurious wake-ups
441     mCallbackRingBuffer[mRbWritePtr].mMel = mel;
442     mCallbackRingBuffer[mRbWritePtr].mMelsSize = 0;
443     mCallbackRingBuffer[mRbWritePtr].mPort = port;
444 
445     incRingBufferIndex(mRbWritePtr);
446 }
447 
newMelValues(const std::vector<float> & mels,size_t melsSize,audio_port_handle_t port)448 void MelProcessor::MelWorker::newMelValues(const std::vector<float>& mels,
449                                            size_t melsSize,
450                                            audio_port_handle_t port) {
451     ALOGV("%s", __func__);
452 
453     if (ringBufferIsFull()) {
454         ALOGW("%s: cannot add %zu mel values for port %d, MelWorker buffer is full", __func__,
455               melsSize, port);
456         return;
457     }
458 
459     // worker is thread-safe, no lock since there is only one writer and we take into account
460     // spurious wake-ups
461     std::copy_n(std::begin(mels), melsSize, mCallbackRingBuffer[mRbWritePtr].mMels.begin());
462     mCallbackRingBuffer[mRbWritePtr].mMelsSize = melsSize;
463     mCallbackRingBuffer[mRbWritePtr].mMel = 0.f;
464     mCallbackRingBuffer[mRbWritePtr].mPort = port;
465 
466     incRingBufferIndex(mRbWritePtr);
467 }
468 
ringBufferIsFull() const469 bool MelProcessor::MelWorker::ringBufferIsFull() const {
470     size_t curIdx = mRbWritePtr.load();
471     size_t nextIdx = curIdx >= kRingBufferSize - 1 ? 0 : curIdx + 1;
472 
473     return nextIdx == mRbReadPtr;
474 }
475 
incRingBufferIndex(std::atomic_size_t & idx)476 void MelProcessor::MelWorker::incRingBufferIndex(std::atomic_size_t& idx) {
477     size_t nextIdx;
478     size_t expected;
479     do {
480         expected = idx.load();
481         nextIdx = expected >= kRingBufferSize - 1 ? 0 : expected + 1;
482     } while (!idx.compare_exchange_strong(expected, nextIdx));
483 }
484 
485 }   // namespace android
486