Adding support for TYPE_SENSOR_PLACEMENT in sensor HAL

Bug:163083513
Test: Build, and run cts testSensorAdditionalInfo and make sure it
passes for accel and gyro

Change-Id: I4dd766ef9cc0a2e3c75cea3f17a6a1a5b3ade831

2 files changed, 154 insertions, 7 deletions

diff --git a/hal/sensors/2.0/Sensor.cpp b/hal/sensors/2.0/Sensor.cpp
index c221039..3e3150e 100644
--- a/hal/sensors/2.0/Sensor.cpp
+++ b/hal/sensors/2.0/Sensor.cpp
@@ -28,9 +28,11 @@ namespace V2_0 {
 namespace subhal {
 namespace implementation {
 
+using ::android::hardware::sensors::V1_0::AdditionalInfoType;
 using ::android::hardware::sensors::V1_0::MetaDataEventType;
 using ::android::hardware::sensors::V1_0::SensorFlagBits;
 using ::android::hardware::sensors::V1_0::SensorStatus;
+using ::sensor::hal::configuration::V1_0::Location;
 using ::sensor::hal::configuration::V1_0::Orientation;
 
 SensorBase::SensorBase(int32_t sensorHandle, ISensorsEventCallback* callback, SensorType type)
@@ -95,11 +97,27 @@ void HWSensorBase::batch(int32_t samplingPeriodNs) {
     }
 }
 
+void HWSensorBase::sendAdditionalInfoReport() {
+    std::vector<Event> events;
+
+    for (const auto& frame : mAdditionalInfoFrames) {
+        events.emplace_back(Event{
+                .sensorHandle = mSensorInfo.sensorHandle,
+                .sensorType = SensorType::ADDITIONAL_INFO,
+                .timestamp = android::elapsedRealtimeNano(),
+                .u.additional = frame,
+        });
+    }
+
+    if (!events.empty()) mCallback->postEvents(events, isWakeUpSensor());
+}
+
 void HWSensorBase::activate(bool enable) {
     std::unique_lock<std::mutex> lock(mRunMutex);
     if (mIsEnabled != enable) {
         mIsEnabled = enable;
         enable_sensor(mIioData.sysfspath, enable);
+        if (enable) sendAdditionalInfoReport();
         mWaitCV.notify_all();
     }
 }
@@ -122,8 +140,14 @@ Result SensorBase::flush() {
     return Result::OK;
 }
 
+Result HWSensorBase::flush() {
+    SensorBase::flush();
+    sendAdditionalInfoReport();
+    return Result::OK;
+}
+
 template <size_t N>
-float getChannelData(const std::array<float, N>& channelData, int64_t map, bool negate) {
+static float getChannelData(const std::array<float, N>& channelData, int64_t map, bool negate) {
     return negate ? -channelData[map] : channelData[map];
 }
 
@@ -224,14 +248,14 @@ ssize_t HWSensorBase::calculateScanSize() {
     return numBytes;
 }
 
-status_t checkAxis(int64_t map) {
+static status_t checkAxis(int64_t map) {
     if (map < 0 || map >= NUM_OF_DATA_CHANNELS)
         return BAD_VALUE;
     else
         return OK;
 }
 
-std::optional<std::vector<Orientation>> getOrientation(
+static std::optional<std::vector<Orientation>> getOrientation(
         std::optional<std::vector<Configuration>> config) {
     if (!config) return std::nullopt;
     if (config->empty()) return std::nullopt;
@@ -239,7 +263,15 @@ std::optional<std::vector<Orientation>> getOrientation(
     return sensorCfg.getOrientation();
 }
 
-status_t checkOrientation(std::optional<std::vector<Configuration>> config) {
+static std::optional<std::vector<Location>> getLocation(
+        std::optional<std::vector<Configuration>> config) {
+    if (!config) return std::nullopt;
+    if (config->empty()) return std::nullopt;
+    Configuration& sensorCfg = (*config)[0];
+    return sensorCfg.getLocation();
+}
+
+static status_t checkOrientation(std::optional<std::vector<Configuration>> config) {
     status_t ret = OK;
     std::optional<std::vector<Orientation>> sensorOrientationList = getOrientation(config);
     if (!sensorOrientationList) return OK;
@@ -289,13 +321,113 @@ void HWSensorBase::setOrientation(std::optional<std::vector<Configuration>> conf
     }
 }
 
-status_t checkIIOData(const struct iio_device_data& iio_data) {
+static status_t checkIIOData(const struct iio_device_data& iio_data) {
     status_t ret = OK;
     for (auto i = 0u; i < iio_data.channelInfo.size(); i++) {
         if (iio_data.channelInfo[i].index > NUM_OF_DATA_CHANNELS) return BAD_VALUE;
     }
     return ret;
 }
+
+static status_t setSensorPlacementData(AdditionalInfo* sensorPlacement, int index, float value) {
+    if (!sensorPlacement) return BAD_VALUE;
+
+    int arraySize =
+            sizeof(sensorPlacement->u.data_float) / sizeof(sensorPlacement->u.data_float[0]);
+    if (index < 0 || index >= arraySize) return BAD_VALUE;
+
+    sensorPlacement->u.data_float[index] = value;
+    return OK;
+}
+
+status_t HWSensorBase::getSensorPlacement(AdditionalInfo* sensorPlacement,
+                                          const std::optional<std::vector<Configuration>>& config) {
+    if (!sensorPlacement) return BAD_VALUE;
+
+    auto sensorLocationList = getLocation(config);
+    if (!sensorLocationList) return BAD_VALUE;
+    if (sensorLocationList->empty()) return BAD_VALUE;
+
+    auto sensorOrientationList = getOrientation(config);
+    if (!sensorOrientationList) return BAD_VALUE;
+    if (sensorOrientationList->empty()) return BAD_VALUE;
+
+    sensorPlacement->type = AdditionalInfoType::AINFO_SENSOR_PLACEMENT;
+    sensorPlacement->serial = 0;
+    memset(&sensorPlacement->u.data_float, 0, sizeof(sensorPlacement->u.data_float));
+
+    Location& sensorLocation = (*sensorLocationList)[0];
+    // SensorPlacementData is given as a 3x4 matrix consisting of a 3x3 rotation matrix (R)
+    // concatenated with a 3x1 location vector (t) in row major order. Example: This raw buffer:
+    // {x1,y1,z1,l1,x2,y2,z2,l2,x3,y3,z3,l3} corresponds to the following 3x4 matrix:
+    //  x1 y1 z1 l1
+    //  x2 y2 z2 l2
+    //  x3 y3 z3 l3
+    // LOCATION_X_IDX,LOCATION_Y_IDX,LOCATION_Z_IDX corresponds to the indexes of the location
+    // vector (l1,l2,l3) in the raw buffer.
+    status_t ret = setSensorPlacementData(sensorPlacement, HWSensorBase::LOCATION_X_IDX,
+                                          sensorLocation.getX());
+    if (ret != OK) return ret;
+    ret = setSensorPlacementData(sensorPlacement, HWSensorBase::LOCATION_Y_IDX,
+                                 sensorLocation.getY());
+    if (ret != OK) return ret;
+    ret = setSensorPlacementData(sensorPlacement, HWSensorBase::LOCATION_Z_IDX,
+                                 sensorLocation.getZ());
+    if (ret != OK) return ret;
+
+    Orientation& sensorOrientation = (*sensorOrientationList)[0];
+    if (sensorOrientation.getRotate()) {
+        // If the HAL is already rotating the sensor orientation to align with the Android
+        // Coordinate system, then the sensor rotation matrix will be an identity matrix
+        // ROTATION_X_IDX, ROTATION_Y_IDX, ROTATION_Z_IDX corresponds to indexes of the
+        // (x1,y1,z1) in the raw buffer.
+        ret = setSensorPlacementData(sensorPlacement, HWSensorBase::ROTATION_X_IDX + 0, 1);
+        if (ret != OK) return ret;
+        ret = setSensorPlacementData(sensorPlacement, HWSensorBase::ROTATION_Y_IDX + 4, 1);
+        if (ret != OK) return ret;
+        ret = setSensorPlacementData(sensorPlacement, HWSensorBase::ROTATION_Z_IDX + 8, 1);
+        if (ret != OK) return ret;
+    } else {
+        ret = setSensorPlacementData(
+                sensorPlacement,
+                HWSensorBase::ROTATION_X_IDX + 4 * sensorOrientation.getFirstX()->getMap(),
+                sensorOrientation.getFirstX()->getNegate() ? -1 : 1);
+        if (ret != OK) return ret;
+        ret = setSensorPlacementData(
+                sensorPlacement,
+                HWSensorBase::ROTATION_Y_IDX + 4 * sensorOrientation.getFirstY()->getMap(),
+                sensorOrientation.getFirstY()->getNegate() ? -1 : 1);
+        if (ret != OK) return ret;
+        ret = setSensorPlacementData(
+                sensorPlacement,
+                HWSensorBase::ROTATION_Z_IDX + 4 * sensorOrientation.getFirstZ()->getMap(),
+                sensorOrientation.getFirstZ()->getNegate() ? -1 : 1);
+        if (ret != OK) return ret;
+    }
+    return OK;
+}
+
+status_t HWSensorBase::setAdditionalInfoFrames(
+        const std::optional<std::vector<Configuration>>& config) {
+    AdditionalInfo additionalInfoSensorPlacement;
+    status_t ret = getSensorPlacement(&additionalInfoSensorPlacement, config);
+    if (ret != OK) return ret;
+
+    const AdditionalInfo additionalInfoBegin = {
+            .type = AdditionalInfoType::AINFO_BEGIN,
+            .serial = 0,
+    };
+    const AdditionalInfo additionalInfoEnd = {
+            .type = AdditionalInfoType::AINFO_END,
+            .serial = 0,
+    };
+
+    mAdditionalInfoFrames.insert(
+            mAdditionalInfoFrames.end(),
+            {additionalInfoBegin, additionalInfoSensorPlacement, additionalInfoEnd});
+    return OK;
+}
+
 HWSensorBase* HWSensorBase::buildSensor(int32_t sensorHandle, ISensorsEventCallback* callback,
                                         const struct iio_device_data& iio_data,
                                         const std::optional<std::vector<Configuration>>& config) {
@@ -324,6 +456,8 @@ HWSensorBase::HWSensorBase(int32_t sensorHandle, ISensorsEventCallback* callback
             (data.power_microwatts / 1000.f) / SENSOR_VOLTAGE_DEFAULT;  // converting uW to mA
     mIioData = data;
     setOrientation(config);
+    status_t ret = setAdditionalInfoFrames(config);
+    if (ret == OK) mSensorInfo.flags |= SensorFlagBits::ADDITIONAL_INFO;
     unsigned int max_sampling_frequency = 0;
     unsigned int min_sampling_frequency = UINT_MAX;
     for (auto i = 0u; i < data.sampling_freq_avl.size(); i++) {
diff --git a/hal/sensors/2.0/Sensor.h b/hal/sensors/2.0/Sensor.h
index 8f537ad..dc2c3f4 100644
--- a/hal/sensors/2.0/Sensor.h
+++ b/hal/sensors/2.0/Sensor.h
@@ -30,12 +30,12 @@
 // Subtract the timestamp channel to get the number of data channels
 #define NUM_OF_DATA_CHANNELS NUM_OF_CHANNEL_SUPPORTED - 1
 
+using ::android::hardware::sensors::V1_0::AdditionalInfo;
 using ::android::hardware::sensors::V1_0::Event;
 using ::android::hardware::sensors::V1_0::OperationMode;
 using ::android::hardware::sensors::V1_0::Result;
 using ::android::hardware::sensors::V1_0::SensorInfo;
 using ::android::hardware::sensors::V1_0::SensorType;
-using ::sensor::hal::configuration::V1_0::AxisType;
 using ::sensor::hal::configuration::V1_0::Configuration;
 
 namespace android {
@@ -72,7 +72,7 @@ class SensorBase {
     const SensorInfo& getSensorInfo() const;
     virtual void batch(int32_t samplingPeriodNs) = 0;
     virtual void activate(bool enable) = 0;
-    Result flush();
+    virtual Result flush();
     void setOperationMode(OperationMode mode);
     bool supportsDataInjection() const;
     Result injectEvent(const Event& event);
@@ -101,14 +101,23 @@ class HWSensorBase : public SensorBase {
     ~HWSensorBase();
     void batch(int32_t samplingPeriodNs);
     void activate(bool enable);
+    Result flush();
     struct iio_device_data mIioData;
 
   private:
+    static constexpr uint8_t LOCATION_X_IDX = 3;
+    static constexpr uint8_t LOCATION_Y_IDX = 7;
+    static constexpr uint8_t LOCATION_Z_IDX = 11;
+    static constexpr uint8_t ROTATION_X_IDX = 0;
+    static constexpr uint8_t ROTATION_Y_IDX = 1;
+    static constexpr uint8_t ROTATION_Z_IDX = 2;
+
     ssize_t mScanSize;
     struct pollfd mPollFdIio;
     std::vector<uint8_t> mSensorRawData;
     int64_t mXMap, mYMap, mZMap;
     bool mXNegate, mYNegate, mZNegate;
+    std::vector<AdditionalInfo> mAdditionalInfoFrames;
 
     HWSensorBase(int32_t sensorHandle, ISensorsEventCallback* callback,
                  const struct iio_device_data& iio_data,
@@ -119,6 +128,10 @@ class HWSensorBase : public SensorBase {
     void setOrientation(std::optional<std::vector<Configuration>> config);
     void processScanData(uint8_t* data, Event* evt);
     void setAxisDefaultValues();
+    status_t setAdditionalInfoFrames(const std::optional<std::vector<Configuration>>& config);
+    void sendAdditionalInfoReport();
+    status_t getSensorPlacement(AdditionalInfo* sensorPlacement,
+                                const std::optional<std::vector<Configuration>>& config);
 };
 }  // namespace implementation
 }  // namespace subhal