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