vibrator: cs40l25: implement bandwidth amplitude map

Implement the formula used to generate the bandwidth amplitude map of
the device.

Details of the formula can be found at go/haptics-bw-amp-map-formula

Bug: 230807667
Test: verify using idlcli that the map values are correct
Change-Id: Id576c57385244f4886516029af812d15ce66fe88
Signed-off-by: chasewu <chasewu@google.com>

5 files changed, 118 insertions, 9 deletions

diff --git a/vibrator/common/utils.h b/vibrator/common/utils.h
index 4ffd2b0c..188554da 100644
--- a/vibrator/common/utils.h
+++ b/vibrator/common/utils.h
@@ -15,14 +15,15 @@
  */
 #pragma once
 
-#include <fstream>
-#include <map>
-#include <sstream>
-
 #include <android-base/macros.h>
+#include <android-base/parsedouble.h>
 #include <android-base/properties.h>
 #include <log/log.h>
 
+#include <fstream>
+#include <map>
+#include <sstream>
+
 namespace aidl {
 namespace android {
 namespace hardware {
@@ -85,7 +86,16 @@ inline void unpack<std::string>(std::istream &stream, std::string *value) {
 
 template <typename T>
 inline Enable_If_Signed<T, T> getProperty(const std::string &key, const T def) {
-    return ::android::base::GetIntProperty(key, def);
+    if (std::is_floating_point_v<T>) {
+        float result;
+        std::string value = ::android::base::GetProperty(key, "");
+        if (!value.empty() && ::android::base::ParseFloat(value, &result)) {
+            return result;
+        }
+        return def;
+    } else {
+        return ::android::base::GetIntProperty(key, def);
+    }
 }
 
 template <typename T>
diff --git a/vibrator/cs40l25/Hardware.h b/vibrator/cs40l25/Hardware.h
index 9fa3685f..167e382b 100644
--- a/vibrator/cs40l25/Hardware.h
+++ b/vibrator/cs40l25/Hardware.h
@@ -127,6 +127,8 @@ class HwCal : public Vibrator::HwCal, private HwCalBase {
 
     static constexpr uint32_t VERSION_DEFAULT = 1;
     static constexpr int32_t DEFAULT_FREQUENCY_SHIFT = 0;
+    static constexpr float DEFAULT_DEVICE_MASS = 0.21;
+    static constexpr float DEFAULT_LOC_COEFF = 0.5;
     static constexpr uint32_t Q_DEFAULT = 15.5 * Q_FLOAT_TO_FIXED;
     static constexpr std::array<uint32_t, 6> V_LEVELS_DEFAULT = {60, 70, 80, 90, 100, 76};
     static constexpr std::array<uint32_t, 2> V_TICK_DEFAULT = {10, 70};
@@ -146,6 +148,12 @@ class HwCal : public Vibrator::HwCal, private HwCalBase {
     bool getLongFrequencyShift(int32_t *value) override {
         return getProperty("long.frequency.shift", value, DEFAULT_FREQUENCY_SHIFT);
     }
+    bool getDeviceMass(float *value) override {
+        return getProperty("device.mass", value, DEFAULT_DEVICE_MASS);
+    }
+    bool getLocCoeff(float *value) override {
+        return getProperty("loc.coeff", value, DEFAULT_LOC_COEFF);
+    }
     bool getF0(uint32_t *value) override { return getPersist(F0_CONFIG, value); }
     bool getRedc(uint32_t *value) override { return getPersist(REDC_CONFIG, value); }
     bool getQ(uint32_t *value) override {
diff --git a/vibrator/cs40l25/Vibrator.cpp b/vibrator/cs40l25/Vibrator.cpp
index 3da42340..669258f2 100644
--- a/vibrator/cs40l25/Vibrator.cpp
+++ b/vibrator/cs40l25/Vibrator.cpp
@@ -16,6 +16,7 @@
 
 #include "Vibrator.h"
 
+#include <android-base/properties.h>
 #include <hardware/hardware.h>
 #include <hardware/vibrator.h>
 #include <log/log.h>
@@ -94,6 +95,12 @@ static constexpr int32_t Q14_BIT_SHIFT = 14;
 // See the LRA Calibration Support documentation for more details.
 static constexpr int32_t Q16_BIT_SHIFT = 16;
 
+// Measured ReDC, redc_measured, is represented by Q7.17 fixed
+// point format on cs40l2x devices. The expression to calculate redc is:
+//   redc = redc_measured * 5.857 / 2^Q17_BIT_SHIFT
+// See the LRA Calibration Support documentation for more details.
+static constexpr int32_t Q17_BIT_SHIFT = 17;
+
 static constexpr int32_t COMPOSE_PWLE_PRIMITIVE_DURATION_MAX_MS = 999;
 static constexpr float PWLE_LEVEL_MIN = 0.0f;
 static constexpr float PWLE_LEVEL_MAX = 1.0f;
@@ -215,6 +222,7 @@ Vibrator::Vibrator(std::unique_ptr<HwApi> hwapi, std::unique_ptr<HwCal> hwcal)
     }
     if (mHwCal->getRedc(&caldata)) {
         mHwApi->setRedc(caldata);
+        mRedc = caldata;
     }
     if (mHwCal->getQ(&caldata)) {
         mHwApi->setQ(caldata);
@@ -271,6 +279,8 @@ Vibrator::Vibrator(std::unique_ptr<HwApi> hwapi, std::unique_ptr<HwCal> hwcal)
     }
 
     createPwleMaxLevelLimitMap();
+    mGenerateBandwidthAmplitudeMapDone = false;
+    mBandwidthAmplitudeMap = generateBandwidthAmplitudeMap();
     mIsUnderExternalControl = false;
     setPwleRampDown();
     mIsChirpEnabled = mHwCal->isChirpEnabled();
@@ -605,14 +615,85 @@ ndk::ScopedAStatus Vibrator::getFrequencyMinimum(float *freqMinimumHz) {
     }
 }
 
+static float redcToFloat(uint32_t redcMeasured) {
+    return redcMeasured * 5.857 / (1 << Q17_BIT_SHIFT);
+}
+
+std::vector<float> Vibrator::generateBandwidthAmplitudeMap() {
+    // Use constant Q Factor of 10 from HW's suggestion
+    const float qFactor = 10.0f;
+    const float blSys = 1.1f;
+    const float gravity = 9.81f;
+    const float maxVoltage = 12.3f;
+    float deviceMass = 0, locCoeff = 0;
+
+    mHwCal->getDeviceMass(&deviceMass);
+    mHwCal->getLocCoeff(&locCoeff);
+    if (!deviceMass || !locCoeff) {
+        ALOGE("Failed to get Device Mass: %f and Loc Coeff: %f", deviceMass, locCoeff);
+        return std::vector<float>();
+    }
+
+    // Resistance value need to be retrieved from calibration file
+    if (!mRedc) {
+        ALOGE("Failed to get redc");
+        return std::vector<float>();
+    }
+    const float rSys = redcToFloat(mRedc);
+
+    std::vector<float> bandwidthAmplitudeMap(PWLE_BW_MAP_SIZE, 1.0);
+
+    const float wnSys = mResonantFrequency * 2 * M_PI;
+
+    float frequencyHz = PWLE_FREQUENCY_MIN_HZ;
+    float frequencyRadians = 0.0f;
+    float vLevel = 0.4f;
+    float vSys = (mLongEffectVol[1] / 100.0) * maxVoltage * vLevel;
+    float maxAsys = 0;
+
+    for (int i = 0; i < PWLE_BW_MAP_SIZE; i++) {
+        frequencyRadians = frequencyHz * 2 * M_PI;
+        vLevel = pwleMaxLevelLimitMap[i];
+        vSys = (mLongEffectVol[1] / 100.0) * maxVoltage * vLevel;
+
+        float var1 = pow((pow(wnSys, 2) - pow(frequencyRadians, 2)), 2);
+        float var2 = pow((wnSys * frequencyRadians / qFactor), 2);
+
+        float psysAbs = sqrt(var1 + var2);
+        // The equation and all related details: b/170919640#comment5
+        float amplitudeSys = (vSys * blSys * locCoeff / rSys / deviceMass) *
+                             pow(frequencyRadians, 2) / psysAbs / gravity;
+        // Record the maximum acceleration for the next for loop
+        if (amplitudeSys > maxAsys)
+            maxAsys = amplitudeSys;
+
+        bandwidthAmplitudeMap[i] = amplitudeSys;
+        frequencyHz += PWLE_FREQUENCY_RESOLUTION_HZ;
+    }
+    // Scaled the map between 0.00 and 1.00
+    if (maxAsys > 0) {
+        for (int j = 0; j < PWLE_BW_MAP_SIZE; j++) {
+            bandwidthAmplitudeMap[j] = std::floor((bandwidthAmplitudeMap[j] / maxAsys) * 100) / 100;
+        }
+        mGenerateBandwidthAmplitudeMapDone = true;
+    } else {
+        return std::vector<float>();
+    }
+
+    return bandwidthAmplitudeMap;
+}
+
 ndk::ScopedAStatus Vibrator::getBandwidthAmplitudeMap(std::vector<float> *_aidl_return) {
-    // TODO(b/170919640): complete implementation
     int32_t capabilities;
     Vibrator::getCapabilities(&capabilities);
     if (capabilities & IVibrator::CAP_FREQUENCY_CONTROL) {
-        std::vector<float> bandwidthAmplitudeMap(PWLE_BW_MAP_SIZE, 1.0);
-        *_aidl_return = bandwidthAmplitudeMap;
-        return ndk::ScopedAStatus::ok();
+        if (!mGenerateBandwidthAmplitudeMapDone) {
+            mBandwidthAmplitudeMap = generateBandwidthAmplitudeMap();
+        }
+        *_aidl_return = mBandwidthAmplitudeMap;
+        return (!mBandwidthAmplitudeMap.empty())
+                       ? ndk::ScopedAStatus::ok()
+                       : ndk::ScopedAStatus::fromExceptionCode(EX_ILLEGAL_STATE);
     } else {
         return ndk::ScopedAStatus::fromExceptionCode(EX_UNSUPPORTED_OPERATION);
     }
diff --git a/vibrator/cs40l25/Vibrator.h b/vibrator/cs40l25/Vibrator.h
index ad229a40..70543e30 100644
--- a/vibrator/cs40l25/Vibrator.h
+++ b/vibrator/cs40l25/Vibrator.h
@@ -125,6 +125,10 @@ class Vibrator : public BnVibrator {
         virtual bool getQ(uint32_t *value) = 0;
         // Obtains frequency shift for long vibrations.
         virtual bool getLongFrequencyShift(int32_t *value) = 0;
+        // Obtains device mass for calculating the bandwidth amplitude map
+        virtual bool getDeviceMass(float *value) = 0;
+        // Obtains loc coeff for calculating the bandwidth amplitude map
+        virtual bool getLocCoeff(float *value) = 0;
         // Obtains the discreet voltage levels to be applied for the various
         // waveforms, in units of 1%.
         virtual bool getVolLevels(std::array<uint32_t, 6> *value) = 0;
@@ -206,6 +210,7 @@ class Vibrator : public BnVibrator {
     bool enableHapticPcmAmp(struct pcm **haptic_pcm, bool enable, int card, int device);
     void createPwleMaxLevelLimitMap();
     void setPwleRampDown();
+    std::vector<float> generateBandwidthAmplitudeMap();
 
     std::unique_ptr<HwApi> mHwApi;
     std::unique_ptr<HwCal> mHwCal;
@@ -222,8 +227,11 @@ class Vibrator : public BnVibrator {
     bool mHasHapticAlsaDevice;
     bool mIsUnderExternalControl;
     float mResonantFrequency;
+    uint32_t mRedc{0};
     int8_t mActiveId{-1};
     bool mIsChirpEnabled;
+    std::vector<float> mBandwidthAmplitudeMap;
+    bool mGenerateBandwidthAmplitudeMapDone;
 };
 
 }  // namespace vibrator
diff --git a/vibrator/cs40l25/tests/mocks.h b/vibrator/cs40l25/tests/mocks.h
index 0edb8a63..efcef527 100644
--- a/vibrator/cs40l25/tests/mocks.h
+++ b/vibrator/cs40l25/tests/mocks.h
@@ -68,6 +68,8 @@ class MockCal : public ::aidl::android::hardware::vibrator::Vibrator::HwCal {
     MOCK_METHOD1(getClickVolLevels, bool(std::array<uint32_t, 2> *value));
     MOCK_METHOD1(getLongVolLevels, bool(std::array<uint32_t, 2> *value));
     MOCK_METHOD0(isChirpEnabled, bool());
+    MOCK_METHOD1(getDeviceMass, bool(float *value));
+    MOCK_METHOD1(getLocCoeff, bool(float *value));
     MOCK_METHOD1(debug, void(int fd));
 
     ~MockCal() override { destructor(); };