release-request-c924aaac-f0a2-4215-8dc4-e314f22460d9-for-git_oc-mr1-release-4301796 snap-temp-L23200000097143969

Change-Id: I4c958bd4b7b999502074782752420833e6dbaa49

6 files changed, 303 insertions, 109 deletions

diff --git a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
index 61c3d23a..e8d88422 100644
--- a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
+++ b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
@@ -107,7 +107,7 @@
 #define DBG_WM_CALC               0
 #define TIMESTAMP_DBG             0
 
-#define BMI160_APP_VERSION 15
+#define BMI160_APP_VERSION 16
 
 // fixme: to list required definitions for a slave mag
 #ifdef USE_BMM150
@@ -4203,10 +4203,10 @@ static size_t calcFifoSize(const int* iPeriod, const int* iLatency, const int* f
     for (i = 0; i < n; i++) {
         if (iPeriod[i] > 0) {
             anyActive = true;
-            size_t t =  minLatency / iPeriod[i];
+            size_t t = minLatency / iPeriod[i];
             head = t > head ? t : head;
             s += t * factor[i];
-            DEBUG_PRINT_IF(DBG_WM_CALC, "cfifo: %d, s+= %d*%d, head = %d", i, t, factor[i], head);
+            DEBUG_PRINT_IF(DBG_WM_CALC, "cfifo %d: s += %d * %d, head = %d", i, t, factor[i], head);
         }
     }
 
@@ -4216,7 +4216,7 @@ static size_t calcFifoSize(const int* iPeriod, const int* iLatency, const int* f
 /**
  * Calculate the watermark setting from sensor registration information
  *
- * It is assumed  that all sensor period share a common denominator (true for BMI160) and the
+ * It is assumed that all sensor periods share a common denominator (true for BMI160) and the
  * latency of sensor will be lower bounded by its sampling period.
  *
  * @return watermark register setting
@@ -4228,12 +4228,12 @@ static uint8_t calcWatermark2_(TASK) {
     int i;
 
     for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; ++i) {
-        if (T(sensors[i]).configed) {
+        if (T(sensors[i]).configed && T(sensors[i]).latency != SENSOR_LATENCY_NODATA) {
             period[i - ACC] = SENSOR_HZ((float)WATERMARK_MAX_SENSOR_RATE) / T(sensors[i]).rate;
             latency[i - ACC] = U64_DIV_BY_U64_CONSTANT(
                     T(sensors[i]).latency + WATERMARK_TIME_UNIT_NS/2, WATERMARK_TIME_UNIT_NS);
-            DEBUG_PRINT_IF(DBG_WM_CALC, "cwm2: f %dHz, l %dus => T %d unit, L %d unit",
-                    (int) T(sensors[i]).rate/1024,
+            DEBUG_PRINT_IF(DBG_WM_CALC, "cwm2 %d: f %dHz, l %dus => T %d unit, L %d unit",
+                    i, (int) T(sensors[i]).rate/1024,
                     (int) U64_DIV_BY_U64_CONSTANT(T(sensors[i]).latency, 1000),
                     period[i-ACC], latency[i-ACC]);
         }
diff --git a/sensorhal/directchannel.cpp b/sensorhal/directchannel.cpp
index 8d87782d..b0df24c8 100644
--- a/sensorhal/directchannel.cpp
+++ b/sensorhal/directchannel.cpp
@@ -74,6 +74,10 @@ AshmemDirectChannel::~AshmemDirectChannel() {
     ::close(mAshmemFd);
 }
 
+bool AshmemDirectChannel::memoryMatches(const struct sensors_direct_mem_t * /*mem*/) const {
+    return false;
+}
+
 ANDROID_SINGLETON_STATIC_INSTANCE(GrallocHalWrapper);
 
 GrallocHalWrapper::GrallocHalWrapper()
@@ -128,10 +132,15 @@ GrallocHalWrapper::GrallocHalWrapper()
                                                     GRALLOC1_FUNCTION_LOCK));
             mPfnUnlock = (GRALLOC1_PFN_UNLOCK)(mGralloc1Device->getFunction(mGralloc1Device,
                                                       GRALLOC1_FUNCTION_UNLOCK));
