1 files changed, 73 insertions, 10 deletions

diff --git a/sensors/2.0/default/Sensor.cpp b/sensors/2.0/default/Sensor.cpp
index f4c9b57520..1691b3e7a2 100644
--- a/sensors/2.0/default/Sensor.cpp
+++ b/sensors/2.0/default/Sensor.cpp
@@ -16,35 +16,98 @@
 
 #include "Sensor.h"
 
+#include <utils/SystemClock.h>
+
 namespace android {
 namespace hardware {
 namespace sensors {
 namespace V2_0 {
 namespace implementation {
 
-Sensor::Sensor() : mIsEnabled(false), mSamplingPeriodNs(0) {}
+using ::android::hardware::sensors::V1_0::SensorStatus;
+
+Sensor::Sensor(ISensorsEventCallback* callback)
+    : mIsEnabled(false), mSamplingPeriodNs(0), mLastSampleTimeNs(0), mCallback(callback) {
+    mRunThread = std::thread(startThread, this);
+}
+
+Sensor::~Sensor() {
+    mStopThread = true;
+    mIsEnabled = false;
+    mWaitCV.notify_all();
+    mRunThread.join();
+}
 
 const SensorInfo& Sensor::getSensorInfo() const {
     return mSensorInfo;
 }
 
-bool Sensor::batch(int32_t samplingPeriodNs) {
-    bool success = true;
-    if (samplingPeriodNs >= mSensorInfo.minDelay * 1000 &&
-        samplingPeriodNs <= mSensorInfo.maxDelay * 1000) {
+void Sensor::batch(int32_t samplingPeriodNs) {
+    if (samplingPeriodNs < mSensorInfo.minDelay * 1000) {
+        samplingPeriodNs = mSensorInfo.minDelay * 1000;
+    } else if (samplingPeriodNs > mSensorInfo.maxDelay * 1000) {
+        samplingPeriodNs = mSensorInfo.maxDelay * 1000;
+    }
+
+    if (mSamplingPeriodNs != samplingPeriodNs) {
         mSamplingPeriodNs = samplingPeriodNs;
-    } else {
-        success = false;
+        // Wake up the 'run' thread to check if a new event should be generated now
+        mWaitCV.notify_all();
     }
-    return success;
 }
 
 void Sensor::activate(bool enable) {
-    mIsEnabled = enable;
+    if (mIsEnabled != enable) {
+        mIsEnabled = enable;
+        mWaitCV.notify_all();
+    }
+}
+
+void Sensor::startThread(Sensor* sensor) {
+    sensor->run();
+}
+
+void Sensor::run() {
+    std::mutex runMutex;
+    std::unique_lock<std::mutex> runLock(runMutex);
+    constexpr int64_t kNanosecondsInSeconds = 1000 * 1000 * 1000;
+
+    while (!mStopThread) {
+        if (!mIsEnabled) {
+            mWaitCV.wait(runLock, [&] { return mIsEnabled || mStopThread; });
+        } else {
+            timespec curTime;
+            clock_gettime(CLOCK_REALTIME, &curTime);
+            int64_t now = (curTime.tv_sec * kNanosecondsInSeconds) + curTime.tv_nsec;
+            int64_t nextSampleTime = mLastSampleTimeNs + mSamplingPeriodNs;
+
+            if (now >= nextSampleTime) {
+                mLastSampleTimeNs = now;
+                nextSampleTime = mLastSampleTimeNs + mSamplingPeriodNs;
+                mCallback->postEvents(readEvents());
+            }
+
+            mWaitCV.wait_for(runLock, std::chrono::nanoseconds(nextSampleTime - now));
+        }
+    }
+}
+
+std::vector<Event> Sensor::readEvents() {
+    std::vector<Event> events;
+    Event event;
+    event.sensorHandle = mSensorInfo.sensorHandle;
+    event.sensorType = mSensorInfo.type;
+    event.timestamp = ::android::elapsedRealtimeNano();
+    event.u.vec3.x = 1;
+    event.u.vec3.y = 2;
+    event.u.vec3.z = 3;
+    event.u.vec3.status = SensorStatus::ACCURACY_HIGH;
+    events.push_back(event);
+    return events;
 }
 
 }  // namespace implementation
 }  // namespace V2_0
 }  // namespace sensors
 }  // namespace hardware
-}  // namespace android
\ No newline at end of file
+}  // namespace android