Spatializer: add support for new latency modes

Add support for additional low latency modes needed for spatial audio
over Bluetooth LE. The new latency modes are used to indicate support
(HAL -> framework) and select (framework -> HAL) the HID transport
mechanism over LE link: either LE-AC or ISO.
Within the ISO mode, two options are defined: SW to indicate that the
IMU data connection to the spatializer effect is done
via the audio framework or HW to indicate that the connection is directly
from the BT controller to the spatializer effect. This last option is only
supported if the spatializer is instantiated in offload mode.

Although all modes are included in the parsing and selection logic, this
CL has only a partial implementation and does not support modes only
possible when the spatializer is offloaded to the audio DSP.

Bug: 307588546
Test: make
Test: manual dynamic spatial audio regression over BT classic
Change-Id: Idb88e4656c562c58df9c24b1ed66551f518790ec
diff --git a/services/audiopolicy/service/Android.bp b/services/audiopolicy/service/Android.bp
index c674909..fb55225 100644
--- a/services/audiopolicy/service/Android.bp
+++ b/services/audiopolicy/service/Android.bp
@@ -31,6 +31,7 @@
         "libmediametrics",
         "libmediautils",
         "libpermission",
+        "libPlatformProperties",
         "libsensor",
         "libsensorprivacy",
         "libshmemcompat",
@@ -42,6 +43,7 @@
         "audiopolicy-aidl-cpp",
         "audiopolicy-types-aidl-cpp",
         "capture_state_listener-aidl-cpp",
+        "com.android.media.audio-aconfig-cc",
         "framework-permission-aidl-cpp",
         "packagemanager_aidl-cpp",
         "spatializer-aidl-cpp",
diff --git a/services/audiopolicy/service/Spatializer.cpp b/services/audiopolicy/service/Spatializer.cpp
index 7859c2c..90418a5 100644
--- a/services/audiopolicy/service/Spatializer.cpp
+++ b/services/audiopolicy/service/Spatializer.cpp
@@ -27,7 +27,9 @@
 #include <sys/types.h>
 
 #include <android/content/AttributionSourceState.h>
+#include <android/sysprop/BluetoothProperties.sysprop.h>
 #include <audio_utils/fixedfft.h>
+#include <com_android_media_audio.h>
 #include <cutils/bitops.h>
 #include <hardware/sensors.h>
 #include <media/stagefright/foundation/AHandler.h>
@@ -213,6 +215,38 @@
     Spatializer::EngineCallbackHandler::kRotation2Key,
 };
 
+// Mapping table between strings read form property bluetooth.core.le.dsa_transport_preference
+// and low latency modes emums.
+//TODO b/273373363: use AIDL enum when available
+const std::map<std::string, audio_latency_mode_t> Spatializer::sStringToLatencyModeMap = {
+    {"le-acl", AUDIO_LATENCY_MODE_LOW},
+    {"iso-sw", AUDIO_LATENCY_MODE_DYNAMIC_SPATIAL_AUDIO_SOFTWARE},
+    {"iso-hw", AUDIO_LATENCY_MODE_DYNAMIC_SPATIAL_AUDIO_HARDWARE},
+};
+
+void Spatializer::loadOrderedLowLatencyModes() {
+    if (!com::android::media::audio::dsa_over_bt_le_audio()) {
+        return;
+    }
+    auto latencyModesStrs = android::sysprop::BluetoothProperties::dsa_transport_preference();
+    std::lock_guard lock(mLock);
+    // First load preferred low latency modes ordered from the property
+    for (auto str : latencyModesStrs) {
+        if (!str.has_value()) continue;
+        if (auto it = sStringToLatencyModeMap.find(str.value());
+                it != sStringToLatencyModeMap.end()) {
+            mOrderedLowLatencyModes.push_back(it->second);
+        }
+    }
+    // Then add unlisted latency modes at the end of the ordered list
+    for (auto it : sStringToLatencyModeMap) {
+        if (std::find(mOrderedLowLatencyModes.begin(), mOrderedLowLatencyModes.end(), it.second)
+                == mOrderedLowLatencyModes.end()) {
+             mOrderedLowLatencyModes.push_back(it.second);
+        }
+    }
+}
+
 // ---------------------------------------------------------------------------
 sp<Spatializer> Spatializer::create(SpatializerPolicyCallback* callback,
                                     const sp<EffectsFactoryHalInterface>& effectsFactoryHal) {
@@ -244,6 +278,7 @@
             spatializer.clear();
             ALOGW("%s loadEngine error: %d  effect %p", __func__, status, effect.get());
         } else {
+            spatializer->loadOrderedLowLatencyModes();
             spatializer->mLocalLog.log("%s with effect Id %p", __func__, effect.get());
         }
     }
