Merge changes from topic "provide events > 1.5 Hz" into rvc-qpr-devandroid-11.0.0_r46android-11.0.0_r43android-11.0.0_r40android-11.0.0_r39android-11.0.0_r38android-11.0.0_r37android-11.0.0_r36android-11.0.0_r35android-11.0.0_r34android-11.0.0_r33android-11.0.0_r32android-11.0.0_r30android11-qpr3-s1-releaseandroid11-qpr3-releaseandroid11-qpr2-releaseandroid11-qpr1-c-release

* changes:
  Add support for variable sampling period
  Add minor improvements in sensors
  Set minDelay and maxDelay to 0 for ON_CHANGE_MODE sensors
  Check if sensorHandle refers to the sensor that is available
  Report the sensors available from goldfish sensor HAL

5 files changed, 199 insertions, 77 deletions

diff --git a/sensors/multihal_sensors.cpp b/sensors/multihal_sensors.cpp
index 3502add9..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();
@@ -56,13 +58,16 @@ MultihalSensors::MultihalSensors() : m_qemuSensorsFd(qemud_channel_open("sensors
         ::abort();
     }
     buffer[len] = 0;
-    uint32_t availableSensorsMask = 0;
-    if (sscanf(buffer, "%u", &availableSensorsMask) != 1) {
+    uint32_t hostSensorsMask = 0;
+    if (sscanf(buffer, "%u", &hostSensorsMask) != 1) {
         ALOGE("%s:%d: Can't parse qemud response", __func__, __LINE__);
         ::abort();
     }
     m_availableSensorsMask =
-        availableSensorsMask & ((1u << getSensorNumber()) - 1);
+        hostSensorsMask & ((1u << getSensorNumber()) - 1);
+
+    ALOGI("%s:%d: host sensors mask=%x, available sensors mask=%x",
+          __func__, __LINE__, hostSensorsMask, m_availableSensorsMask);
 
     if (!::android::base::Socketpair(AF_LOCAL, SOCK_STREAM, 0,
                                      &m_callersFd, &m_sensorThreadFd)) {
@@ -70,11 +75,17 @@ MultihalSensors::MultihalSensors() : m_qemuSensorsFd(qemud_channel_open("sensors
         ::abort();
     }
 
-    m_sensorThread = std::thread(qemuSensorListenerThreadStart, this);
+    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();
 }
@@ -104,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;
@@ -120,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;
 }
 
@@ -147,26 +164,35 @@ Return<Result> MultihalSensors::batch(const int32_t sensorHandle,
                                       const int64_t maxReportLatencyNs) {
     (void)maxReportLatencyNs;
 
+    if (!isSensorHandleValid(sensorHandle)) {
+        return Result::BAD_VALUE;
+    }
+
     const SensorInfo* sensor = getSensorInfoByHandle(sensorHandle);
-    if (sensor) {
-        if (samplingPeriodNs >= sensor->minDelay) {
-            return Result::OK;
-        } else {
-            return Result::BAD_VALUE;
-        }
-    } else {
+    LOG_ALWAYS_FATAL_IF(!sensor);
+
+    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) {
-    const SensorInfo* sensor = getSensorInfoByHandle(sensorHandle);
-    if (!sensor) {
+    if (!isSensorHandleValid(sensorHandle)) {
         return Result::BAD_VALUE;
     }
 
-    std::unique_lock<std::mutex> lock(m_apiMtx);
-    if (!(m_activeSensorsMask & (1u << sensorHandle))) {
+    const SensorInfo* sensor = getSensorInfoByHandle(sensorHandle);
+    LOG_ALWAYS_FATAL_IF(!sensor);
+
+    std::unique_lock<std::mutex> lock(m_mtx);
+    if (!isSensorActive(sensorHandle)) {
         return Result::BAD_VALUE;
     }
 
@@ -175,48 +201,58 @@ 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;
 }
 
 Return<Result> MultihalSensors::injectSensorData_2_1(const Event& event) {
+    if (!isSensorHandleValid(event.sensorHandle)) {
+        return Result::BAD_VALUE;
+    }
     if (event.sensorType == SensorType::ADDITIONAL_INFO) {
         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;
     }
     const SensorInfo* sensor = getSensorInfoByHandle(event.sensorHandle);
-    if (!sensor) {
-        return Result::BAD_VALUE;
-    }
+    LOG_ALWAYS_FATAL_IF(!sensor);
     if (sensor->type != event.sensorType) {
         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},
@@ -227,6 +263,65 @@ bool MultihalSensors::qemuSensorThreadSendCommand(const char cmd) const {
     return TEMP_FAILURE_RETRY(write(m_callersFd.get(), &cmd, 1)) == 1;
 }
 
+bool MultihalSensors::isSensorHandleValid(int sensorHandle) const {
+    if (!goldfish::isSensorHandleValid(sensorHandle)) {
+        return false;
+    }
+
+    if (!(m_availableSensorsMask & (1u << sensorHandle))) {
+        return false;
+    }
+
+    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 80ed6a3b..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;
@@ -81,14 +86,18 @@ private:
         float lastHingeAngle2Value = kSensorNoValue;
     };
 
+    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();
-    static void qemuSensorListenerThreadStart(MultihalSensors* that);
+    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_epoll.cpp b/sensors/multihal_sensors_epoll.cpp
index 0be12245..cf2ae56b 100644
--- a/sensors/multihal_sensors_epoll.cpp
+++ b/sensors/multihal_sensors_epoll.cpp
@@ -38,10 +38,6 @@ int qemuSensortThreadRcvCommand(const int fd) {
 }
 }  // namespace
 
-void MultihalSensors::qemuSensorListenerThreadStart(MultihalSensors* that) {
-    that->qemuSensorListenerThread();
-}
-
 void MultihalSensors::qemuSensorListenerThread() {
     const unique_fd epollFd(epoll_create1(0));
     if (!epollFd.ok()) {
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;
 }
 
diff --git a/sensors/sensor_list.cpp b/sensors/sensor_list.cpp
index 245379fa..d228ba62 100644
--- a/sensors/sensor_list.cpp
+++ b/sensors/sensor_list.cpp
@@ -122,11 +122,11 @@ const SensorInfo kAllSensors[] = {
         .maxRange = 80.0,
         .resolution = 1.0,
         .power = 0.0,
-        .minDelay = 10000,
+        .minDelay = 0,
         .fifoReservedEventCount = 0,
         .fifoMaxEventCount = 0,
         .requiredPermission = "",
-        .maxDelay = 500000,
+        .maxDelay = 0,
         .flags = SensorFlagBits::DATA_INJECTION |
                  SensorFlagBits::ON_CHANGE_MODE
     },
@@ -140,11 +140,11 @@ const SensorInfo kAllSensors[] = {
         .maxRange = 1.0,
         .resolution = 1.0,
         .power = 20.0,
-        .minDelay = 10000,
+        .minDelay = 0,
         .fifoReservedEventCount = 0,
         .fifoMaxEventCount = 0,
         .requiredPermission = "",
-        .maxDelay = 500000,
+        .maxDelay = 0,
         .flags = SensorFlagBits::DATA_INJECTION |
                  SensorFlagBits::ON_CHANGE_MODE |
                  SensorFlagBits::WAKE_UP
@@ -159,11 +159,11 @@ const SensorInfo kAllSensors[] = {
         .maxRange = 40000.0,
         .resolution = 1.0,
         .power = 20.0,
-        .minDelay = 10000,
+        .minDelay = 0,
         .fifoReservedEventCount = 0,
         .fifoMaxEventCount = 0,
         .requiredPermission = "",
-        .maxDelay = 500000,
+        .maxDelay = 0,
         .flags = SensorFlagBits::DATA_INJECTION |
                  SensorFlagBits::ON_CHANGE_MODE
     },
@@ -195,11 +195,11 @@ const SensorInfo kAllSensors[] = {
         .maxRange = 100.0,
         .resolution = 1.0,
         .power = 20.0,
-        .minDelay = 10000,
+        .minDelay = 0,
         .fifoReservedEventCount = 0,
         .fifoMaxEventCount = 0,
         .requiredPermission = "",
-        .maxDelay = 500000,
+        .maxDelay = 0,
         .flags = SensorFlagBits::DATA_INJECTION |
                  SensorFlagBits::ON_CHANGE_MODE
     },
@@ -248,11 +248,11 @@ const SensorInfo kAllSensors[] = {
         .maxRange = 360,
         .resolution = 1.0,
         .power = 3.0,
-        .minDelay = 10000,
+        .minDelay = 0,
         .fifoReservedEventCount = 0,
         .fifoMaxEventCount = 0,
         .requiredPermission = "",
-        .maxDelay = 500000,
+        .maxDelay = 0,
         .flags = SensorFlagBits::DATA_INJECTION |
                  SensorFlagBits::ON_CHANGE_MODE |
                  SensorFlagBits::WAKE_UP
@@ -267,11 +267,11 @@ const SensorInfo kAllSensors[] = {
         .maxRange = 360,
         .resolution = 1.0,
         .power = 3.0,
-        .minDelay = 10000,
+        .minDelay = 0,
         .fifoReservedEventCount = 0,
         .fifoMaxEventCount = 0,
         .requiredPermission = "",
-        .maxDelay = 500000,
+        .maxDelay = 0,
         .flags = SensorFlagBits::DATA_INJECTION |
                  SensorFlagBits::ON_CHANGE_MODE |
                  SensorFlagBits::WAKE_UP
@@ -286,11 +286,11 @@ const SensorInfo kAllSensors[] = {
         .maxRange = 360,
         .resolution = 1.0,
         .power = 3.0,
-        .minDelay = 10000,
+        .minDelay = 0,
         .fifoReservedEventCount = 0,
         .fifoMaxEventCount = 0,
         .requiredPermission = "",
-        .maxDelay = 500000,
+        .maxDelay = 0,
         .flags = SensorFlagBits::DATA_INJECTION |
                  SensorFlagBits::ON_CHANGE_MODE |
                  SensorFlagBits::WAKE_UP