+            mPfnGetBackingStore = (GRALLOC1_PFN_GET_BACKING_STORE)
+                    (mGralloc1Device->getFunction(mGralloc1Device,
+                                                  GRALLOC1_FUNCTION_GET_BACKING_STORE));
             if (mPfnRetain == nullptr || mPfnRelease == nullptr
-                    || mPfnLock == nullptr || mPfnUnlock == nullptr) {
-                ALOGE("Function pointer for retain, release, lock and unlock are %p, %p, %p, %p",
-                      mPfnRetain, mPfnRelease, mPfnLock, mPfnUnlock);
+                    || mPfnLock == nullptr || mPfnUnlock == nullptr
+                    || mPfnGetBackingStore == nullptr) {
+                ALOGE("Function pointer for retain, release, lock, unlock and getBackingStore are "
+                      "%p, %p, %p, %p, %p",
+                      mPfnRetain, mPfnRelease, mPfnLock, mPfnUnlock, mPfnGetBackingStore);
                 err = BAD_VALUE;
                 break;
             }
@@ -223,6 +232,21 @@ int GrallocHalWrapper::unlock(const native_handle_t *handle) {
     }
 }
 
+bool GrallocHalWrapper::isSameMemory(const native_handle_t *h1, const native_handle_t *h2) {
+    switch (mVersion) {
+        case 0:
+            return false; // version 1.0 cannot compare two memory
+        case 1: {
+            gralloc1_backing_store_t s1, s2;
+
+            return mPfnGetBackingStore(mGralloc1Device, h1, &s1) == GRALLOC1_ERROR_NONE
+                    && mPfnGetBackingStore(mGralloc1Device, h2, &s2) == GRALLOC1_ERROR_NONE
+                    && s1 == s2;
+        }
+    }
+    return false;
+}
+
 int GrallocHalWrapper::mapGralloc1Error(int grallocError) {
     switch (grallocError) {
         case GRALLOC1_ERROR_NONE:
@@ -302,4 +326,9 @@ GrallocDirectChannel::~GrallocDirectChannel() {
     }
 }
 
+bool GrallocDirectChannel::memoryMatches(const struct sensors_direct_mem_t *mem) const {
+    return mem->type == SENSOR_DIRECT_MEM_TYPE_GRALLOC &&
+            GrallocHalWrapper::getInstance().isSameMemory(mem->handle, mNativeHandle);
+}
+
 } // namespace android
diff --git a/sensorhal/directchannel.h b/sensorhal/directchannel.h
index 4842ffcc..70d86456 100644
--- a/sensorhal/directchannel.h
+++ b/sensorhal/directchannel.h
@@ -31,6 +31,7 @@ class DirectChannelBase {
 public:
     DirectChannelBase() : mError(NO_INIT), mSize(0), mBase(nullptr) { }
     virtual ~DirectChannelBase() {}
+    virtual bool memoryMatches(const struct sensors_direct_mem_t *mem) const = 0;
 
     bool isValid();
     int getError();
@@ -47,7 +48,8 @@ protected:
 class AshmemDirectChannel : public DirectChannelBase {
 public:
     AshmemDirectChannel(const struct sensors_direct_mem_t *mem);
-    virtual ~AshmemDirectChannel();
+    ~AshmemDirectChannel() override;
+    bool memoryMatches(const struct sensors_direct_mem_t *mem) const override;
 private:
     int mAshmemFd;
 };
@@ -58,6 +60,7 @@ public:
     int unregisterBuffer(const native_handle_t *handle);
     int lock(const native_handle_t *handle, int usage, int l, int t, int w, int h, void **vaddr);
     int unlock(const native_handle_t *handle);
+    bool isSameMemory(const native_handle_t *h1, const native_handle_t *h2);
     bool unregisterImplyDelete() { return mUnregisterImplyDelete; }
 private:
     friend class Singleton<GrallocHalWrapper>;
@@ -77,13 +80,15 @@ private:
     GRALLOC1_PFN_RELEASE mPfnRelease;
     GRALLOC1_PFN_LOCK mPfnLock;
     GRALLOC1_PFN_UNLOCK mPfnUnlock;
+    GRALLOC1_PFN_GET_BACKING_STORE mPfnGetBackingStore;
     bool mUnregisterImplyDelete;
 };
 
 class GrallocDirectChannel : public DirectChannelBase {
 public:
     GrallocDirectChannel(const struct sensors_direct_mem_t *mem);
-    virtual ~GrallocDirectChannel();
+    ~GrallocDirectChannel() override;
+    bool memoryMatches(const struct sensors_direct_mem_t *mem) const override;
 private:
     native_handle_t *mNativeHandle;
 };
diff --git a/sensorhal/hubconnection.cpp b/sensorhal/hubconnection.cpp
index 42366c47..01b95df4 100644
--- a/sensorhal/hubconnection.cpp
+++ b/sensorhal/hubconnection.cpp
@@ -277,9 +277,18 @@ HubConnection::HubConnection()
 
 #ifdef DIRECT_REPORT_ENABLED
     mDirectChannelHandle = 1;