@@ -371,6 +406,30 @@
         return BAD_VALUE;
     }
 
+    //TODO b/273373363: use AIDL enum when available
+    if (com::android::media::audio::dsa_over_bt_le_audio()
+            && mSupportsHeadTracking) {
+        mHeadtrackingConnectionMode = HEADTRACKING_CONNECTION_FRAMEWORK_PROCESSED;
+        std::vector<uint8_t> headtrackingConnectionModes;
+        status = getHalParameter<true>(effect, SPATIALIZER_PARAM_SUPPORTED_HEADTRACKING_CONNECTION,
+                &headtrackingConnectionModes);
+        if (status == NO_ERROR) {
+            for (const auto htConnectionMode : headtrackingConnectionModes) {
+                if (htConnectionMode < HEADTRACKING_CONNECTION_FRAMEWORK_PROCESSED ||
+                        htConnectionMode > HEADTRACKING_CONNECTION_DIRECT_TO_SENSOR_TUNNEL) {
+                    ALOGW("%s: ignoring HT connection mode:%d", __func__, (int)htConnectionMode);
+                    continue;
+                }
+                mSupportedHeadtrackingConnectionModes.insert(
+                        static_cast<headtracking_connection_t> (htConnectionMode));
+            }
+            ALOGW_IF(mSupportedHeadtrackingConnectionModes.find(
+                    HEADTRACKING_CONNECTION_FRAMEWORK_PROCESSED)
+                        == mSupportedHeadtrackingConnectionModes.end(),
+                    "%s: HEADTRACKING_CONNECTION_FRAMEWORK_PROCESSED not reported", __func__);
+        }
+    }
+
     // Currently we expose only RELATIVE_WORLD.
     // This is a limitation of the head tracking library based on a UX choice.
     mHeadTrackingModes.push_back(HeadTracking::Mode::DISABLED);
@@ -831,6 +890,7 @@
             } else {
                 setEffectParameter_l(SPATIALIZER_PARAM_HEADTRACKING_MODE,
                                      std::vector<HeadTracking::Mode>{spatializerMode});
+                setEngineHeadtrackingConnectionMode_l();
             }
         }
         callback = mHeadTrackingCallback;
@@ -841,6 +901,32 @@
     }
 }
 
+void Spatializer::setEngineHeadtrackingConnectionMode_l() {
+    if (!com::android::media::audio::dsa_over_bt_le_audio()) {
+        return;
+    }
+    if (mActualHeadTrackingMode != HeadTracking::Mode::DISABLED
+            && !mSupportedHeadtrackingConnectionModes.empty()) {
+        setEffectParameter_l(SPATIALIZER_PARAM_HEADTRACKING_CONNECTION,
+                static_cast<uint8_t>(mHeadtrackingConnectionMode),
+                static_cast<uint32_t>(mHeadSensor));
+    }
+}
+
+void Spatializer::sortSupportedLatencyModes_l() {
+    if (!com::android::media::audio::dsa_over_bt_le_audio()) {
+        return;
+    }
+    std::sort(mSupportedLatencyModes.begin(), mSupportedLatencyModes.end(),
+            [this](audio_latency_mode_t x, audio_latency_mode_t y) {
+                auto itX = std::find(mOrderedLowLatencyModes.begin(),
+                    mOrderedLowLatencyModes.end(), x);
+                auto itY = std::find(mOrderedLowLatencyModes.begin(),
+                    mOrderedLowLatencyModes.end(), y);
+                return itX < itY;
+            });
+}
+
 status_t Spatializer::attachOutput(audio_io_handle_t output, size_t numActiveTracks) {
     bool outputChanged = false;
     sp<media::INativeSpatializerCallback> callback;
@@ -881,6 +967,7 @@
         status = AudioSystem::getSupportedLatencyModes(mOutput, &latencyModes);
         if (status == OK) {
             mSupportedLatencyModes = latencyModes;
+            sortSupportedLatencyModes_l();
         }
 
         checkEngineState_l();
@@ -950,6 +1037,7 @@
             __func__, (int)output, (int)mOutput, modes.size());
     if (output == mOutput) {
         mSupportedLatencyModes = std::move(modes);
+        sortSupportedLatencyModes_l();
         checkSensorsState_l();
     }
 }
