xref: /aosp_15_r20/frameworks/av/services/audiopolicy/service/Spatializer.cpp (revision ec779b8e0859a360c3d303172224686826e6e0e1)
1 /*
2 **
3 ** Copyright 2021, The Android Open Source Project
4 **
5 ** Licensed under the Apache License, Version 2.0 (the "License");
6 ** you may not use this file except in compliance with the License.
7 ** You may obtain a copy of the License at
8 **
9 **     http://www.apache.org/licenses/LICENSE-2.0
10 **
11 ** Unless required by applicable law or agreed to in writing, software
12 ** distributed under the License is distributed on an "AS IS" BASIS,
13 ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 ** See the License for the specific language governing permissions and
15 ** limitations under the License.
16 */
17 
18 #include <string>
19 #define LOG_TAG "Spatializer"
20 //#define LOG_NDEBUG 0
21 #include <utils/Log.h>
22 
23 #include <algorithm>
24 #include <inttypes.h>
25 #include <limits.h>
26 #include <stdint.h>
27 #include <sys/types.h>
28 
29 #include <android/content/AttributionSourceState.h>
30 #include <android/sysprop/BluetoothProperties.sysprop.h>
31 #include <audio_utils/fixedfft.h>
32 #include <cutils/bitops.h>
33 #include <hardware/sensors.h>
34 #include <media/stagefright/foundation/AHandler.h>
35 #include <media/stagefright/foundation/AMessage.h>
36 #include <media/MediaMetricsItem.h>
37 #include <media/QuaternionUtil.h>
38 #include <media/ShmemCompat.h>
39 #include <mediautils/SchedulingPolicyService.h>
40 #include <mediautils/ServiceUtilities.h>
41 #include <utils/Thread.h>
42 
43 #include "Spatializer.h"
44 #include "SpatializerHelper.h"
45 
46 namespace android {
47 
48 using aidl_utils::binderStatusFromStatusT;
49 using aidl_utils::statusTFromBinderStatus;
50 using android::content::AttributionSourceState;
51 using binder::Status;
52 using internal::ToString;
53 using media::HeadTrackingMode;
54 using media::Pose3f;
55 using media::SensorPoseProvider;
56 using media::audio::common::HeadTracking;
57 using media::audio::common::Spatialization;
58 
59 using namespace std::chrono_literals;
60 
61 #define VALUE_OR_RETURN_BINDER_STATUS(x) \
62     ({ auto _tmp = (x); \
63        if (!_tmp.ok()) return aidl_utils::binderStatusFromStatusT(_tmp.error()); \
64        std::move(_tmp.value()); })
65 
getMaxChannelMask(const std::vector<audio_channel_mask_t> & masks,size_t channelLimit=SIZE_MAX)66 static audio_channel_mask_t getMaxChannelMask(
67         const std::vector<audio_channel_mask_t>& masks, size_t channelLimit = SIZE_MAX) {
68     uint32_t maxCount = 0;
69     audio_channel_mask_t maxMask = AUDIO_CHANNEL_NONE;
70     for (auto mask : masks) {
71         const size_t count = audio_channel_count_from_out_mask(mask);
72         if (count > channelLimit) continue;  // ignore masks greater than channelLimit
73         if (count > maxCount) {
74             maxMask = mask;
75             maxCount = count;
76         }
77     }
78     return maxMask;
79 }
80 
recordFromTranslationRotationVector(const std::vector<float> & trVector)81 static std::vector<float> recordFromTranslationRotationVector(
82         const std::vector<float>& trVector) {
83     auto headToStageOpt = Pose3f::fromVector(trVector);
84     if (!headToStageOpt) return {};
85 
86     const auto stageToHead = headToStageOpt.value().inverse();
87     const auto stageToHeadTranslation = stageToHead.translation();
88     constexpr float RAD_TO_DEGREE = 180.f / M_PI;
89     std::vector<float> record{
90         stageToHeadTranslation[0], stageToHeadTranslation[1], stageToHeadTranslation[2],
91         0.f, 0.f, 0.f};
92     media::quaternionToAngles(stageToHead.rotation(), &record[3], &record[4], &record[5]);
93     record[3] *= RAD_TO_DEGREE;
94     record[4] *= RAD_TO_DEGREE;
95     record[5] *= RAD_TO_DEGREE;
96     return record;
97 }
98 
99 template<typename T>
safe_clamp(const T & value,const T & low,const T & high)100 static constexpr const T& safe_clamp(const T& value, const T& low, const T& high) {
101     if constexpr (std::is_floating_point_v<T>) {
102         return value != value /* constexpr isnan */
103                 ? low : std::clamp(value, low, high);
104     } else /* constexpr */ {
105         return std::clamp(value, low, high);
106     }
107 }
108 
109 // ---------------------------------------------------------------------------
110 
111 class Spatializer::EngineCallbackHandler : public AHandler {
112 public:
EngineCallbackHandler(wp<Spatializer> spatializer)113     EngineCallbackHandler(wp<Spatializer> spatializer)
114             : mSpatializer(spatializer) {
115     }
116 
117     enum {
118         // Device state callbacks
119         kWhatOnFramesProcessed,    // AudioEffect::EVENT_FRAMES_PROCESSED
120         kWhatOnHeadToStagePose,    // SpatializerPoseController::Listener::onHeadToStagePose
121         kWhatOnActualModeChange,   // SpatializerPoseController::Listener::onActualModeChange
122         kWhatOnLatencyModesChanged, // Spatializer::onSupportedLatencyModesChanged
123     };
124     static constexpr const char *kNumFramesKey = "numFrames";
125     static constexpr const char *kModeKey = "mode";
126     static constexpr const char *kTranslation0Key = "translation0";
127     static constexpr const char *kTranslation1Key = "translation1";
128     static constexpr const char *kTranslation2Key = "translation2";
129     static constexpr const char *kRotation0Key = "rotation0";
130     static constexpr const char *kRotation1Key = "rotation1";
131     static constexpr const char *kRotation2Key = "rotation2";
132     static constexpr const char *kLatencyModesKey = "latencyModes";
133 
134     class LatencyModes : public RefBase {
135     public:
LatencyModes(audio_io_handle_t output,const std::vector<audio_latency_mode_t> & latencyModes)136         LatencyModes(audio_io_handle_t output,
137                 const std::vector<audio_latency_mode_t>& latencyModes)
138             : mOutput(output), mLatencyModes(latencyModes) {}
139         ~LatencyModes() = default;
140 
141         audio_io_handle_t mOutput;
142         std::vector<audio_latency_mode_t> mLatencyModes;
143     };
144 
onMessageReceived(const sp<AMessage> & msg)145     void onMessageReceived(const sp<AMessage> &msg) override {
146         // No ALooper method to get the tid so update
147         // Spatializer priority on the first message received.
148         std::call_once(mPrioritySetFlag, [](){
149             const pid_t pid = getpid();
150             const pid_t tid = gettid();
151             (void)requestSpatializerPriority(pid, tid);
152         });
153 
154         sp<Spatializer> spatializer = mSpatializer.promote();
155         if (spatializer == nullptr) {
156             ALOGW("%s: Cannot promote spatializer", __func__);
157             return;
158         }
159         switch (msg->what()) {
160             case kWhatOnFramesProcessed: {
161                 int numFrames;
162                 if (!msg->findInt32(kNumFramesKey, &numFrames)) {
163                     ALOGE("%s: Cannot find num frames!", __func__);
164                     return;
165                 }
166                 if (numFrames > 0) {
167                     spatializer->calculateHeadPose();
168                 }
169                 } break;
170             case kWhatOnHeadToStagePose: {
171                 std::vector<float> headToStage(sHeadPoseKeys.size());
172                 for (size_t i = 0 ; i < sHeadPoseKeys.size(); i++) {
173                     if (!msg->findFloat(sHeadPoseKeys[i], &headToStage[i])) {
174                         ALOGE("%s: Cannot find kTranslation0Key!", __func__);
175                         return;
176                     }
177                 }
178                 spatializer->onHeadToStagePoseMsg(headToStage);
179                 } break;
180             case kWhatOnActualModeChange: {
181                 int mode;
182                 if (!msg->findInt32(kModeKey, &mode)) {
183                     ALOGE("%s: Cannot find actualMode!", __func__);
184                     return;
185                 }
186                 spatializer->onActualModeChangeMsg(static_cast<HeadTrackingMode>(mode));
187                 } break;
188 
189             case kWhatOnLatencyModesChanged: {
190                 sp<RefBase> object;
191                 if (!msg->findObject(kLatencyModesKey, &object)) {
192                     ALOGE("%s: Cannot find latency modes!", __func__);
193                     return;
194                 }
195                 sp<LatencyModes> latencyModes = static_cast<LatencyModes*>(object.get());
196                 spatializer->onSupportedLatencyModesChangedMsg(
197                     latencyModes->mOutput, std::move(latencyModes->mLatencyModes));
198                 } break;
199 
200             default:
201                 LOG_ALWAYS_FATAL("Invalid callback message %d", msg->what());
202         }
203     }
204 private:
205     wp<Spatializer> mSpatializer;
206     std::once_flag mPrioritySetFlag;
207 };
208 
209 const std::vector<const char *> Spatializer::sHeadPoseKeys = {
210     Spatializer::EngineCallbackHandler::kTranslation0Key,
211     Spatializer::EngineCallbackHandler::kTranslation1Key,
212     Spatializer::EngineCallbackHandler::kTranslation2Key,
213     Spatializer::EngineCallbackHandler::kRotation0Key,
214     Spatializer::EngineCallbackHandler::kRotation1Key,
215     Spatializer::EngineCallbackHandler::kRotation2Key,
216 };
217 
218 // Mapping table between strings read form property bluetooth.core.le.dsa_transport_preference
219 // and low latency modes emums.
220 //TODO b/273373363: use AIDL enum when available
221 const std::map<std::string, audio_latency_mode_t> Spatializer::sStringToLatencyModeMap = {
222     {"le-acl", AUDIO_LATENCY_MODE_LOW},
223     {"iso-sw", AUDIO_LATENCY_MODE_DYNAMIC_SPATIAL_AUDIO_SOFTWARE},
224     {"iso-hw", AUDIO_LATENCY_MODE_DYNAMIC_SPATIAL_AUDIO_HARDWARE},
225 };
226 
loadOrderedLowLatencyModes()227 void Spatializer::loadOrderedLowLatencyModes() {
228     if (!com::android::media::audio::dsa_over_bt_le_audio()) {
229         return;
230     }
231     auto latencyModesStrs = android::sysprop::BluetoothProperties::dsa_transport_preference();
232     audio_utils::lock_guard lock(mMutex);
233     // First load preferred low latency modes ordered from the property
234     for (auto str : latencyModesStrs) {
235         if (!str.has_value()) continue;
236         if (auto it = sStringToLatencyModeMap.find(str.value());
237                 it != sStringToLatencyModeMap.end()) {
238             mOrderedLowLatencyModes.push_back(it->second);
239         }
240     }
241     // Then add unlisted latency modes at the end of the ordered list
242     for (auto it : sStringToLatencyModeMap) {
243         if (std::find(mOrderedLowLatencyModes.begin(), mOrderedLowLatencyModes.end(), it.second)
244                 == mOrderedLowLatencyModes.end()) {
245              mOrderedLowLatencyModes.push_back(it.second);
246         }
247     }
248 }
249 
250 // ---------------------------------------------------------------------------
create(SpatializerPolicyCallback * callback,const sp<EffectsFactoryHalInterface> & effectsFactoryHal)251 sp<Spatializer> Spatializer::create(SpatializerPolicyCallback* callback,
252                                     const sp<EffectsFactoryHalInterface>& effectsFactoryHal) {
253     sp<Spatializer> spatializer;
254 
255     if (effectsFactoryHal == nullptr) {
256         ALOGW("%s failed to create effect factory interface", __func__);
257         return spatializer;
258     }
259 
260     std::vector<effect_descriptor_t> descriptors;
261     status_t status = effectsFactoryHal->getDescriptors(FX_IID_SPATIALIZER, &descriptors);
262     if (status != NO_ERROR) {
263         ALOGW("%s failed to get spatializer descriptor, error %d", __func__, status);
264         return spatializer;
265     }
266     ALOG_ASSERT(!descriptors.empty(),
267             "%s getDescriptors() returned no error but empty list", __func__);
268 
269     // TODO: get supported spatialization modes from FX engine or descriptor
270     sp<EffectHalInterface> effect;
271     status = effectsFactoryHal->createEffect(&descriptors[0].uuid, AUDIO_SESSION_OUTPUT_STAGE,
272             AUDIO_IO_HANDLE_NONE, AUDIO_PORT_HANDLE_NONE, &effect);
273     ALOGI("%s FX create status %d effect %p", __func__, status, effect.get());
274 
275     if (status == NO_ERROR && effect != nullptr) {
276         spatializer = new Spatializer(descriptors[0], callback);
277         if (spatializer->loadEngineConfiguration(effect) != NO_ERROR) {
278             spatializer.clear();
279             ALOGW("%s loadEngine error: %d  effect %p", __func__, status, effect.get());
280         } else {
281             spatializer->loadOrderedLowLatencyModes();
282             spatializer->mLocalLog.log("%s with effect Id %p", __func__, effect.get());
283         }
284     }
285 
286     return spatializer;
287 }
288 
Spatializer(effect_descriptor_t engineDescriptor,SpatializerPolicyCallback * callback)289 Spatializer::Spatializer(effect_descriptor_t engineDescriptor, SpatializerPolicyCallback* callback)
290     : mEngineDescriptor(engineDescriptor),
291       mPolicyCallback(callback) {
292     ALOGV("%s", __func__);
293     setMinSchedulerPolicy(SCHED_NORMAL, ANDROID_PRIORITY_AUDIO);
294     setInheritRt(true);
295 }
296 
onFirstRef()297 void Spatializer::onFirstRef() {
298     mLooper = new ALooper;
299     mLooper->setName("Spatializer-looper");
300     mLooper->start(
301             /*runOnCallingThread*/false,
302             /*canCallJava*/       false,
303             PRIORITY_URGENT_AUDIO);
304 
305     mHandler = new EngineCallbackHandler(this);
306     mLooper->registerHandler(mHandler);
307 }
308 
~Spatializer()309 Spatializer::~Spatializer() {
310     ALOGV("%s", __func__);
311     if (mLooper != nullptr) {
312         mLooper->stop();
313         mLooper->unregisterHandler(mHandler->id());
314     }
315     mLooper.clear();
316     mHandler.clear();
317 }
318 
channelMaskVectorToString(const std::vector<audio_channel_mask_t> & masks)319 static std::string channelMaskVectorToString(
320         const std::vector<audio_channel_mask_t>& masks) {
321     std::stringstream ss;
322     for (const auto &mask : masks) {
323         if (ss.tellp() != 0) ss << "|";
324         ss << mask;
325     }
326     return ss.str();
327 }
328 
loadEngineConfiguration(sp<EffectHalInterface> effect)329 status_t Spatializer::loadEngineConfiguration(sp<EffectHalInterface> effect) {
330     ALOGV("%s", __func__);
331 
332     std::vector<bool> supportsHeadTracking;
333     status_t status = getHalParameter<false>(effect, SPATIALIZER_PARAM_HEADTRACKING_SUPPORTED,
334                                          &supportsHeadTracking);
335     if (status != NO_ERROR) {
336         ALOGW("%s: cannot get SPATIALIZER_PARAM_HEADTRACKING_SUPPORTED", __func__);
337         return status;
338     }
339     mSupportsHeadTracking = supportsHeadTracking[0];
340 
341     std::vector<Spatialization::Level> spatializationLevels;
342     status = getHalParameter<true>(effect, SPATIALIZER_PARAM_SUPPORTED_LEVELS,
343             &spatializationLevels);
344     if (status != NO_ERROR) {
345         ALOGW("%s: cannot get SPATIALIZER_PARAM_SUPPORTED_LEVELS", __func__);
346         return status;
347     }
348     bool noneLevelFound = false;
349     bool activeLevelFound = false;
350     for (const auto spatializationLevel : spatializationLevels) {
351         if (!aidl_utils::isValidEnum(spatializationLevel)) {
352             ALOGW("%s: ignoring spatializationLevel:%s", __func__,
353                   ToString(spatializationLevel).c_str());
354             continue;
355         }
356         if (spatializationLevel == Spatialization::Level::NONE) {
357             noneLevelFound = true;
358         } else {
359             activeLevelFound = true;
360         }
361         // we don't detect duplicates.
362         mLevels.emplace_back(spatializationLevel);
363     }
364     if (!noneLevelFound || !activeLevelFound) {
365         ALOGW("%s: SPATIALIZER_PARAM_SUPPORTED_LEVELS must include NONE"
366                 " and another valid level",  __func__);
367         return BAD_VALUE;
368     }
369 
370     std::vector<Spatialization::Mode> spatializationModes;
371     status = getHalParameter<true>(effect, SPATIALIZER_PARAM_SUPPORTED_SPATIALIZATION_MODES,
372             &spatializationModes);
373     if (status != NO_ERROR) {
374         ALOGW("%s: cannot get SPATIALIZER_PARAM_SUPPORTED_SPATIALIZATION_MODES", __func__);
375         return status;
376     }
377 
378     for (const auto spatializationMode : spatializationModes) {
379         if (!aidl_utils::isValidEnum(spatializationMode)) {
380             ALOGW("%s: ignoring spatializationMode:%s", __func__,
381                   ToString(spatializationMode).c_str());
382             continue;
383         }
384         // we don't detect duplicates.
385         mSpatializationModes.emplace_back(spatializationMode);
386     }
387     if (mSpatializationModes.empty()) {
388         ALOGW("%s: SPATIALIZER_PARAM_SUPPORTED_SPATIALIZATION_MODES reports empty", __func__);
389         return BAD_VALUE;
390     }
391 
392     std::vector<audio_channel_mask_t> channelMasks;
393     status = getHalParameter<true>(effect, SPATIALIZER_PARAM_SUPPORTED_CHANNEL_MASKS,
394                                  &channelMasks);
395     if (status != NO_ERROR) {
396         ALOGW("%s: cannot get SPATIALIZER_PARAM_SUPPORTED_CHANNEL_MASKS", __func__);
397         return status;
398     }
399     for (const auto channelMask : channelMasks) {
400         const bool channel_mask_spatialized =
401                 SpatializerHelper::isStereoSpatializationFeatureEnabled()
402                         ? audio_channel_mask_contains_stereo(channelMask)
403                         : audio_is_channel_mask_spatialized(channelMask);
404         if (!channel_mask_spatialized) {
405             ALOGW("%s: ignoring channelMask:%#x", __func__, channelMask);
406             continue;
407         }
408         // we don't detect duplicates.
409         mChannelMasks.emplace_back(channelMask);
410     }
411     if (mChannelMasks.empty()) {
412         ALOGW("%s: SPATIALIZER_PARAM_SUPPORTED_CHANNEL_MASKS reports empty", __func__);
413         return BAD_VALUE;
414     }
415 
416     std::vector<audio_channel_mask_t> spatializedChannelMasks;
417     status = getHalParameter<true>(effect, SPATIALIZER_PARAM_SPATIALIZED_CHANNEL_MASKS,
418                                    &spatializedChannelMasks);
419     if (status != NO_ERROR) {
420         ALOGW("%s: cannot get SPATIALIZER_PARAM_SPATIALIZED_CHANNEL_MASKS", __func__);
421         // do not return an error yet as spatializer implementations may not have been
422         // updated yet to support this parameter
423     }
424     if (spatializedChannelMasks.empty()) {
425         ALOGW("%s: SPATIALIZER_PARAM_SPATIALIZED_CHANNEL_MASKS reports empty", __func__);
426         // do not return an error yet as spatializer implementations may not have been
427         // updated yet to support this parameter
428     }
429     for (const audio_channel_mask_t spatializedMask : spatializedChannelMasks) {
430         // spatialized masks must be contained in the supported input masks
431         if (std::find(mChannelMasks.begin(), mChannelMasks.end(), spatializedMask)
432                 != mChannelMasks.end()) {
433             mSpatializedChannelMasks.emplace_back(spatializedMask);
434         } else {
435             ALOGE("%s: spatialized mask %#x not in list reported by PARAM_SUPPORTED_CHANNEL_MASKS",
436                   __func__, spatializedMask);
437             return BAD_VALUE;
438         }
439     }
440 
441     if (com::android::media::audio::dsa_over_bt_le_audio()
442             && mSupportsHeadTracking) {
443         mHeadtrackingConnectionMode = HeadTracking::ConnectionMode::FRAMEWORK_PROCESSED;
444         std::vector<HeadTracking::ConnectionMode> headtrackingConnectionModes;
445         status = getHalParameter<true>(effect, SPATIALIZER_PARAM_SUPPORTED_HEADTRACKING_CONNECTION,
446                 &headtrackingConnectionModes);
447         if (status == NO_ERROR) {
448             for (const auto htConnectionMode : headtrackingConnectionModes) {
449                 if (htConnectionMode < HeadTracking::ConnectionMode::FRAMEWORK_PROCESSED ||
450                     htConnectionMode > HeadTracking::ConnectionMode::DIRECT_TO_SENSOR_TUNNEL) {
451                     ALOGW("%s: ignoring HT connection mode:%s", __func__,
452                           ToString(htConnectionMode).c_str());
453                     continue;
454                 }
455                 mSupportedHeadtrackingConnectionModes.insert(htConnectionMode);
456             }
457             ALOGW_IF(mSupportedHeadtrackingConnectionModes.find(
458                     HeadTracking::ConnectionMode::FRAMEWORK_PROCESSED) ==
459                         mSupportedHeadtrackingConnectionModes.end(),
460                     "%s: Headtracking FRAMEWORK_PROCESSED not reported", __func__);
461         }
462     }
463 
464     // Currently we expose only RELATIVE_WORLD.
465     // This is a limitation of the head tracking library based on a UX choice.
466     mHeadTrackingModes.push_back(HeadTracking::Mode::DISABLED);
467     if (mSupportsHeadTracking) {
468         mHeadTrackingModes.push_back(HeadTracking::Mode::RELATIVE_WORLD);
469     }
470     mediametrics::LogItem(mMetricsId)
471         .set(AMEDIAMETRICS_PROP_EVENT, AMEDIAMETRICS_PROP_EVENT_VALUE_CREATE)
472         .set(AMEDIAMETRICS_PROP_CHANNELMASKS, channelMaskVectorToString(mChannelMasks))
473         .set(AMEDIAMETRICS_PROP_LEVELS, aidl_utils::enumsToString(mLevels))
474         .set(AMEDIAMETRICS_PROP_MODES, aidl_utils::enumsToString(mSpatializationModes))
475         .set(AMEDIAMETRICS_PROP_HEADTRACKINGMODES, aidl_utils::enumsToString(mHeadTrackingModes))
476         .set(AMEDIAMETRICS_PROP_STATUS, (int32_t)status)
477         .record();
478     return NO_ERROR;
479 }
480 
481 /* static */
sendEmptyCreateSpatializerMetricWithStatus(status_t status)482 void Spatializer::sendEmptyCreateSpatializerMetricWithStatus(status_t status) {
483     mediametrics::LogItem(kDefaultMetricsId)
484         .set(AMEDIAMETRICS_PROP_EVENT, AMEDIAMETRICS_PROP_EVENT_VALUE_CREATE)
485         .set(AMEDIAMETRICS_PROP_CHANNELMASKS, "")
486         .set(AMEDIAMETRICS_PROP_LEVELS, "")
487         .set(AMEDIAMETRICS_PROP_MODES, "")
488         .set(AMEDIAMETRICS_PROP_HEADTRACKINGMODES, "")
489         .set(AMEDIAMETRICS_PROP_STATUS, (int32_t)status)
490         .record();
491 }
492 
493 /** Gets the channel mask, sampling rate and format set for the spatializer input. */
getAudioInConfig() const494 audio_config_base_t Spatializer::getAudioInConfig() const {
495     audio_utils::lock_guard lock(mMutex);
496     audio_config_base_t config = AUDIO_CONFIG_BASE_INITIALIZER;
497     // For now use highest supported channel count
498     config.channel_mask = getMaxChannelMask(mChannelMasks, FCC_LIMIT);
499     return config;
500 }
501 
getSpatializedChannelMasks(std::vector<int> * masks)502 Status Spatializer::getSpatializedChannelMasks(std::vector<int>* masks) {
503         audio_utils::lock_guard lock(mMutex);
504         ALOGV("%s", __func__);
505         if (masks == nullptr) {
506             return binderStatusFromStatusT(BAD_VALUE);
507         }
508         masks->insert(masks->end(), mSpatializedChannelMasks.begin(),
509                       mSpatializedChannelMasks.end());
510         return Status::ok();
511     }
512 
registerCallback(const sp<media::INativeSpatializerCallback> & callback)513 status_t Spatializer::registerCallback(
514         const sp<media::INativeSpatializerCallback>& callback) {
515     audio_utils::lock_guard lock(mMutex);
516     if (callback == nullptr) {
517         return BAD_VALUE;
518     }
519 
520     if (mSpatializerCallback != nullptr) {
521         if (IInterface::asBinder(callback) == IInterface::asBinder(mSpatializerCallback)) {
522             ALOGW("%s: Registering callback %p again",
523                 __func__, mSpatializerCallback.get());
524             return NO_ERROR;
525         }
526         ALOGE("%s: Already one client registered with callback %p",
527             __func__, mSpatializerCallback.get());
528         return INVALID_OPERATION;
529     }
530 
531     sp<IBinder> binder = IInterface::asBinder(callback);
532     status_t status = binder->linkToDeath(this);
533     if (status == NO_ERROR) {
534         mSpatializerCallback = callback;
535     }
536     ALOGV("%s status %d", __func__, status);
537     return status;
538 }
539 
540 // IBinder::DeathRecipient
binderDied(__unused const wp<IBinder> & who)541 void Spatializer::binderDied(__unused const wp<IBinder> &who) {
542     {
543         audio_utils::lock_guard lock(mMutex);
544         mLevel = Spatialization::Level::NONE;
545         mSpatializerCallback.clear();
546     }
547     ALOGV("%s", __func__);
548     mPolicyCallback->onCheckSpatializer();
549 }
550 
551 // ISpatializer
getSupportedLevels(std::vector<Spatialization::Level> * levels)552 Status Spatializer::getSupportedLevels(std::vector<Spatialization::Level> *levels) {
553     ALOGV("%s", __func__);
554     if (levels == nullptr) {
555         return binderStatusFromStatusT(BAD_VALUE);
556     }
557     // Spatialization::Level::NONE is already required from the effect or we don't load it.
558     levels->insert(levels->end(), mLevels.begin(), mLevels.end());
559     return Status::ok();
560 }
561 
setLevel(Spatialization::Level level)562 Status Spatializer::setLevel(Spatialization::Level level) {
563     ALOGV("%s level %s", __func__,  ToString(level).c_str());
564     mLocalLog.log("%s with %s", __func__, ToString(level).c_str());
565     if (level != Spatialization::Level::NONE
566             && std::find(mLevels.begin(), mLevels.end(), level) == mLevels.end()) {
567         return binderStatusFromStatusT(BAD_VALUE);
568     }
569     sp<media::INativeSpatializerCallback> callback;
570     bool levelChanged = false;
571     {
572         audio_utils::lock_guard lock(mMutex);
573         levelChanged = mLevel != level;
574         mLevel = level;
575         callback = mSpatializerCallback;
576 
577         if (levelChanged && mEngine != nullptr) {
578             checkEngineState_l();
579         }
580         checkSensorsState_l();
581     }
582 
583     if (levelChanged) {
584         mPolicyCallback->onCheckSpatializer();
585         if (callback != nullptr) {
586             callback->onLevelChanged(level);
587         }
588     }
589     return Status::ok();
590 }
591 
getLevel(Spatialization::Level * level)592 Status Spatializer::getLevel(Spatialization::Level *level) {
593     if (level == nullptr) {
594         return binderStatusFromStatusT(BAD_VALUE);
595     }
596     audio_utils::lock_guard lock(mMutex);
597     *level = mLevel;
598     ALOGV("%s level %s", __func__, ToString(*level).c_str());
599     return Status::ok();
600 }
601 
isHeadTrackingSupported(bool * supports)602 Status Spatializer::isHeadTrackingSupported(bool *supports) {
603     ALOGV("%s mSupportsHeadTracking %s", __func__, ToString(mSupportsHeadTracking).c_str());
604     if (supports == nullptr) {
605         return binderStatusFromStatusT(BAD_VALUE);
606     }
607     audio_utils::lock_guard lock(mMutex);
608     *supports = mSupportsHeadTracking;
609     return Status::ok();
610 }
611 
getSupportedHeadTrackingModes(std::vector<HeadTracking::Mode> * modes)612 Status Spatializer::getSupportedHeadTrackingModes(
613         std::vector<HeadTracking::Mode>* modes) {
614     audio_utils::lock_guard lock(mMutex);
615     ALOGV("%s", __func__);
616     if (modes == nullptr) {
617         return binderStatusFromStatusT(BAD_VALUE);
618     }
619     modes->insert(modes->end(), mHeadTrackingModes.begin(), mHeadTrackingModes.end());
620     return Status::ok();
621 }
622 
setDesiredHeadTrackingMode(HeadTracking::Mode mode)623 Status Spatializer::setDesiredHeadTrackingMode(HeadTracking::Mode mode) {
624     ALOGV("%s mode %s", __func__, ToString(mode).c_str());
625 
626     if (!mSupportsHeadTracking) {
627         return binderStatusFromStatusT(INVALID_OPERATION);
628     }
629     mLocalLog.log("%s with %s", __func__, ToString(mode).c_str());
630     audio_utils::lock_guard lock(mMutex);
631     switch (mode) {
632         case HeadTracking::Mode::OTHER:
633             return binderStatusFromStatusT(BAD_VALUE);
634         case HeadTracking::Mode::DISABLED:
635             mDesiredHeadTrackingMode = HeadTrackingMode::STATIC;
636             break;
637         case HeadTracking::Mode::RELATIVE_WORLD:
638             mDesiredHeadTrackingMode = HeadTrackingMode::WORLD_RELATIVE;
639             break;
640         case HeadTracking::Mode::RELATIVE_SCREEN:
641             mDesiredHeadTrackingMode = HeadTrackingMode::SCREEN_RELATIVE;
642             break;
643     }
644 
645     checkPoseController_l();
646     checkSensorsState_l();
647 
648     return Status::ok();
649 }
650 
getActualHeadTrackingMode(HeadTracking::Mode * mode)651 Status Spatializer::getActualHeadTrackingMode(HeadTracking::Mode *mode) {
652     if (mode == nullptr) {
653         return binderStatusFromStatusT(BAD_VALUE);
654     }
655     audio_utils::lock_guard lock(mMutex);
656     *mode = mActualHeadTrackingMode;
657     ALOGV("%s mode %d", __func__, (int)*mode);
658     return Status::ok();
659 }
660 
recenterHeadTracker()661 Status Spatializer::recenterHeadTracker() {
662     if (!mSupportsHeadTracking) {
663         return binderStatusFromStatusT(INVALID_OPERATION);
664     }
665     audio_utils::lock_guard lock(mMutex);
666     if (mPoseController != nullptr) {
667         mPoseController->recenter();
668     }
669     return Status::ok();
670 }
671 
setGlobalTransform(const std::vector<float> & screenToStage)672 Status Spatializer::setGlobalTransform(const std::vector<float>& screenToStage) {
673     ALOGV("%s", __func__);
674     if (!mSupportsHeadTracking) {
675         return binderStatusFromStatusT(INVALID_OPERATION);
676     }
677     std::optional<Pose3f> maybePose = Pose3f::fromVector(screenToStage);
678     if (!maybePose.has_value()) {
679         ALOGW("Invalid screenToStage vector.");
680         return binderStatusFromStatusT(BAD_VALUE);
681     }
682     audio_utils::lock_guard lock(mMutex);
683     if (mPoseController != nullptr) {
684         mLocalLog.log("%s with screenToStage %s", __func__,
685                 media::VectorRecorder::toString<float>(screenToStage).c_str());
686         mPoseController->setScreenToStagePose(maybePose.value());
687     }
688     return Status::ok();
689 }
690 
release()691 Status Spatializer::release() {
692     ALOGV("%s", __func__);
693     bool levelChanged = false;
694     {
695         audio_utils::lock_guard lock(mMutex);
696         if (mSpatializerCallback == nullptr) {
697             return binderStatusFromStatusT(INVALID_OPERATION);
698         }
699 
700         sp<IBinder> binder = IInterface::asBinder(mSpatializerCallback);
701         binder->unlinkToDeath(this);
702         mSpatializerCallback.clear();
703 
704         levelChanged = mLevel != Spatialization::Level::NONE;
705         mLevel = Spatialization::Level::NONE;
706     }
707 
708     if (levelChanged) {
709         mPolicyCallback->onCheckSpatializer();
710     }
711     return Status::ok();
712 }
713 
setHeadSensor(int sensorHandle)714 Status Spatializer::setHeadSensor(int sensorHandle) {
715     ALOGV("%s sensorHandle %d", __func__, sensorHandle);
716     if (!mSupportsHeadTracking) {
717         return binderStatusFromStatusT(INVALID_OPERATION);
718     }
719     audio_utils::lock_guard lock(mMutex);
720     if (mHeadSensor != sensorHandle) {
721         mLocalLog.log("%s with 0x%08x", __func__, sensorHandle);
722         mHeadSensor = sensorHandle;
723         checkPoseController_l();
724         checkSensorsState_l();
725     }
726     return Status::ok();
727 }
728 
setScreenSensor(int sensorHandle)729 Status Spatializer::setScreenSensor(int sensorHandle) {
730     ALOGV("%s sensorHandle %d", __func__, sensorHandle);
731     if (!mSupportsHeadTracking) {
732         return binderStatusFromStatusT(INVALID_OPERATION);
733     }
734     audio_utils::lock_guard lock(mMutex);
735     if (mScreenSensor != sensorHandle) {
736         mLocalLog.log("%s with 0x%08x", __func__, sensorHandle);
737         mScreenSensor = sensorHandle;
738         // TODO: consider a new method setHeadAndScreenSensor()
739         // because we generally set both at the same time.
740         // This will avoid duplicated work and recentering.
741         checkSensorsState_l();
742     }
743     return Status::ok();
744 }
745 
setDisplayOrientation(float physicalToLogicalAngle)746 Status Spatializer::setDisplayOrientation(float physicalToLogicalAngle) {
747     ALOGV("%s physicalToLogicalAngle %f", __func__, physicalToLogicalAngle);
748     mLocalLog.log("%s with %f", __func__, physicalToLogicalAngle);
749     const float angle = safe_clamp(physicalToLogicalAngle, 0.f, (float)(2. * M_PI));
750     // It is possible due to numerical inaccuracies to exceed the boundaries of 0 to 2 * M_PI.
751     ALOGI_IF(angle != physicalToLogicalAngle,
752             "%s: clamping %f to %f", __func__, physicalToLogicalAngle, angle);
753     audio_utils::lock_guard lock(mMutex);
754     mDisplayOrientation = angle;
755     if (mPoseController != nullptr) {
756         // This turns on the rate-limiter.
757         mPoseController->setDisplayOrientation(angle);
758     }
759     if (mEngine != nullptr) {
760         setEffectParameter_l(
761             SPATIALIZER_PARAM_DISPLAY_ORIENTATION, std::vector<float>{angle});
762     }
763     return Status::ok();
764 }
765 
setHingeAngle(float hingeAngle)766 Status Spatializer::setHingeAngle(float hingeAngle) {
767     ALOGV("%s hingeAngle %f", __func__, hingeAngle);
768     mLocalLog.log("%s with %f", __func__, hingeAngle);
769     const float angle = safe_clamp(hingeAngle, 0.f, (float)(2. * M_PI));
770     // It is possible due to numerical inaccuracies to exceed the boundaries of 0 to 2 * M_PI.
771     ALOGI_IF(angle != hingeAngle,
772             "%s: clamping %f to %f", __func__, hingeAngle, angle);
773     audio_utils::lock_guard lock(mMutex);
774     mHingeAngle = angle;
775     if (mEngine != nullptr) {
776         setEffectParameter_l(SPATIALIZER_PARAM_HINGE_ANGLE, std::vector<float>{angle});
777     }
778     return Status::ok();
779 }
780 
setFoldState(bool folded)781 Status Spatializer::setFoldState(bool folded) {
782     ALOGV("%s foldState %d", __func__, (int)folded);
783     mLocalLog.log("%s with %d", __func__, (int)folded);
784     audio_utils::lock_guard lock(mMutex);
785     mFoldedState = folded;
786     if (mEngine != nullptr) {
787         // we don't suppress multiple calls with the same folded state - that's
788         // done at the caller.
789         setEffectParameter_l(SPATIALIZER_PARAM_FOLD_STATE, std::vector<uint8_t>{mFoldedState});
790     }
791     return Status::ok();
792 }
793 
getSupportedModes(std::vector<Spatialization::Mode> * modes)794 Status Spatializer::getSupportedModes(std::vector<Spatialization::Mode> *modes) {
795     ALOGV("%s", __func__);
796     if (modes == nullptr) {
797         return binderStatusFromStatusT(BAD_VALUE);
798     }
799     *modes = mSpatializationModes;
800     return Status::ok();
801 }
802 
registerHeadTrackingCallback(const sp<media::ISpatializerHeadTrackingCallback> & callback)803 Status Spatializer::registerHeadTrackingCallback(
804         const sp<media::ISpatializerHeadTrackingCallback>& callback) {
805     ALOGV("%s callback %p", __func__, callback.get());
806     audio_utils::lock_guard lock(mMutex);
807     if (!mSupportsHeadTracking) {
808         return binderStatusFromStatusT(INVALID_OPERATION);
809     }
810     mHeadTrackingCallback = callback;
811     return Status::ok();
812 }
813 
setParameter(int key,const std::vector<unsigned char> & value)814 Status Spatializer::setParameter(int key, const std::vector<unsigned char>& value) {
815     ALOGV("%s key %d", __func__, key);
816     audio_utils::lock_guard lock(mMutex);
817     status_t status = INVALID_OPERATION;
818     if (mEngine != nullptr) {
819         status = setEffectParameter_l(key, value);
820     }
821     return binderStatusFromStatusT(status);
822 }
823 
getParameter(int key,std::vector<unsigned char> * value)824 Status Spatializer::getParameter(int key, std::vector<unsigned char> *value) {
825     ALOGV("%s key %d value size %d", __func__, key,
826           (value != nullptr ? (int)value->size() : -1));
827     if (value == nullptr) {
828         return binderStatusFromStatusT(BAD_VALUE);
829     }
830     audio_utils::lock_guard lock(mMutex);
831     status_t status = INVALID_OPERATION;
832     if (mEngine != nullptr) {
833         ALOGV("%s key %d mEngine %p", __func__, key, mEngine.get());
834         status = getEffectParameter_l(key, value);
835     }
836     return binderStatusFromStatusT(status);
837 }
838 
getOutput(int * output)839 Status Spatializer::getOutput(int *output) {
840     ALOGV("%s", __func__);
841     if (output == nullptr) {
842         binderStatusFromStatusT(BAD_VALUE);
843     }
844     audio_utils::lock_guard lock(mMutex);
845     *output = VALUE_OR_RETURN_BINDER_STATUS(legacy2aidl_audio_io_handle_t_int32_t(mOutput));
846     ALOGV("%s got output %d", __func__, *output);
847     return Status::ok();
848 }
849 
850 // SpatializerPoseController::Listener
onHeadToStagePose(const Pose3f & headToStage)851 void Spatializer::onHeadToStagePose(const Pose3f& headToStage) {
852     ALOGV("%s", __func__);
853     LOG_ALWAYS_FATAL_IF(!mSupportsHeadTracking,
854             "onHeadToStagePose() called with no head tracking support!");
855 
856     auto vec = headToStage.toVector();
857     LOG_ALWAYS_FATAL_IF(vec.size() != sHeadPoseKeys.size(),
858             "%s invalid head to stage vector size %zu", __func__, vec.size());
859     sp<AMessage> msg =
860             new AMessage(EngineCallbackHandler::kWhatOnHeadToStagePose, mHandler);
861     for (size_t i = 0 ; i < sHeadPoseKeys.size(); i++) {
862         msg->setFloat(sHeadPoseKeys[i], vec[i]);
863     }
864     msg->post();
865 }
866 
resetEngineHeadPose_l()867 void Spatializer::resetEngineHeadPose_l() {
868     ALOGV("%s mEngine %p", __func__, mEngine.get());
869     if (mEngine == nullptr) {
870         return;
871     }
872     const std::vector<float> headToStage(6, 0.0);
873     setEffectParameter_l(SPATIALIZER_PARAM_HEAD_TO_STAGE, headToStage);
874     setEffectParameter_l(SPATIALIZER_PARAM_HEADTRACKING_MODE,
875             std::vector<HeadTracking::Mode>{HeadTracking::Mode::DISABLED});
876 }
877 
onHeadToStagePoseMsg(const std::vector<float> & headToStage)878 void Spatializer::onHeadToStagePoseMsg(const std::vector<float>& headToStage) {
879     ALOGV("%s", __func__);
880     sp<media::ISpatializerHeadTrackingCallback> callback;
881     {
882         audio_utils::lock_guard lock(mMutex);
883         callback = mHeadTrackingCallback;
884         if (mEngine != nullptr) {
885             setEffectParameter_l(SPATIALIZER_PARAM_HEAD_TO_STAGE, headToStage);
886             const auto record = recordFromTranslationRotationVector(headToStage);
887             mPoseRecorder.record(record);
888             mPoseDurableRecorder.record(record);
889         }
890     }
891 
892     if (callback != nullptr) {
893         callback->onHeadToSoundStagePoseUpdated(headToStage);
894     }
895 }
896 
onActualModeChange(HeadTrackingMode mode)897 void Spatializer::onActualModeChange(HeadTrackingMode mode) {
898     std::string modeStr = ToString(mode);
899     ALOGV("%s(%s)", __func__, modeStr.c_str());
900     sp<AMessage> msg = new AMessage(EngineCallbackHandler::kWhatOnActualModeChange, mHandler);
901     msg->setInt32(EngineCallbackHandler::kModeKey, static_cast<int>(mode));
902     msg->post();
903 }
904 
onActualModeChangeMsg(HeadTrackingMode mode)905 void Spatializer::onActualModeChangeMsg(HeadTrackingMode mode) {
906     ALOGV("%s(%s)", __func__, ToString(mode).c_str());
907     sp<media::ISpatializerHeadTrackingCallback> callback;
908     HeadTracking::Mode spatializerMode;
909     {
910         audio_utils::lock_guard lock(mMutex);
911         if (!mSupportsHeadTracking) {
912             spatializerMode = HeadTracking::Mode::DISABLED;
913         } else {
914             switch (mode) {
915                 case HeadTrackingMode::STATIC:
916                     spatializerMode = HeadTracking::Mode::DISABLED;
917                     break;
918                 case HeadTrackingMode::WORLD_RELATIVE:
919                     spatializerMode = HeadTracking::Mode::RELATIVE_WORLD;
920                     break;
921                 case HeadTrackingMode::SCREEN_RELATIVE:
922                     spatializerMode = HeadTracking::Mode::RELATIVE_SCREEN;
923                     break;
924                 default:
925                     LOG_ALWAYS_FATAL("Unknown mode: %s", ToString(mode).c_str());
926             }
927         }
928         mActualHeadTrackingMode = spatializerMode;
929         if (mEngine != nullptr) {
930             if (spatializerMode == HeadTracking::Mode::DISABLED) {
931                 resetEngineHeadPose_l();
932             } else {
933                 setEffectParameter_l(SPATIALIZER_PARAM_HEADTRACKING_MODE,
934                                      std::vector<HeadTracking::Mode>{spatializerMode});
935                 setEngineHeadtrackingConnectionMode_l();
936             }
937         }
938         callback = mHeadTrackingCallback;
939         mLocalLog.log("%s: updating mode to %s", __func__, ToString(mode).c_str());
940     }
941     if (callback != nullptr) {
942         callback->onHeadTrackingModeChanged(spatializerMode);
943     }
944 }
945 
setEngineHeadtrackingConnectionMode_l()946 void Spatializer::setEngineHeadtrackingConnectionMode_l() {
947     if (!com::android::media::audio::dsa_over_bt_le_audio()) {
948         return;
949     }
950     if (mActualHeadTrackingMode != HeadTracking::Mode::DISABLED
951             && !mSupportedHeadtrackingConnectionModes.empty()) {
952         setEffectParameter_l(SPATIALIZER_PARAM_HEADTRACKING_CONNECTION,
953                 static_cast<uint8_t>(mHeadtrackingConnectionMode),
954                 static_cast<uint32_t>(mHeadSensor));
955     }
956 }
957 
sortSupportedLatencyModes_l()958 void Spatializer::sortSupportedLatencyModes_l() {
959     if (!com::android::media::audio::dsa_over_bt_le_audio()) {
960         return;
961     }
962     std::sort(mSupportedLatencyModes.begin(), mSupportedLatencyModes.end(),
963             [this](audio_latency_mode_t x, audio_latency_mode_t y) {
964                 auto itX = std::find(mOrderedLowLatencyModes.begin(),
965                     mOrderedLowLatencyModes.end(), x);
966                 auto itY = std::find(mOrderedLowLatencyModes.begin(),
967                     mOrderedLowLatencyModes.end(), y);
968                 return itX < itY;
969             });
970 }
971 
attachOutput(audio_io_handle_t output,const std::vector<audio_channel_mask_t> & activeTracksMasks)972 status_t Spatializer::attachOutput(audio_io_handle_t output,
973           const std::vector<audio_channel_mask_t>& activeTracksMasks) {
974     bool outputChanged = false;
975     sp<media::INativeSpatializerCallback> callback;
976 
977     {
978         audio_utils::lock_guard lock(mMutex);
979         ALOGV("%s output %d mOutput %d", __func__, (int)output, (int)mOutput);
980         mLocalLog.log("%s with output %d tracks %zu (mOutput %d)", __func__, (int)output,
981                       activeTracksMasks.size(), (int)mOutput);
982         if (mOutput != AUDIO_IO_HANDLE_NONE) {
983             LOG_ALWAYS_FATAL_IF(mEngine == nullptr, "%s output set without FX engine", __func__);
984             // remove FX instance
985             mEngine->setEnabled(false);
986             mEngine.clear();
987             mPoseController.reset();
988             AudioSystem::removeSupportedLatencyModesCallback(this);
989         }
990 
991         // create FX instance on output
992         AttributionSourceState attributionSource = AttributionSourceState();
993         mEngine = new AudioEffect(attributionSource);
994         mEngine->set(nullptr /* type */, &mEngineDescriptor.uuid, 0 /* priority */,
995                      wp<AudioEffect::IAudioEffectCallback>::fromExisting(this),
996                      AUDIO_SESSION_OUTPUT_STAGE, output, {} /* device */, false /* probe */,
997                      true /* notifyFramesProcessed */);
998         status_t status = mEngine->initCheck();
999         ALOGV("%s mEngine create status %d", __func__, (int)status);
1000         if (status != NO_ERROR) {
1001             return status;
1002         }
1003 
1004         outputChanged = mOutput != output;
1005         mOutput = output;
1006         mActiveTracksMasks = activeTracksMasks;
1007         AudioSystem::addSupportedLatencyModesCallback(this);
1008 
1009         std::vector<audio_latency_mode_t> latencyModes;
1010         status = AudioSystem::getSupportedLatencyModes(mOutput, &latencyModes);
1011         if (status == OK) {
1012             mSupportedLatencyModes = latencyModes;
1013             sortSupportedLatencyModes_l();
1014         }
1015 
1016         checkEngineState_l();
1017         if (mSupportsHeadTracking) {
1018             checkPoseController_l();
1019             checkSensorsState_l();
1020         }
1021         callback = mSpatializerCallback;
1022 
1023         // Restore common effect state.
1024         setEffectParameter_l(SPATIALIZER_PARAM_DISPLAY_ORIENTATION,
1025                 std::vector<float>{mDisplayOrientation});
1026         setEffectParameter_l(SPATIALIZER_PARAM_FOLD_STATE,
1027                 std::vector<uint8_t>{mFoldedState});
1028         setEffectParameter_l(SPATIALIZER_PARAM_HINGE_ANGLE,
1029                 std::vector<float>{mHingeAngle});
1030     }
1031 
1032     if (outputChanged && callback != nullptr) {
1033         callback->onOutputChanged(output);
1034     }
1035 
1036     return NO_ERROR;
1037 }
1038 
detachOutput()1039 audio_io_handle_t Spatializer::detachOutput() {
1040     audio_io_handle_t output = AUDIO_IO_HANDLE_NONE;
1041     sp<media::INativeSpatializerCallback> callback;
1042 
1043     {
1044         audio_utils::lock_guard lock(mMutex);
1045         mLocalLog.log("%s with output %d num tracks %zu",
1046             __func__, (int)mOutput, mActiveTracksMasks.size());
1047         ALOGV("%s mOutput %d", __func__, (int)mOutput);
1048         if (mOutput == AUDIO_IO_HANDLE_NONE) {
1049             return output;
1050         }
1051         // remove FX instance
1052         mEngine->setEnabled(false);
1053         mEngine.clear();
1054         AudioSystem::removeSupportedLatencyModesCallback(this);
1055         output = mOutput;
1056         mOutput = AUDIO_IO_HANDLE_NONE;
1057         mPoseController.reset();
1058         callback = mSpatializerCallback;
1059     }
1060 
1061     if (callback != nullptr) {
1062         callback->onOutputChanged(AUDIO_IO_HANDLE_NONE);
1063     }
1064     return output;
1065 }
1066 
onSupportedLatencyModesChanged(audio_io_handle_t output,const std::vector<audio_latency_mode_t> & modes)1067 void Spatializer::onSupportedLatencyModesChanged(
1068         audio_io_handle_t output, const std::vector<audio_latency_mode_t>& modes) {
1069     ALOGV("%s output %d num modes %zu", __func__, (int)output, modes.size());
1070     sp<AMessage> msg =
1071             new AMessage(EngineCallbackHandler::kWhatOnLatencyModesChanged, mHandler);
1072     msg->setObject(EngineCallbackHandler::kLatencyModesKey,
1073         sp<EngineCallbackHandler::LatencyModes>::make(output, modes));
1074     msg->post();
1075 }
1076 
onSupportedLatencyModesChangedMsg(audio_io_handle_t output,std::vector<audio_latency_mode_t> && modes)1077 void Spatializer::onSupportedLatencyModesChangedMsg(
1078         audio_io_handle_t output, std::vector<audio_latency_mode_t>&& modes) {
1079     audio_utils::lock_guard lock(mMutex);
1080     ALOGV("%s output %d mOutput %d num modes %zu",
1081             __func__, (int)output, (int)mOutput, modes.size());
1082     if (output == mOutput) {
1083         mSupportedLatencyModes = std::move(modes);
1084         sortSupportedLatencyModes_l();
1085         checkSensorsState_l();
1086     }
1087 }
1088 
updateActiveTracks(const std::vector<audio_channel_mask_t> & activeTracksMasks)1089 void Spatializer::updateActiveTracks(
1090         const std::vector<audio_channel_mask_t>& activeTracksMasks) {
1091     audio_utils::lock_guard lock(mMutex);
1092     if (mActiveTracksMasks != activeTracksMasks) {
1093         mLocalLog.log("%s from %zu to %zu",
1094                 __func__, mActiveTracksMasks.size(), activeTracksMasks.size());
1095         mActiveTracksMasks = activeTracksMasks;
1096         checkEngineState_l();
1097         checkSensorsState_l();
1098     }
1099 }
1100 
selectHeadtrackingConnectionMode_l()1101 audio_latency_mode_t Spatializer::selectHeadtrackingConnectionMode_l() {
1102     if (!com::android::media::audio::dsa_over_bt_le_audio()) {
1103         return AUDIO_LATENCY_MODE_LOW;
1104     }
1105     // mSupportedLatencyModes is ordered according to system preferences loaded in
1106     // mOrderedLowLatencyModes
1107     mHeadtrackingConnectionMode = HeadTracking::ConnectionMode::FRAMEWORK_PROCESSED;
1108     audio_latency_mode_t requestedLatencyMode = mSupportedLatencyModes[0];
1109     if (requestedLatencyMode == AUDIO_LATENCY_MODE_DYNAMIC_SPATIAL_AUDIO_HARDWARE) {
1110         if (mSupportedHeadtrackingConnectionModes.find(
1111                 HeadTracking::ConnectionMode::DIRECT_TO_SENSOR_TUNNEL)
1112                     != mSupportedHeadtrackingConnectionModes.end()) {
1113             mHeadtrackingConnectionMode = HeadTracking::ConnectionMode::DIRECT_TO_SENSOR_TUNNEL;
1114         } else if (mSupportedHeadtrackingConnectionModes.find(
1115                 HeadTracking::ConnectionMode::DIRECT_TO_SENSOR_SW)
1116                     != mSupportedHeadtrackingConnectionModes.end()) {
1117             mHeadtrackingConnectionMode = HeadTracking::ConnectionMode::DIRECT_TO_SENSOR_SW;
1118         } else {
1119             // if the engine does not support direct reading of IMU data, do not allow
1120             // DYNAMIC_SPATIAL_AUDIO_HARDWARE mode and fallback to next mode
1121             if (mSupportedLatencyModes.size() > 1) {
1122                 requestedLatencyMode = mSupportedLatencyModes[1];
1123             } else {
1124                 // If only DYNAMIC_SPATIAL_AUDIO_HARDWARE mode is reported by the
1125                 // HAL and the engine does not support it, assert as this is a
1126                 // product configuration error
1127                 LOG_ALWAYS_FATAL("%s: the audio HAL reported only low latency with"
1128                         "HW HID tunneling but the spatializer does not support it",
1129                         __func__);
1130             }
1131         }
1132     }
1133     return requestedLatencyMode;
1134 }
1135 
checkSensorsState_l()1136 void Spatializer::checkSensorsState_l() {
1137     mRequestedLatencyMode = AUDIO_LATENCY_MODE_FREE;
1138     const bool supportsSetLatencyMode = !mSupportedLatencyModes.empty();
1139     bool supportsLowLatencyMode;
1140     if (com::android::media::audio::dsa_over_bt_le_audio()) {
1141         // mSupportedLatencyModes is ordered with MODE_FREE always at the end:
1142         // the first entry is never MODE_FREE if at least one low ltency mode is supported.
1143         supportsLowLatencyMode = supportsSetLatencyMode
1144                 && mSupportedLatencyModes[0] != AUDIO_LATENCY_MODE_FREE;
1145     } else {
1146         supportsLowLatencyMode = supportsSetLatencyMode && std::find(
1147             mSupportedLatencyModes.begin(), mSupportedLatencyModes.end(),
1148             AUDIO_LATENCY_MODE_LOW) != mSupportedLatencyModes.end();
1149     }
1150     if (mSupportsHeadTracking) {
1151         if (mPoseController != nullptr) {
1152             // TODO(b/253297301, b/255433067) reenable low latency condition check
1153             // for Head Tracking after Bluetooth HAL supports it correctly.
1154             if (shouldUseHeadTracking_l() && mLevel != Spatialization::Level::NONE
1155                     && mDesiredHeadTrackingMode != HeadTrackingMode::STATIC
1156                     && mHeadSensor != SpatializerPoseController::INVALID_SENSOR) {
1157                 if (supportsLowLatencyMode) {
1158                     mRequestedLatencyMode = selectHeadtrackingConnectionMode_l();
1159                 }
1160                 if (mEngine != nullptr) {
1161                     setEffectParameter_l(SPATIALIZER_PARAM_HEADTRACKING_MODE,
1162                             std::vector<HeadTracking::Mode>{mActualHeadTrackingMode});
1163                     setEngineHeadtrackingConnectionMode_l();
1164                 }
1165                 // TODO: b/307588546: configure mPoseController according to selected
1166                 // mHeadtrackingConnectionMode
1167                 mPoseController->setHeadSensor(mHeadSensor);
1168                 mPoseController->setScreenSensor(mScreenSensor);
1169             } else {
1170                 mPoseController->setHeadSensor(SpatializerPoseController::INVALID_SENSOR);
1171                 mPoseController->setScreenSensor(SpatializerPoseController::INVALID_SENSOR);
1172                 resetEngineHeadPose_l();
1173             }
1174         } else {
1175             resetEngineHeadPose_l();
1176         }
1177     }
1178     if (mOutput != AUDIO_IO_HANDLE_NONE && supportsSetLatencyMode) {
1179         const status_t status =
1180                 AudioSystem::setRequestedLatencyMode(mOutput, mRequestedLatencyMode);
1181         ALOGD("%s: setRequestedLatencyMode for output thread(%d) to %s returned %d", __func__,
1182               mOutput, toString(mRequestedLatencyMode).c_str(), status);
1183     }
1184 }
1185 
1186 
1187 /* static */
containsImmersiveChannelMask(const std::vector<audio_channel_mask_t> & masks)1188 bool Spatializer::containsImmersiveChannelMask(
1189         const std::vector<audio_channel_mask_t>& masks)
1190 {
1191     for (auto mask : masks) {
1192         if (audio_is_channel_mask_spatialized(mask)) {
1193             return true;
1194         }
1195     }
1196     // Only non-immersive channel masks, e.g. AUDIO_CHANNEL_OUT_STEREO, are present.
1197     return false;
1198 }
1199 
shouldUseHeadTracking_l() const1200 bool Spatializer::shouldUseHeadTracking_l() const {
1201     // Headtracking only available on immersive channel masks.
1202     return containsImmersiveChannelMask(mActiveTracksMasks);
1203 }
1204 
checkEngineState_l()1205 void Spatializer::checkEngineState_l() {
1206     if (mEngine != nullptr) {
1207         if (mLevel != Spatialization::Level::NONE && mActiveTracksMasks.size() > 0) {
1208             mEngine->setEnabled(true);
1209             setEffectParameter_l(SPATIALIZER_PARAM_LEVEL,
1210                     std::vector<Spatialization::Level>{mLevel});
1211         } else {
1212             setEffectParameter_l(SPATIALIZER_PARAM_LEVEL,
1213                     std::vector<Spatialization::Level>{Spatialization::Level::NONE});
1214             mEngine->setEnabled(false);
1215         }
1216     }
1217 }
1218 
checkPoseController_l()1219 void Spatializer::checkPoseController_l() {
1220     bool isControllerNeeded = mDesiredHeadTrackingMode != HeadTrackingMode::STATIC
1221             && mHeadSensor != SpatializerPoseController::INVALID_SENSOR;
1222 
1223     if (isControllerNeeded && mPoseController == nullptr) {
1224         mPoseController = std::make_shared<SpatializerPoseController>(
1225                 static_cast<SpatializerPoseController::Listener*>(this),
1226                 10ms, std::nullopt);
1227         LOG_ALWAYS_FATAL_IF(mPoseController == nullptr,
1228                             "%s could not allocate pose controller", __func__);
1229         mPoseController->setDisplayOrientation(mDisplayOrientation);
1230     } else if (!isControllerNeeded && mPoseController != nullptr) {
1231         mPoseController.reset();
1232         resetEngineHeadPose_l();
1233     }
1234     if (mPoseController != nullptr) {
1235         mPoseController->setDesiredMode(mDesiredHeadTrackingMode);
1236     }
1237 }
1238 
calculateHeadPose()1239 void Spatializer::calculateHeadPose() {
1240     ALOGV("%s", __func__);
1241     audio_utils::lock_guard lock(mMutex);
1242     if (mPoseController != nullptr) {
1243         mPoseController->calculateAsync();
1244     }
1245 }
1246 
onFramesProcessed(int32_t framesProcessed)1247 void Spatializer::onFramesProcessed(int32_t framesProcessed) {
1248     sp<AMessage> msg =
1249             new AMessage(EngineCallbackHandler::kWhatOnFramesProcessed, mHandler);
1250     msg->setInt32(EngineCallbackHandler::kNumFramesKey, framesProcessed);
1251     msg->post();
1252 }
1253 
toString(unsigned level) const1254 std::string Spatializer::toString(unsigned level) const {
1255     std::string prefixSpace(level, ' ');
1256     std::string ss = prefixSpace + "Spatializer:\n";
1257     bool needUnlock = false;
1258 
1259     prefixSpace += ' ';
1260     if (!mMutex.try_lock()) {
1261         // dumpsys even try_lock failed, information dump can be useful although may not accurate
1262         ss.append(prefixSpace).append("try_lock failed, dumpsys below maybe INACCURATE!\n");
1263     } else {
1264         needUnlock = true;
1265     }
1266 
1267     // Spatializer class information.
1268     // 1. Capabilities (mLevels, mHeadTrackingModes, mSpatializationModes, mChannelMasks, etc)
1269     ss.append(prefixSpace).append("Supported levels: [");
1270     for (auto& level : mLevels) {
1271         base::StringAppendF(&ss, " %s", ToString(level).c_str());
1272     }
1273     base::StringAppendF(&ss, "], mLevel: %s", ToString(mLevel).c_str());
1274 
1275     base::StringAppendF(&ss, "\n%smHeadTrackingModes: [", prefixSpace.c_str());
1276     for (auto& mode : mHeadTrackingModes) {
1277         base::StringAppendF(&ss, " %s", ToString(mode).c_str());
1278     }
1279     base::StringAppendF(&ss, "], Desired: %s, Actual %s\n",
1280                         ToString(mDesiredHeadTrackingMode).c_str(),
1281                         ToString(mActualHeadTrackingMode).c_str());
1282 
1283     base::StringAppendF(&ss, "%smSpatializationModes: [", prefixSpace.c_str());
1284     for (auto& mode : mSpatializationModes) {
1285         base::StringAppendF(&ss, " %s", ToString(mode).c_str());
1286     }
1287     ss += "]\n";
1288 
1289     base::StringAppendF(&ss, "%smChannelMasks: ", prefixSpace.c_str());
1290     for (auto& mask : mChannelMasks) {
1291         base::StringAppendF(&ss, "%s", audio_channel_out_mask_to_string(mask));
1292     }
1293     base::StringAppendF(&ss, "%smSpatializedChannelMasks: ", prefixSpace.c_str());
1294     for (auto& mask : mSpatializedChannelMasks) {
1295         base::StringAppendF(&ss, "%s", audio_channel_out_mask_to_string(mask));
1296     }
1297     base::StringAppendF(&ss, "\n%smSupportsHeadTracking: %s\n", prefixSpace.c_str(),
1298                         mSupportsHeadTracking ? "true" : "false");
1299     // 2. Settings (Output, tracks)
1300     base::StringAppendF(&ss, "%sNum Active Tracks: %zu\n",
1301             prefixSpace.c_str(), mActiveTracksMasks.size());
1302     base::StringAppendF(&ss, "%sOutputStreamHandle: %d\n", prefixSpace.c_str(), (int)mOutput);
1303 
1304     // 3. Sensors, Effect information.
1305     base::StringAppendF(&ss, "%sHeadSensorHandle: 0x%08x\n", prefixSpace.c_str(), mHeadSensor);
1306     base::StringAppendF(&ss, "%sScreenSensorHandle: 0x%08x\n", prefixSpace.c_str(), mScreenSensor);
1307     base::StringAppendF(&ss, "%sEffectHandle: %p\n", prefixSpace.c_str(), mEngine.get());
1308     base::StringAppendF(&ss, "%sDisplayOrientation: %f\n", prefixSpace.c_str(),
1309                         mDisplayOrientation);
1310 
1311     // 4. Show flag or property state.
1312     base::StringAppendF(
1313             &ss, "%sStereo Spatialization: %s\n", prefixSpace.c_str(),
1314             SpatializerHelper::isStereoSpatializationFeatureEnabled() ? "true" : "false");
1315 
1316     ss.append(prefixSpace + "CommandLog:\n");
1317     ss += mLocalLog.dumpToString((prefixSpace + " ").c_str(), mMaxLocalLogLine);
1318 
1319     // PostController dump.
1320     if (mPoseController != nullptr) {
1321         ss.append(mPoseController->toString(level + 1))
1322             .append(prefixSpace)
1323             .append("Pose (active stage-to-head) [tx, ty, tz : pitch, roll, yaw]:\n")
1324             .append(prefixSpace)
1325             .append(" PerMinuteHistory:\n")
1326             .append(mPoseDurableRecorder.toString(level + 3))
1327             .append(prefixSpace)
1328             .append(" PerSecondHistory:\n")
1329             .append(mPoseRecorder.toString(level + 3));
1330     } else {
1331         ss.append(prefixSpace).append("SpatializerPoseController not exist\n");
1332     }
1333 
1334     if (needUnlock) {
1335         mMutex.unlock();
1336     }
1337     return ss;
1338 }
1339 
1340 } // namespace android
1341