Add support for variable sampling period

The new sensor HAL version introduced
variable sampling periods for sensors.
Our host side does not support this. This
change latches values for CONTINUOUS_MODE
sensors and repeats them at the requested
rate.

Bug: 159469598
Test: atest android.hardware.cts.SensorBatchingTests
Change-Id: I0487f148fccab1066df253f66110a7e2539c82f2
Merged-In: I0487f148fccab1066df253f66110a7e2539c82f2

3 files changed, 154 insertions, 46 deletions

diff --git a/sensors/multihal_sensors.cpp b/sensors/multihal_sensors.cpp
index a0bcbcbf..104ff7f6 100644
--- a/sensors/multihal_sensors.cpp
+++ b/sensors/multihal_sensors.cpp
@@ -26,7 +26,9 @@ using ahs21::SensorType;
 using ahs10::SensorFlagBits;
 using ahs10::MetaDataEventType;
 
-MultihalSensors::MultihalSensors() : m_qemuSensorsFd(qemud_channel_open("sensors")) {
+MultihalSensors::MultihalSensors()
+        : m_qemuSensorsFd(qemud_channel_open("sensors"))
+        , m_batchInfo(getSensorNumber()) {
     if (!m_qemuSensorsFd.ok()) {
         ALOGE("%s:%d: m_qemuSensorsFd is not opened", __func__, __LINE__);
         ::abort();
@@ -74,10 +76,16 @@ MultihalSensors::MultihalSensors() : m_qemuSensorsFd(qemud_channel_open("sensors
     }
 
     m_sensorThread = std::thread(&MultihalSensors::qemuSensorListenerThread, this);
+    m_batchThread = std::thread(&MultihalSensors::batchThread, this);
 }
 
 MultihalSensors::~MultihalSensors() {
-    disableAllSensors();
+    setAllQemuSensors(false);
+
+    m_batchRunning = false;
+    m_batchUpdated.notify_one();
+    m_batchThread.join();
+
     qemuSensorThreadSendCommand(kCMD_QUIT);
     m_sensorThread.join();
 }
@@ -107,7 +115,7 @@ Return<void> MultihalSensors::getSensorsList_2_1(getSensorsList_2_1_cb _hidl_cb)
 }
 
 Return<Result> MultihalSensors::setOperationMode(const OperationMode mode) {
-    std::unique_lock<std::mutex> lock(m_apiMtx);
+    std::unique_lock<std::mutex> lock(m_mtx);
 
     if (m_activeSensorsMask) {
         return Result::INVALID_OPERATION;
@@ -123,25 +131,31 @@ Return<Result> MultihalSensors::activate(const int32_t sensorHandle,
         return Result::BAD_VALUE;
     }
 
-    std::unique_lock<std::mutex> lock(m_apiMtx);
+    std::unique_lock<std::mutex> lock(m_mtx);
+    BatchInfo& batchInfo = m_batchInfo[sensorHandle];
 
-    uint32_t newActiveMask;
     if (enabled) {
-        newActiveMask = m_activeSensorsMask | (1u << sensorHandle);
-    } else {
-        newActiveMask = m_activeSensorsMask & ~(1u << sensorHandle);
-    }
-    if (m_activeSensorsMask == newActiveMask) {
-        return Result::OK;
-    }
-
-    if (m_opMode == OperationMode::NORMAL) {
-        if (!activateQemuSensorImpl(m_qemuSensorsFd.get(), sensorHandle, enabled)) {
-            return Result::INVALID_OPERATION;
+        const SensorInfo* sensor = getSensorInfoByHandle(sensorHandle);
+        LOG_ALWAYS_FATAL_IF(!sensor);
+        if (!(sensor->flags & static_cast<uint32_t>(SensorFlagBits::ON_CHANGE_MODE))) {
+            if (batchInfo.samplingPeriodNs <= 0) {
+                return Result::BAD_VALUE;
+            }
+
+            BatchEventRef batchEventRef;
+            batchEventRef.timestamp =
+                ::android::elapsedRealtimeNano() + batchInfo.samplingPeriodNs;
+            batchEventRef.sensorHandle = sensorHandle;
+            batchEventRef.generation = ++batchInfo.generation;
+
+            m_batchQueue.push(batchEventRef);
+            m_batchUpdated.notify_one();
         }
-    }
 
-    m_activeSensorsMask = newActiveMask;
+        m_activeSensorsMask = m_activeSensorsMask | (1u << sensorHandle);
+    } else {
+        m_activeSensorsMask = m_activeSensorsMask & ~(1u << sensorHandle);
+    }
     return Result::OK;
 }
 
@@ -156,11 +170,17 @@ Return<Result> MultihalSensors::batch(const int32_t sensorHandle,
 
     const SensorInfo* sensor = getSensorInfoByHandle(sensorHandle);
     LOG_ALWAYS_FATAL_IF(!sensor);
-    if (samplingPeriodNs >= sensor->minDelay) {
-        return Result::OK;
-    } else {
+
+    if (samplingPeriodNs < sensor->minDelay) {
         return Result::BAD_VALUE;
     }
+
+    std::unique_lock<std::mutex> lock(m_mtx);
+    if (m_opMode == OperationMode::NORMAL) {
+        m_batchInfo[sensorHandle].samplingPeriodNs = samplingPeriodNs;
+    }
+
+    return Result::OK;
 }
 
 Return<Result> MultihalSensors::flush(const int32_t sensorHandle) {
@@ -170,8 +190,9 @@ Return<Result> MultihalSensors::flush(const int32_t sensorHandle) {
 
     const SensorInfo* sensor = getSensorInfoByHandle(sensorHandle);
     LOG_ALWAYS_FATAL_IF(!sensor);
-    std::unique_lock<std::mutex> lock(m_apiMtx);
-    if (!(m_activeSensorsMask & (1u << sensorHandle))) {
+
+    std::unique_lock<std::mutex> lock(m_mtx);
+    if (!isSensorActive(sensorHandle)) {
         return Result::BAD_VALUE;
     }
 
@@ -180,7 +201,7 @@ Return<Result> MultihalSensors::flush(const int32_t sensorHandle) {
     event.sensorType = SensorType::META_DATA;
     event.u.meta.what = MetaDataEventType::META_DATA_FLUSH_COMPLETE;
 
-    postSensorEventLocked(event);
+    doPostSensorEventLocked(*sensor, event);
     return Result::OK;
 }
 
@@ -192,7 +213,7 @@ Return<Result> MultihalSensors::injectSensorData_2_1(const Event& event) {
         return Result::OK;
     }
 
-    std::unique_lock<std::mutex> lock(m_apiMtx);
+    std::unique_lock<std::mutex> lock(m_mtx);
     if (m_opMode != OperationMode::DATA_INJECTION) {
         return Result::INVALID_OPERATION;
     }
@@ -202,27 +223,36 @@ Return<Result> MultihalSensors::injectSensorData_2_1(const Event& event) {
         return Result::BAD_VALUE;
     }
 
-    postSensorEventLocked(event);
+    doPostSensorEventLocked(*sensor, event);
     return Result::OK;
 }
 
 Return<Result> MultihalSensors::initialize(const sp<IHalProxyCallback>& halProxyCallback) {
-    std::unique_lock<std::mutex> lock(m_apiMtx);
-    disableAllSensors();
+    std::unique_lock<std::mutex> lock(m_mtx);
+    setAllQemuSensors(true);   // we need to start sampling sensors for batching
     m_opMode = OperationMode::NORMAL;
     m_halProxyCallback = halProxyCallback;
     return Result::OK;
 }
 
 void MultihalSensors::postSensorEvent(const Event& event) {
-    std::unique_lock<std::mutex> lock(m_apiMtx);
-    postSensorEventLocked(event);
+    const SensorInfo* sensor = getSensorInfoByHandle(event.sensorHandle);
+    LOG_ALWAYS_FATAL_IF(!sensor);
+
+    std::unique_lock<std::mutex> lock(m_mtx);
+    if (sensor->flags & static_cast<uint32_t>(SensorFlagBits::ON_CHANGE_MODE)) {
+        if (isSensorActive(event.sensorHandle)) {
+            doPostSensorEventLocked(*sensor, event);
+        }
+    } else {    // CONTINUOUS_MODE
+        m_batchInfo[event.sensorHandle].event = event;
+    }
 }
 
-void MultihalSensors::postSensorEventLocked(const Event& event) {
+void MultihalSensors::doPostSensorEventLocked(const SensorInfo& sensor,
+                                              const Event& event) {
     const bool isWakeupEvent =
-        getSensorInfoByHandle(event.sensorHandle)->flags &
-        static_cast<uint32_t>(SensorFlagBits::WAKE_UP);
+        sensor.flags & static_cast<uint32_t>(SensorFlagBits::WAKE_UP);
 
     m_halProxyCallback->postEvents(
         {event},
@@ -233,7 +263,7 @@ bool MultihalSensors::qemuSensorThreadSendCommand(const char cmd) const {
     return TEMP_FAILURE_RETRY(write(m_callersFd.get(), &cmd, 1)) == 1;
 }
 
-bool MultihalSensors::isSensorHandleValid(int32_t sensorHandle) const {
+bool MultihalSensors::isSensorHandleValid(int sensorHandle) const {
     if (!goldfish::isSensorHandleValid(sensorHandle)) {
         return false;
     }
@@ -245,6 +275,53 @@ bool MultihalSensors::isSensorHandleValid(int32_t sensorHandle) const {
     return true;
 }
 
+void MultihalSensors::batchThread() {
+    using high_resolution_clock = std::chrono::high_resolution_clock;
+
+    while (m_batchRunning) {
+        std::unique_lock<std::mutex> lock(m_mtx);
+        if (m_batchQueue.empty()) {
+            m_batchUpdated.wait(lock);
+        } else {
+            const int64_t t = m_batchQueue.top().timestamp;
+            const auto d = std::chrono::nanoseconds(t);
+            high_resolution_clock::time_point waitUntil(d);
+            m_batchUpdated.wait_until(lock, waitUntil);
+        }
+
+        const int64_t nowNs = ::android::elapsedRealtimeNano();
+        while (!m_batchQueue.empty() && (nowNs >= m_batchQueue.top().timestamp)) {
+            BatchEventRef evRef = m_batchQueue.top();
+            m_batchQueue.pop();
+
+            const int sensorHandle = evRef.sensorHandle;
+            LOG_ALWAYS_FATAL_IF(!goldfish::isSensorHandleValid(sensorHandle));
+            if (!isSensorActive(sensorHandle)) {
+                continue;
+            }
+
+            BatchInfo &batchInfo = m_batchInfo[sensorHandle];
+            if (batchInfo.event.sensorType == SensorType::META_DATA) {
+                ALOGW("%s:%d the host has not provided value yet for sensorHandle=%d",
+                      __func__, __LINE__, sensorHandle);
+            } else {
+                batchInfo.event.timestamp = evRef.timestamp;
+                const SensorInfo* sensor = getSensorInfoByHandle(sensorHandle);
+                LOG_ALWAYS_FATAL_IF(!sensor);
+                doPostSensorEventLocked(*sensor, batchInfo.event);
+            }
+
+            if (evRef.generation == batchInfo.generation) {
+                const int64_t samplingPeriodNs = batchInfo.samplingPeriodNs;
+                LOG_ALWAYS_FATAL_IF(samplingPeriodNs <= 0);
+
+                evRef.timestamp += samplingPeriodNs;
+                m_batchQueue.push(evRef);
+            }
+        }
+    }
+}
+
 /// not supported //////////////////////////////////////////////////////////////
 Return<void> MultihalSensors::registerDirectChannel(const SharedMemInfo& mem,
                                                     registerDirectChannel_cb _hidl_cb) {
diff --git a/sensors/multihal_sensors.h b/sensors/multihal_sensors.h
index abe49dc5..382af7f7 100644
--- a/sensors/multihal_sensors.h
+++ b/sensors/multihal_sensors.h
@@ -17,8 +17,12 @@
 #pragma once
 #include <android-base/unique_fd.h>
 #include <V2_1/SubHal.h>
+#include <atomic>
+#include <condition_variable>
 #include <cstdint>
+#include <queue>
 #include <thread>
+#include <vector>
 
 namespace goldfish {
 namespace ahs = ::android::hardware::sensors;
@@ -26,6 +30,7 @@ namespace ahs21 = ahs::V2_1;
 namespace ahs10 = ahs::V1_0;
 
 using ahs21::implementation::IHalProxyCallback;
+using ahs21::SensorInfo;
 using ahs21::Event;
 using ahs10::OperationMode;
 using ahs10::RateLevel;
@@ -82,13 +87,17 @@ private:
     };
 
     bool isSensorHandleValid(int sensorHandle) const;
+    bool isSensorActive(int sensorHandle) const {
+        return m_activeSensorsMask & (1u << sensorHandle);  // m_mtx required
+    }
     static bool activateQemuSensorImpl(int pipe, int sensorHandle, bool enabled);
-    bool disableAllSensors();
+    bool setAllQemuSensors(bool enabled);
     void parseQemuSensorEvent(const int pipe, QemuSensorsProtocolState* state);
     void postSensorEvent(const Event& event);
-    void postSensorEventLocked(const Event& event);
+    void doPostSensorEventLocked(const SensorInfo& sensor, const Event& event);
 
     void qemuSensorListenerThread();
+    void batchThread();
 
     static constexpr char kCMD_QUIT = 'q';
     bool qemuSensorThreadSendCommand(char cmd) const;
@@ -105,7 +114,32 @@ private:
     uint32_t                m_activeSensorsMask = 0;
     OperationMode           m_opMode = OperationMode::NORMAL;
     sp<IHalProxyCallback>   m_halProxyCallback;
-    mutable std::mutex      m_apiMtx;
+
+    // batching
+    struct BatchEventRef {
+        int64_t  timestamp = -1;
+        int      sensorHandle = -1;
+        int      generation = 0;
+
+        bool operator<(const BatchEventRef &rhs) const {
+            // not a typo, we want m_batchQueue.top() to be the smallest timestamp
+            return timestamp > rhs.timestamp;
+        }
+    };
+
+    struct BatchInfo {
+        Event       event;
+        int64_t     samplingPeriodNs = 0;
+        int         generation = 0;
+    };
+
+    std::priority_queue<BatchEventRef>      m_batchQueue;
+    std::vector<BatchInfo>                  m_batchInfo;
+    std::condition_variable                 m_batchUpdated;
+    std::thread                             m_batchThread;
+    std::atomic<bool>                       m_batchRunning = true;
+
+    mutable std::mutex                      m_mtx;
 };
 
 }  // namespace goldfish
diff --git a/sensors/multihal_sensors_qemu.cpp b/sensors/multihal_sensors_qemu.cpp
index a07d569b..f5da3a15 100644
--- a/sensors/multihal_sensors_qemu.cpp
+++ b/sensors/multihal_sensors_qemu.cpp
@@ -69,19 +69,16 @@ bool MultihalSensors::activateQemuSensorImpl(const int pipe,
     }
 }
 
-bool MultihalSensors::disableAllSensors() {
-    if (m_opMode == OperationMode::NORMAL) {
-        uint32_t mask = m_activeSensorsMask;
-        for (int i = 0; mask; ++i, mask >>= 1) {
-            if (mask & 1) {
-                if (!activateQemuSensorImpl(m_qemuSensorsFd.get(), i, false)) {
-                    return false;
-                }
+bool MultihalSensors::setAllQemuSensors(const bool enabled) {
+    uint32_t mask = m_availableSensorsMask;
+    for (int i = 0; mask; ++i, mask >>= 1) {
+        if (mask & 1) {
+            if (!activateQemuSensorImpl(m_qemuSensorsFd.get(), i, enabled)) {
+                return false;
             }
         }
     }
 
-    m_activeSensorsMask = 0;
     return true;
 }