@@ -964,26 +1052,75 @@
     }
 }
 
+//TODO b/273373363: use AIDL enum when available
+audio_latency_mode_t Spatializer::selectHeadtrackingConnectionMode_l() {
+    if (!com::android::media::audio::dsa_over_bt_le_audio()) {
+        return AUDIO_LATENCY_MODE_LOW;
+    }
+    // mSupportedLatencyModes is ordered according to system preferences loaded in
+    // mOrderedLowLatencyModes
+    mHeadtrackingConnectionMode = HEADTRACKING_CONNECTION_FRAMEWORK_PROCESSED;
+    audio_latency_mode_t requestedLatencyMode = mSupportedLatencyModes[0];
+    if (requestedLatencyMode == AUDIO_LATENCY_MODE_DYNAMIC_SPATIAL_AUDIO_HARDWARE) {
+        if (mSupportedHeadtrackingConnectionModes.find(
+                HEADTRACKING_CONNECTION_DIRECT_TO_SENSOR_TUNNEL)
+                    != mSupportedHeadtrackingConnectionModes.end()) {
+            mHeadtrackingConnectionMode = HEADTRACKING_CONNECTION_DIRECT_TO_SENSOR_TUNNEL;
+        } else if (mSupportedHeadtrackingConnectionModes.find(
+                HEADTRACKING_CONNECTION_DIRECT_TO_SENSOR_SW)
+                    != mSupportedHeadtrackingConnectionModes.end()) {
+            mHeadtrackingConnectionMode = HEADTRACKING_CONNECTION_DIRECT_TO_SENSOR_SW;
+        } else {
+            // if the engine does not support direct reading of IMU data, do not allow
+            // DYNAMIC_SPATIAL_AUDIO_HARDWARE mode and fallback to next mode
+            if (mSupportedLatencyModes.size() > 1) {
+                requestedLatencyMode = mSupportedLatencyModes[1];
+            } else {
+                // If only DYNAMIC_SPATIAL_AUDIO_HARDWARE mode is reported by the
+                // HAL and the engine does not support it, assert as this is a
+                // product configuration error
+                LOG_ALWAYS_FATAL("%s: the audio HAL reported only low latency with"
+                        "HW HID tunneling but the spatializer does not support it",
+                        __func__);
+            }
+        }
+    }
+    return requestedLatencyMode;
+}
+
 void Spatializer::checkSensorsState_l() {
     audio_latency_mode_t requestedLatencyMode = AUDIO_LATENCY_MODE_FREE;
     const bool supportsSetLatencyMode = !mSupportedLatencyModes.empty();
-    const bool supportsLowLatencyMode = supportsSetLatencyMode && std::find(
+    bool supportsLowLatencyMode;
+    if (com::android::media::audio::dsa_over_bt_le_audio()) {
+        // mSupportedLatencyModes is ordered with MODE_FREE always at the end:
+        // the first entry is never MODE_FREE if at least one low ltency mode is supported.
+        supportsLowLatencyMode = supportsSetLatencyMode
+                && mSupportedLatencyModes[0] != AUDIO_LATENCY_MODE_FREE;
+    } else {
+        supportsLowLatencyMode = supportsSetLatencyMode && std::find(
             mSupportedLatencyModes.begin(), mSupportedLatencyModes.end(),
             AUDIO_LATENCY_MODE_LOW) != mSupportedLatencyModes.end();
+    }
     if (mSupportsHeadTracking) {
         if (mPoseController != nullptr) {
             // TODO(b/253297301, b/255433067) reenable low latency condition check
             // for Head Tracking after Bluetooth HAL supports it correctly.
             if (mNumActiveTracks > 0 && mLevel != Spatialization::Level::NONE
-                && mDesiredHeadTrackingMode != HeadTrackingMode::STATIC
-                && mHeadSensor != SpatializerPoseController::INVALID_SENSOR) {
+                    && mDesiredHeadTrackingMode != HeadTrackingMode::STATIC
+                    && mHeadSensor != SpatializerPoseController::INVALID_SENSOR) {
+                if (supportsLowLatencyMode) {
+                    requestedLatencyMode = selectHeadtrackingConnectionMode_l();
+                }
                 if (mEngine != nullptr) {
                     setEffectParameter_l(SPATIALIZER_PARAM_HEADTRACKING_MODE,
                             std::vector<HeadTracking::Mode>{mActualHeadTrackingMode});
+                    setEngineHeadtrackingConnectionMode_l();
                 }
+                // TODO: b/307588546: configure mPoseController according to selected
+                // mHeadtrackingConnectionMode
                 mPoseController->setHeadSensor(mHeadSensor);
                 mPoseController->setScreenSensor(mScreenSensor);
-                if (supportsLowLatencyMode) requestedLatencyMode = AUDIO_LATENCY_MODE_LOW;
             } else {
                 mPoseController->setHeadSensor(SpatializerPoseController::INVALID_SENSOR);
                 mPoseController->setScreenSensor(SpatializerPoseController::INVALID_SENSOR);
diff --git a/services/audiopolicy/service/Spatializer.h b/services/audiopolicy/service/Spatializer.h
index 4ef07ce..123517e 100644
--- a/services/audiopolicy/service/Spatializer.h
+++ b/services/audiopolicy/service/Spatializer.h
@@ -32,6 +32,7 @@
 #include <media/stagefright/foundation/ALooper.h>
 #include <system/audio_effects/effect_spatializer.h>
 #include <string>
+#include <unordered_set>
 
 #include "SpatializerPoseController.h"
 
@@ -281,6 +282,33 @@
     }
 
     /**
+     * Set a parameter to spatializer engine by calling setParameter on mEngine AudioEffect object.
+     * The variant is for compound parameters with two values of different base types
+     */
+    template<typename P1, typename P2>
+    status_t setEffectParameter_l(uint32_t type, const P1 val1, const P2 val2) REQUIRES(mLock) {
+        static_assert(sizeof(P1) <= sizeof(uint32_t), "The size of P1 must less than 32 bits");
+        static_assert(sizeof(P2) <= sizeof(uint32_t), "The size of P2 must less than 32 bits");
+
+        uint32_t cmd[sizeof(effect_param_t) / sizeof(uint32_t) + 3];
+        effect_param_t *p = (effect_param_t *)cmd;
+        p->psize = sizeof(uint32_t);
+        p->vsize = 2 * sizeof(uint32_t);
+        *(uint32_t *)p->data = type;
+        *((uint32_t *)p->data + 1) = static_cast<uint32_t>(val1);
+        *((uint32_t *)p->data + 2) = static_cast<uint32_t>(val2);
+
+        status_t status = mEngine->setParameter(p);
+        if (status != NO_ERROR) {
+            return status;
+        }
+        if (p->status != NO_ERROR) {
+            return p->status;
+        }
+        return NO_ERROR;
+    }
+
+    /**
      * Get a parameter from spatializer engine by calling getParameter on AudioEffect object.
      * It is possible to read more than one value of type T according to the parameter type
      * by specifying values vector size.
@@ -312,6 +340,34 @@
         return NO_ERROR;
     }
 
+    /**
+     * Get a parameter from spatializer engine by calling getParameter on AudioEffect object.
+     * The variant is for compound parameters with two values of different base types
+     */
+    template<typename P1, typename P2>
+    status_t getEffectParameter_l(uint32_t type, P1 *val1, P2 *val2) REQUIRES(mLock) {
+        static_assert(sizeof(P1) <= sizeof(uint32_t), "The size of P1 must less than 32 bits");
+        static_assert(sizeof(P2) <= sizeof(uint32_t), "The size of P2 must less than 32 bits");
+
+        uint32_t cmd[sizeof(effect_param_t) / sizeof(uint32_t) + 3];
+        effect_param_t *p = (effect_param_t *)cmd;
+        p->psize = sizeof(uint32_t);
+        p->vsize = 2 * sizeof(uint32_t);
+        *(uint32_t *)p->data = type;
+
+        status_t status = mEngine->getParameter(p);
+
+        if (status != NO_ERROR) {
+            return status;
+        }
+        if (p->status != NO_ERROR) {
+            return p->status;
+        }
+        *val1 = static_cast<P1>(*((uint32_t *)p->data + 1));
+        *val2 = static_cast<P2>(*((uint32_t *)p->data + 2));
+        return NO_ERROR;
+    }
+
     virtual void onFramesProcessed(int32_t framesProcessed) override;
 
     /**
@@ -339,6 +395,35 @@
      */
     void resetEngineHeadPose_l() REQUIRES(mLock);
 
+    /** Read bluetooth.core.le.dsa_transport_preference property and populate the ordered list of
+     * preferred low latency modes in mOrderedLowLatencyModes.
+     */
+    void loadOrderedLowLatencyModes();
+
+    /**
+     * Sort mSupportedLatencyModes list according to the preference order stored in
+     * mOrderedLowLatencyModes.
+     * Note: Because MODE_FREE is not in mOrderedLowLatencyModes, it will always be at
+     * the end of the list.
+     */
+    void sortSupportedLatencyModes_l() REQUIRES(mLock);
+
+    /**
+     * Called after enabling head tracking in the spatializer engine to indicate which
+     * connection mode should be used among those supported. The selection depends on
+     * currently supported latency modes reported by the audio HAL.
+     * When the connection mode is direct to the sensor, the sensor ID is also communicated
+     * to the spatializer engine.
+     */
+    void setEngineHeadtrackingConnectionMode_l() REQUIRES(mLock);
+
+    /**
+     * Select the desired head tracking connection mode for the spatializer engine among the list
+     * stored in mSupportedHeadtrackingConnectionModes at init time.
+     * Also returns the desired low latency mode according to selected connection mode.
+     */
+    audio_latency_mode_t selectHeadtrackingConnectionMode_l() REQUIRES(mLock);
+
     /** Effect engine descriptor */
     const effect_descriptor_t mEngineDescriptor;
     /** Callback interface to parent audio policy service */
@@ -398,6 +483,13 @@
     std::vector<media::audio::common::Spatialization::Mode> mSpatializationModes;
     std::vector<audio_channel_mask_t> mChannelMasks;
     bool mSupportsHeadTracking;
+    /** List of supported headtracking connection modes reported by the spatializer.
+     * If the list is empty, the spatializer does not support any optional connection
+     * mode and mode HEADTRACKING_CONNECTION_FRAMEWORK_PROCESSED is assumed.
+     */
+    std::unordered_set<headtracking_connection_t> mSupportedHeadtrackingConnectionModes;
+    /** Selected HT connection mode when several modes are supported by the spatializer */
+    headtracking_connection_t mHeadtrackingConnectionMode;
 
     // Looper thread for mEngine callbacks
     class EngineCallbackHandler;
@@ -407,7 +499,10 @@
 
     size_t mNumActiveTracks GUARDED_BY(mLock) = 0;
     std::vector<audio_latency_mode_t> mSupportedLatencyModes GUARDED_BY(mLock);
-
+    /** preference order for low latency modes according to persist.bluetooth.hid.transport */
+    std::vector<audio_latency_mode_t> mOrderedLowLatencyModes;
+    /** string to latency mode map used to parse bluetooth.core.le.dsa_transport_preference */
+    static const std::map<std::string, audio_latency_mode_t> sStringToLatencyModeMap;
     static const std::vector<const char*> sHeadPoseKeys;
 
     // Local log for command messages.