-    mSensorToChannel.emplace(COMMS_SENSOR_ACCEL, std::unordered_map<int32_t, int32_t>());
-    mSensorToChannel.emplace(COMMS_SENSOR_GYRO, std::unordered_map<int32_t, int32_t>());
-    mSensorToChannel.emplace(COMMS_SENSOR_MAG, std::unordered_map<int32_t, int32_t>());
+    mSensorToChannel.emplace(COMMS_SENSOR_ACCEL,
+                             std::unordered_map<int32_t, DirectChannelTimingInfo>());
+    mSensorToChannel.emplace(COMMS_SENSOR_GYRO,
+                             std::unordered_map<int32_t, DirectChannelTimingInfo>());
+    mSensorToChannel.emplace(COMMS_SENSOR_MAG,
+                             std::unordered_map<int32_t, DirectChannelTimingInfo>());
+    mSensorToChannel.emplace(COMMS_SENSOR_ACCEL_UNCALIBRATED,
+                             std::unordered_map<int32_t, DirectChannelTimingInfo>());
+    mSensorToChannel.emplace(COMMS_SENSOR_GYRO_UNCALIBRATED,
+                             std::unordered_map<int32_t, DirectChannelTimingInfo>());
+    mSensorToChannel.emplace(COMMS_SENSOR_MAG_UNCALIBRATED,
+                             std::unordered_map<int32_t, DirectChannelTimingInfo>());
 #endif // DIRECT_REPORT_ENABLED
 }
 
@@ -736,25 +745,30 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se
         sv->y = sample->iy * mScaleAccel;
         sv->z = sample->iz * mScaleAccel;
         sv->status = SENSOR_STATUS_ACCURACY_HIGH;
-        sendDirectReportEvent(&nev[cnt], 1);
 
-        if (mSensorState[sensor].enable) {
+        sendDirectReportEvent(&nev[cnt], 1);
+        if (mSensorState[sensor].enable && isSampleIntervalSatisfied(sensor, timestamp)) {
             ++cnt;
         }
 
-        if (mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].enable) {
-            ue = &initEv(&nev[cnt++], timestamp,
-                SENSOR_TYPE_ACCELEROMETER_UNCALIBRATED,
-                COMMS_SENSOR_ACCEL_UNCALIBRATED)->uncalibrated_accelerometer;
-            ue->x_uncalib = sample->ix * mScaleAccel + mAccelBias[0];
-            ue->y_uncalib = sample->iy * mScaleAccel + mAccelBias[1];
-            ue->z_uncalib = sample->iz * mScaleAccel + mAccelBias[2];
-            ue->x_bias = mAccelBias[0];
-            ue->y_bias = mAccelBias[1];
-            ue->z_bias = mAccelBias[2];
+        ue = &initEv(&nev[cnt], timestamp,
+            SENSOR_TYPE_ACCELEROMETER_UNCALIBRATED,
+            COMMS_SENSOR_ACCEL_UNCALIBRATED)->uncalibrated_accelerometer;
+        ue->x_uncalib = sample->ix * mScaleAccel + mAccelBias[0];
+        ue->y_uncalib = sample->iy * mScaleAccel + mAccelBias[1];
+        ue->z_uncalib = sample->iz * mScaleAccel + mAccelBias[2];
+        ue->x_bias = mAccelBias[0];
+        ue->y_bias = mAccelBias[1];
+        ue->z_bias = mAccelBias[2];
+
+        sendDirectReportEvent(&nev[cnt], 1);
+        if (mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].enable
+                && isSampleIntervalSatisfied(COMMS_SENSOR_ACCEL_UNCALIBRATED, timestamp)) {
+            ++cnt;
         }
 
-        if (mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].enable) {
+        if (mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].enable
+                && isSampleIntervalSatisfied(COMMS_SENSOR_ACCEL_WRIST_AWARE, timestamp)) {
             sv = &initEv(&nev[cnt++], timestamp,
                 SENSOR_TYPE_ACCELEROMETER_WRIST_AWARE,
                 COMMS_SENSOR_ACCEL_WRIST_AWARE)->acceleration;
@@ -770,22 +784,26 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se
         sv->y = sample->iy * mScaleMag;
         sv->z = sample->iz * mScaleMag;
         sv->status = magAccuracyUpdate(sv);
-        sendDirectReportEvent(&nev[cnt], 1);
 
-        if (mSensorState[sensor].enable) {
+        sendDirectReportEvent(&nev[cnt], 1);
+        if (mSensorState[sensor].enable && isSampleIntervalSatisfied(sensor, timestamp)) {
             ++cnt;
         }
 
-        if (mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].enable) {
-            ue = &initEv(&nev[cnt++], timestamp,
-                SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED,
-                COMMS_SENSOR_MAG_UNCALIBRATED)->uncalibrated_magnetic;
-            ue->x_uncalib = sample->ix * mScaleMag + mMagBias[0];
-            ue->y_uncalib = sample->iy * mScaleMag + mMagBias[1];
-            ue->z_uncalib = sample->iz * mScaleMag + mMagBias[2];
-            ue->x_bias = mMagBias[0];
-            ue->y_bias = mMagBias[1];
-            ue->z_bias = mMagBias[2];
+        ue = &initEv(&nev[cnt], timestamp,
+            SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED,
+            COMMS_SENSOR_MAG_UNCALIBRATED)->uncalibrated_magnetic;
+        ue->x_uncalib = sample->ix * mScaleMag + mMagBias[0];
+        ue->y_uncalib = sample->iy * mScaleMag + mMagBias[1];
+        ue->z_uncalib = sample->iz * mScaleMag + mMagBias[2];
+        ue->x_bias = mMagBias[0];
+        ue->y_bias = mMagBias[1];
+        ue->z_bias = mMagBias[2];
+
+        sendDirectReportEvent(&nev[cnt], 1);
+        if (mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].enable
+                && isSampleIntervalSatisfied(COMMS_SENSOR_MAG_UNCALIBRATED, timestamp)) {
+            ++cnt;
         }
     default:
         break;
@@ -815,32 +833,38 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se
         sv->y = sample->y;
         sv->z = sample->z;
         sv->status = SENSOR_STATUS_ACCURACY_HIGH;
-        sendDirectReportEvent(&nev[cnt], 1);
 
-        if (mSensorState[sensor].enable) {
+        sendDirectReportEvent(&nev[cnt], 1);
+        if (mSensorState[sensor].enable && isSampleIntervalSatisfied(sensor, timestamp)) {
             ++cnt;
         }
 
-        if (mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].enable) {
-            ue = &initEv(&nev[cnt++], timestamp,
-                SENSOR_TYPE_ACCELEROMETER_UNCALIBRATED,
-                COMMS_SENSOR_ACCEL_UNCALIBRATED)->uncalibrated_accelerometer;
-            ue->x_uncalib = sample->x + mAccelBias[0];
-            ue->y_uncalib = sample->y + mAccelBias[1];
-            ue->z_uncalib = sample->z + mAccelBias[2];
-            ue->x_bias = mAccelBias[0];
-            ue->y_bias = mAccelBias[1];
-            ue->z_bias = mAccelBias[2];
+        ue = &initEv(&nev[cnt], timestamp,
+            SENSOR_TYPE_ACCELEROMETER_UNCALIBRATED,
+            COMMS_SENSOR_ACCEL_UNCALIBRATED)->uncalibrated_accelerometer;
+        ue->x_uncalib = sample->x + mAccelBias[0];
+        ue->y_uncalib = sample->y + mAccelBias[1];
+        ue->z_uncalib = sample->z + mAccelBias[2];
+        ue->x_bias = mAccelBias[0];
+        ue->y_bias = mAccelBias[1];
+        ue->z_bias = mAccelBias[2];
+
+        sendDirectReportEvent(&nev[cnt], 1);
+        if (mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].enable
+                && isSampleIntervalSatisfied(COMMS_SENSOR_ACCEL_UNCALIBRATED, timestamp)) {
+            ++cnt;
         }
 
-        if (mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].enable) {
-            sv = &initEv(&nev[cnt++], timestamp,
+        if (mSensorState[COMMS_SENSOR_ACCEL_WRIST_AWARE].enable
+                && isSampleIntervalSatisfied(COMMS_SENSOR_ACCEL_WRIST_AWARE, timestamp)) {
+            sv = &initEv(&nev[cnt], timestamp,
                 SENSOR_TYPE_ACCELEROMETER_WRIST_AWARE,
                 COMMS_SENSOR_ACCEL_WRIST_AWARE)->acceleration;
             sv->x = sample->x;
             sv->y = (mLefty.accel ? -sample->y : sample->y);
             sv->z = sample->z;
             sv->status = SENSOR_STATUS_ACCURACY_HIGH;
+            ++cnt;
         }
         break;
     case COMMS_SENSOR_GYRO:
@@ -849,32 +873,38 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se
         sv->y = sample->y;
         sv->z = sample->z;
         sv->status = SENSOR_STATUS_ACCURACY_HIGH;
-        sendDirectReportEvent(&nev[cnt], 1);
 
-        if (mSensorState[sensor].enable) {
+        sendDirectReportEvent(&nev[cnt], 1);
+        if (mSensorState[sensor].enable && isSampleIntervalSatisfied(sensor, timestamp)) {
             ++cnt;
         }
 
-        if (mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].enable) {
-            ue = &initEv(&nev[cnt++], timestamp,
-                SENSOR_TYPE_GYROSCOPE_UNCALIBRATED,
-                COMMS_SENSOR_GYRO_UNCALIBRATED)->uncalibrated_gyro;
-            ue->x_uncalib = sample->x + mGyroBias[0];
-            ue->y_uncalib = sample->y + mGyroBias[1];
-            ue->z_uncalib = sample->z + mGyroBias[2];
-            ue->x_bias = mGyroBias[0];
-            ue->y_bias = mGyroBias[1];
-            ue->z_bias = mGyroBias[2];
+        ue = &initEv(&nev[cnt], timestamp,
+            SENSOR_TYPE_GYROSCOPE_UNCALIBRATED,
+            COMMS_SENSOR_GYRO_UNCALIBRATED)->uncalibrated_gyro;
+        ue->x_uncalib = sample->x + mGyroBias[0];
+        ue->y_uncalib = sample->y + mGyroBias[1];
+        ue->z_uncalib = sample->z + mGyroBias[2];
+        ue->x_bias = mGyroBias[0];
+        ue->y_bias = mGyroBias[1];
+        ue->z_bias = mGyroBias[2];
+        sendDirectReportEvent(&nev[cnt], 1);
+
+        if (mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].enable
+                && isSampleIntervalSatisfied(COMMS_SENSOR_GYRO_UNCALIBRATED, timestamp)) {
+            ++cnt;
         }
 
-        if (mSensorState[COMMS_SENSOR_GYRO_WRIST_AWARE].enable) {
-            sv = &initEv(&nev[cnt++], timestamp,
+        if (mSensorState[COMMS_SENSOR_GYRO_WRIST_AWARE].enable
+                && isSampleIntervalSatisfied(COMMS_SENSOR_GYRO_WRIST_AWARE, timestamp)) {
+            sv = &initEv(&nev[cnt], timestamp,
                 SENSOR_TYPE_GYROSCOPE_WRIST_AWARE,
                 COMMS_SENSOR_GYRO_WRIST_AWARE)->gyro;
             sv->x = (mLefty.gyro ? -sample->x : sample->x);
             sv->y = sample->y;
             sv->z = (mLefty.gyro ? -sample->z : sample->z);
             sv->status = SENSOR_STATUS_ACCURACY_HIGH;
+            ++cnt;
         }
         break;
     case COMMS_SENSOR_ACCEL_BIAS:
@@ -897,20 +927,24 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se
         sv->status = magAccuracyUpdate(sv);
         sendDirectReportEvent(&nev[cnt], 1);
 
-        if (mSensorState[sensor].enable) {
+        if (mSensorState[sensor].enable && isSampleIntervalSatisfied(sensor, timestamp)) {
             ++cnt;
         }
 
-        if (mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].enable) {
-            ue = &initEv(&nev[cnt++], timestamp,
-                SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED,
-                COMMS_SENSOR_MAG_UNCALIBRATED)->uncalibrated_magnetic;
-            ue->x_uncalib = sample->x + mMagBias[0];
-            ue->y_uncalib = sample->y + mMagBias[1];
-            ue->z_uncalib = sample->z + mMagBias[2];
-            ue->x_bias = mMagBias[0];
-            ue->y_bias = mMagBias[1];
-            ue->z_bias = mMagBias[2];
+        ue = &initEv(&nev[cnt], timestamp,
+            SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED,
+            COMMS_SENSOR_MAG_UNCALIBRATED)->uncalibrated_magnetic;
+        ue->x_uncalib = sample->x + mMagBias[0];
+        ue->y_uncalib = sample->y + mMagBias[1];
+        ue->z_uncalib = sample->z + mMagBias[2];
+        ue->x_bias = mMagBias[0];
+        ue->y_bias = mMagBias[1];
+        ue->z_bias = mMagBias[2];
+        sendDirectReportEvent(&nev[cnt], 1);
+
+        if (mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].enable
+                && isSampleIntervalSatisfied(COMMS_SENSOR_MAG_UNCALIBRATED, timestamp)) {
+            ++cnt;
         }
         break;
     case COMMS_SENSOR_MAG_BIAS:
@@ -1404,6 +1438,9 @@ ssize_t HubConnection::processBuf(uint8_t *buf, size_t len)
                 ev.sensor = 0;
                 ev.meta_data.what = META_DATA_FLUSH_COMPLETE;
                 ev.meta_data.sensor = flush.handle;
+                if (mSensorState[flush.handle].enable) {
+                    updateSampleRate(flush.handle, CONFIG_CMD_FLUSH);
+                }
 
                 if (flush.internal) {
                     if (flush.handle == COMMS_SENSOR_ACCEL_WRIST_AWARE)
@@ -1657,9 +1694,11 @@ void HubConnection::queueActivate(int handle, bool enable)
         initConfigCmd(&cmd, handle);
 
         ret = TEMP_FAILURE_RETRY(::write(mFd, &cmd, sizeof(cmd)));
-        if (ret == sizeof(cmd))
+        if (ret == sizeof(cmd)) {
+            updateSampleRate(handle, enable ? CONFIG_CMD_ENABLE : CONFIG_CMD_DISABLE);
             ALOGV("queueActivate: sensor=%d, handle=%d, enable=%d",
                     cmd.sensorType, handle, enable);
+        }
         else
             ALOGW("queueActivate: failed to send command: sensor=%d, handle=%d, enable=%d",
                     cmd.sensorType, handle, enable);
@@ -1971,7 +2010,14 @@ void HubConnection::sendDirectReportEvent(const sensors_event_t *nev, size_t n)
             auto i = mSensorToChannel.find(nev->sensor);
             if (i != mSensorToChannel.end()) {
                 for (auto &j : i->second) {
-                    mDirectChannel[j.first]->write(nev);
+                    if ((uint64_t)nev->timestamp > j.second.lastTimestamp
+                            && intervalLargeEnough(
+                                nev->timestamp - j.second.lastTimestamp,
+                                rateLevelToDeviceSamplingPeriodNs(
+                                        nev->sensor, j.second.rateLevel))) {
+                        mDirectChannel[j.first]->write(nev);
+                        j.second.lastTimestamp = nev->timestamp;
+                    }
                 }
             }
             ++nev;
@@ -1981,35 +2027,31 @@ void HubConnection::sendDirectReportEvent(const sensors_event_t *nev, size_t n)
 }
 
 void HubConnection::mergeDirectReportRequest(struct ConfigCmd *cmd, int handle) {
+    int maxRateLevel = SENSOR_DIRECT_RATE_STOP;
+
     auto j = mSensorToChannel.find(handle);
     if (j != mSensorToChannel.end()) {
-        bool enable = false;
-        rate_q10_t rate;
-
-        if (!j->second.empty()) {
-            int maxRateLevel = SENSOR_DIRECT_RATE_STOP;
+        for (auto &i : j->second) {
+            maxRateLevel = std::max(i.second.rateLevel, maxRateLevel);
+        }
+    }
+    for (auto handle : mSensorState[handle].alt) {
+        auto j = mSensorToChannel.find(handle);
+        if (j != mSensorToChannel.end()) {
             for (auto &i : j->second) {
-                maxRateLevel = (i.second) > maxRateLevel ? i.second : maxRateLevel;
-            }
-            switch(maxRateLevel) {
-                case SENSOR_DIRECT_RATE_NORMAL:
-                    enable = true;
-                    rate = period_ns_to_frequency_q10(20000000ull); // NORMAL = 50Hz
-                    break;
-                case SENSOR_DIRECT_RATE_FAST:
-                    enable = true;
-                    rate = period_ns_to_frequency_q10(5000000ull);  // FAST = 200Hz
-                    break;
-                default:
-                    break;
+                maxRateLevel = std::max(i.second.rateLevel, maxRateLevel);
             }
         }
+    }
 
-        if (enable) {
-            cmd->rate = (rate > cmd->rate || cmd->cmd == CONFIG_CMD_DISABLE) ? rate : cmd->rate;
-            cmd->latency = 0;
-            cmd->cmd = CONFIG_CMD_ENABLE;
-        }
+    uint64_t period = rateLevelToDeviceSamplingPeriodNs(handle, maxRateLevel);
+    if (period != INT64_MAX) {
+        rate_q10_t rate;
+        rate = period_ns_to_frequency_q10(period);
+
+        cmd->rate = (rate > cmd->rate || cmd->cmd == CONFIG_CMD_DISABLE) ? rate : cmd->rate;
+        cmd->latency = 0;
+        cmd->cmd = CONFIG_CMD_ENABLE;
     }
 }
 
@@ -2017,6 +2059,13 @@ int HubConnection::addDirectChannel(const struct sensors_direct_mem_t *mem) {
     std::unique_ptr<DirectChannelBase> ch;
     int ret = NO_MEMORY;
 
+    Mutex::Autolock autoLock(mDirectChannelLock);
+    for (const auto& c : mDirectChannel) {
+        if (c.second->memoryMatches(mem)) {
+            // cannot reusing same memory
+            return BAD_VALUE;
+        }
+    }
     switch(mem->type) {
         case SENSOR_DIRECT_MEM_TYPE_ASHMEM:
             ch = std::make_unique<AshmemDirectChannel>(mem);
@@ -2030,7 +2079,6 @@ int HubConnection::addDirectChannel(const struct sensors_direct_mem_t *mem) {
 
     if (ch) {
         if (ch->isValid()) {
-            Mutex::Autolock autoLock(mDirectChannelLock);
             ret = mDirectChannelHandle++;
             mDirectChannel.insert(std::make_pair(ret, std::move(ch)));
         } else {
@@ -2126,7 +2174,7 @@ int HubConnection::configDirectReport(int sensor_handle, int channel_handle, int
 
     j->second.erase(channel_handle);
     if (rate_level != SENSOR_DIRECT_RATE_STOP) {
-        j->second.insert(std::make_pair(channel_handle, rate_level));
+        j->second.insert(std::make_pair(channel_handle, (DirectChannelTimingInfo){0, rate_level}));
     }
 
     Mutex::Autolock autoLock2(mLock);
@@ -2146,6 +2194,86 @@ int HubConnection::configDirectReport(int sensor_handle, int channel_handle, int
 bool HubConnection::isDirectReportSupported() const {
     return true;
 }
+
+void HubConnection::updateSampleRate(int handle, int reason) {
+    bool affected = mSensorToChannel.find(handle) != mSensorToChannel.end();
+    for (size_t i = 0; i < MAX_ALTERNATES && !affected; ++i) {
+        if (mSensorState[handle].alt[i] != COMMS_SENSOR_INVALID) {
+            affected |=
+                    mSensorToChannel.find(mSensorState[handle].alt[i]) != mSensorToChannel.end();
+        }
+    }
+    if (!affected) {
+        return;
+    }
+
+    switch (reason) {
+        case CONFIG_CMD_ENABLE:
+            // filter out duplicated enable
+            if (mSensorState[handle].desiredTSample != INT64_MAX) {
+                break;
+            }
+            // fall through
+        case CONFIG_CMD_FLUSH: {
+            constexpr uint64_t PERIOD_800HZ = 1250000;
+            uint64_t period_multiplier =
+                    (frequency_q10_to_period_ns(mSensorState[handle].rate) + PERIOD_800HZ / 2)
+                        / PERIOD_800HZ;
+            uint64_t desiredTSample = PERIOD_800HZ;
+            while (period_multiplier /= 2) {
+                desiredTSample *= 2;
+            }
+            mSensorState[handle].desiredTSample = desiredTSample;
+            ALOGV("DesiredTSample for handle 0x%x set to %" PRIu64, handle, desiredTSample);
+            break;
+        }
+        case CONFIG_CMD_DISABLE:
+            mSensorState[handle].desiredTSample = INT64_MAX;
+            ALOGV("DesiredTSample 0x%x set to disable", handle);
+            break;
+        default:
+            ALOGW("%s: unexpected reason = %d, no-op", __FUNCTION__, reason);
+            break;
+    }
+}
+
+bool HubConnection::isSampleIntervalSatisfied(int handle, uint64_t timestamp) {
+    if (mSensorToChannel.find(handle) == mSensorToChannel.end()) {
+        return true;
+    }
+
+    if (mSensorState[handle].lastTimestamp >= timestamp
+            || mSensorState[handle].desiredTSample == INT64_MAX) {
+        return false;
+    } else if (intervalLargeEnough(timestamp - mSensorState[handle].lastTimestamp,
+                                   mSensorState[handle].desiredTSample)) {
+        mSensorState[handle].lastTimestamp = timestamp;
+        return true;
+    } else {
+        return false;
+    }
+}
+
+uint64_t HubConnection::rateLevelToDeviceSamplingPeriodNs(int handle, int rateLevel) const {
+    if (mSensorToChannel.find(handle) == mSensorToChannel.end()) {
+        return INT64_MAX;
+    }
+
+    switch (rateLevel) {
+        case SENSOR_DIRECT_RATE_VERY_FAST:
+            // No sensor support VERY_FAST, fall through
+        case SENSOR_DIRECT_RATE_FAST:
+            if (handle != COMMS_SENSOR_MAG && handle != COMMS_SENSOR_MAG_UNCALIBRATED) {
+                return 2500*1000; // 400Hz
+            }
+            // fall through
+        case SENSOR_DIRECT_RATE_NORMAL:
+            return 20*1000*1000; // 50 Hz
+            // fall through
+        default:
+            return INT64_MAX;
+    }
+}
 #else // DIRECT_REPORT_ENABLED
 // nop functions if feature is turned off
 int HubConnection::addDirectChannel(const struct sensors_direct_mem_t *) {
@@ -2169,6 +2297,13 @@ void HubConnection::mergeDirectReportRequest(struct ConfigCmd *, int) {
 bool HubConnection::isDirectReportSupported() const {
     return false;
 }
+
+void HubConnection::updateSampleRate(int, int) {
+}
+
+bool HubConnection::isSampleIntervalSatisfied(int, uint64_t) {
+    return true;
+}
 #endif // DIRECT_REPORT_ENABLED
 
 } // namespace android
diff --git a/sensorhal/hubconnection.h b/sensorhal/hubconnection.h
index 3f0a4f9c..7be18aec 100644
--- a/sensorhal/hubconnection.h
+++ b/sensorhal/hubconnection.h
@@ -173,6 +173,8 @@ private:
 
     struct SensorState {
         uint64_t latency;
+        uint64_t lastTimestamp;
+        uint64_t desiredTSample;
         rate_q10_t rate;
         uint8_t sensorType;
         uint8_t primary;
@@ -329,12 +331,24 @@ public:
 private:
     void sendDirectReportEvent(const sensors_event_t *nev, size_t n);
     void mergeDirectReportRequest(struct ConfigCmd *cmd, int handle);
+    bool isSampleIntervalSatisfied(int handle, uint64_t timestamp);
+    void updateSampleRate(int handle, int reason);
 #ifdef DIRECT_REPORT_ENABLED
     int stopAllDirectReportOnChannel(
             int channel_handle, std::vector<int32_t> *unstoppedSensors);
+    uint64_t rateLevelToDeviceSamplingPeriodNs(int handle, int rateLevel) const;
+    inline static bool intervalLargeEnough(uint64_t actual, uint64_t desired) {
+        return (actual + (actual >> 4)) >= desired; // >= 94.11% of desired
+    }
+
+    struct DirectChannelTimingInfo{
+        uint64_t lastTimestamp;
+        int rateLevel;
+    };
     Mutex mDirectChannelLock;
-    //sensor_handle=>(channel_handle, rate_level)
-    std::unordered_map<int32_t, std::unordered_map<int32_t, int32_t> > mSensorToChannel;
+    //sensor_handle=>(channel_handle => DirectChannelTimingInfo)
+    std::unordered_map<int32_t,
+            std::unordered_map<int32_t, DirectChannelTimingInfo> > mSensorToChannel;
     //channel_handle=>ptr of Channel obj
     std::unordered_map<int32_t, std::unique_ptr<DirectChannelBase>> mDirectChannel;
     int32_t mDirectChannelHandle;
diff --git a/util/common/ring.cpp b/util/common/ring.cpp
index 26d44d62..245f3d03 100644
--- a/util/common/ring.cpp
+++ b/util/common/ring.cpp
@@ -22,6 +22,7 @@
 
 #include <stdlib.h>
 #include <string.h>
+#include <atomic>
 
 namespace android {
 
@@ -120,8 +121,18 @@ void LockfreeBuffer::write(const sensors_event_t *ev, size_t size) {
     }
 
     while(size--) {
-        mData[mWritePos] = *(ev++);
+        // part before reserved0 field
+        memcpy(&mData[mWritePos], ev, offsetof(sensors_event_t, reserved0));
+        // part after reserved0 field
+        memcpy(reinterpret_cast<char *>(&mData[mWritePos]) + offsetof(sensors_event_t, timestamp),
+               reinterpret_cast<const char *>(ev) + offsetof(sensors_event_t, timestamp),
+               sizeof(sensors_event_t) - offsetof(sensors_event_t, timestamp));
+        // barrier before writing the atomic counter
+        std::atomic_thread_fence(std::memory_order_release);
         mData[mWritePos].reserved0 = mCounter++;
+        // barrier after writing the atomic counter
+        std::atomic_thread_fence(std::memory_order_release);
+        ++ev;
 
         if (++mWritePos >= mSize) {
             mWritePos = 0;