Merge tag 'android-6.0.1_r22' of https://android.googlesource.com/platform/hardware/invensense into cm-13.0staging/cm-13.0+r22

Android 6.0.1 release 22

7 files changed, 376 insertions, 166 deletions

diff --git a/6515/libsensors_iio/Android.mk b/6515/libsensors_iio/Android.mk
index f33c7ae..f894e50 100755
--- a/6515/libsensors_iio/Android.mk
+++ b/6515/libsensors_iio/Android.mk
@@ -30,6 +30,19 @@ MAJOR_VERSION :=$(shell echo $(PLATFORM_VERSION) | cut -f1 -d.)
 MINOR_VERSION :=$(shell echo $(PLATFORM_VERSION) | cut -f2 -d.)
 VERSION_KK :=$(shell test $(MAJOR_VERSION) -eq 4 -a $(MINOR_VERSION) -gt 3 && echo true)
 VERSION_L  :=$(shell test $(MAJOR_VERSION) -eq 5 -a $(MINOR_VERSION) -eq 0 && echo true)
+
+#
+# Invensense uses the OS version to determine whether to include batch support,
+# but implemented it in a way that requires modifying the code each time we move
+# to a newer OS version.  I will fix this problem in a subsequent change, but for now,
+# hardcode to saying we're ANDROID_L so we can isolate this checkin to being
+# only changes coming from Invensense.
+#
+# Setting ANDROID_L to true is perfectly safe even on ANDROID_M because the code
+# just requires "ANDROID_L or newer"
+#
+VERSION_L :=true
+
 $(info YD>>ANDRIOD VERSION=$(MAJOR_VERSION).$(MINOR_VERSION))
 $(info YD>>VERSION_L=$(VERSION_L), VERSION_KK=$(VERSION_KK))
 #ANDROID version check END
diff --git a/6515/libsensors_iio/InputEventReader.cpp b/6515/libsensors_iio/InputEventReader.cpp
index 968e32e..5ec3e6c 100755
--- a/6515/libsensors_iio/InputEventReader.cpp
+++ b/6515/libsensors_iio/InputEventReader.cpp
@@ -20,6 +20,7 @@
 #include <errno.h>
 #include <unistd.h>
 #include <poll.h>
+#include <string.h>
 
 #include <sys/cdefs.h>
 #include <sys/types.h>
diff --git a/6515/libsensors_iio/MPLSensor.cpp b/6515/libsensors_iio/MPLSensor.cpp
index ad6d4e9..5a57d0b 100644
--- a/6515/libsensors_iio/MPLSensor.cpp
+++ b/6515/libsensors_iio/MPLSensor.cpp
@@ -37,6 +37,7 @@
 #include <string.h>
 #include <linux/input.h>
 #include <utils/Atomic.h>
+#include <utils/SystemClock.h>
 
 #include "MPLSensor.h"
 #include "PressureSensor.IIO.secondary.h"
@@ -62,6 +63,15 @@
 
 #define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))
 
+// query path to determine if vibrator is currently vibrating
+#define VIBRATOR_ENABLE_FILE "/sys/class/timed_output/vibrator/enable"
+
+
+// Minimum time after vibrator triggers SMD before SMD can be declared valid
+// This allows 100mS for events to propogate
+#define MIN_TRIGGER_TIME_AFTER_VIBRATOR_NS 100000000
+
+
 /******************************************************************************/
 /*  MPL Interface                                                             */
 /******************************************************************************/
@@ -75,7 +85,7 @@ static FILE *logfile = NULL;
  * MPLSensor class implementation
  ******************************************************************************/
 
-static struct timespec mt_pre;
+static int64_t mt_pre_ns;
 
 // following extended initializer list would only be available with -std=c++11
 //  or -std=gnu+11
@@ -97,6 +107,7 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *
                          mDmpPedometerEnabled(0),
                          mDmpStepCountEnabled(0),
                          mEnabled(0),
+                         mEnabledCached(0),
                          mBatchEnabled(0),
                          mOldBatchEnabledMask(0),
                          mAccelInputReader(4),
@@ -117,20 +128,6 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *
                          mAccelBiasApplied(false),
                          mPendingMask(0),
                          mSensorMask(0),
-                         mMplFeatureActiveMask(0),
-                         mFeatureActiveMask(0),
-                         mDmpOn(0),
-                         mPedUpdate(0),
-                         mPressureUpdate(0),
-                         mQuatSensorTimestamp(0),
-                         mStepSensorTimestamp(0),
-                         mLastStepCount(-1),
-                         mLeftOverBufferSize(0),
-                         mInitial6QuatValueAvailable(0),
-                         mFlushBatchSet(0),
-                         mSkipReadEvents(0),
-                         mDataMarkerDetected(0),
-                         mEmptyDataMarkerDetected(0),
                          mGyroBatchRate(0),
                          mAccelBatchRate(0),
                          mCompassBatchRate(0),
@@ -144,7 +141,21 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *
                          mResetRate(0),
                          mDataInterrupt(0),
                          mFirstBatchCall(1),
-                         mEnableCalled(1) {
+                         mEnableCalled(1),
+                         mMplFeatureActiveMask(0),
+                         mFeatureActiveMask(0),
+                         mDmpOn(0),
+                         mPedUpdate(0),
+                         mPressureUpdate(0),
+                         mQuatSensorTimestamp(0),
+                         mStepSensorTimestamp(0),
+                         mLastStepCount(-1),
+                         mLeftOverBufferSize(0),
+                         mInitial6QuatValueAvailable(0),
+                         mSkipReadEvents(0),
+                         mSkipExecuteOnData(0),
+                         mDataMarkerDetected(0),
+                         mEmptyDataMarkerDetected(0) {
     VFUNC_LOG;
 
     inv_error_t rv;
@@ -162,10 +173,13 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *
 
     pthread_mutex_init(&mMplMutex, NULL);
     pthread_mutex_init(&mHALMutex, NULL);
+    mFlushBatchSet = 0;
     memset(mGyroOrientation, 0, sizeof(mGyroOrientation));
     memset(mAccelOrientation, 0, sizeof(mAccelOrientation));
     memset(mInitial6QuatValue, 0, sizeof(mInitial6QuatValue));
     mFlushSensorEnabledVector.setCapacity(NumSensors);
+    memset(mEnabledTime, 0, sizeof(mEnabledTime));
+    memset(mLastTimestamp, 0, sizeof(mLastTimestamp));
 
     /* setup sysfs paths */
     inv_init_sysfs_attributes();
@@ -1095,7 +1109,7 @@ int MPLSensor::setDmpFeature(int en)
     int res = 0;
 
     // set sensor engine and fifo
-    if ((mFeatureActiveMask & DMP_FEATURE_MASK) || en) {
+    if (((mFeatureActiveMask & ~INV_DMP_BATCH_MODE) & DMP_FEATURE_MASK) || en) {
         if ((mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION) ||
                 (mFeatureActiveMask & INV_DMP_PED_QUATERNION) ||
                 (mFeatureActiveMask & INV_DMP_QUATERNION)) {
@@ -1822,7 +1836,7 @@ int MPLSensor::enableDmpPedometer(int en, int interruptMode)
             mFeatureActiveMask |= INV_DMP_PEDOMETER_STEP;
         }
 
-        clock_gettime(CLOCK_MONOTONIC, &mt_pre);
+        mt_pre_ns = android::elapsedRealtimeNano();
     } else {
         if (interruptMode) {
             mFeatureActiveMask &= ~INV_DMP_PEDOMETER;
@@ -2613,6 +2627,12 @@ int MPLSensor::gyroHandler(sensors_event_t* s)
     update = inv_get_sensor_type_gyroscope(s->gyro.v, &s->gyro.status,
                                            &s->timestamp);
 #endif
+    if (!mEnabledTime[Gyro] || !(s->timestamp > mEnabledTime[Gyro])) {
+        LOGV_IF(ENG_VERBOSE, "HAL:gyro incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+                mEnabledTime[Gyro], s->timestamp, android::elapsedRealtimeNano());
+        update = 0;
+    }
+
     LOGV_IF(HANDLER_DATA, "HAL:gyro data : %+f %+f %+f -- %lld - %d",
             s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
     return update;
@@ -2629,6 +2649,12 @@ int MPLSensor::rawGyroHandler(sensors_event_t* s)
 	update = inv_get_sensor_type_gyroscope_raw(s->uncalibrated_gyro.uncalib,
                                                &s->gyro.status, &s->timestamp);
 #endif
+    if (!mEnabledTime[RawGyro] || !(s->timestamp > mEnabledTime[RawGyro])) {
+        LOGV_IF(ENG_VERBOSE, "HAL:raw gyro incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+                mEnabledTime[RawGyro], s->timestamp, android::elapsedRealtimeNano());
+        update = 0;
+    }
+
     if(update) {
         memcpy(s->uncalibrated_gyro.bias, mGyroBias, sizeof(mGyroBias));
         LOGV_IF(HANDLER_DATA,"HAL:gyro bias data : %+f %+f %+f -- %lld - %d",
@@ -2653,6 +2679,12 @@ int MPLSensor::accelHandler(sensors_event_t* s)
     update = inv_get_sensor_type_accelerometer(
         s->acceleration.v, &s->acceleration.status, &s->timestamp);
 #endif
+    if (!mEnabledTime[Accelerometer] || !(s->timestamp > mEnabledTime[Accelerometer])) {
+        LOGV_IF(ENG_VERBOSE, "HAL:accel incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+                mEnabledTime[Accelerometer], s->timestamp, android::elapsedRealtimeNano());
+        update = 0;
+    }
+
     LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d",
             s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2],
             s->timestamp, update);
@@ -2664,6 +2696,7 @@ int MPLSensor::compassHandler(sensors_event_t* s)
 {
     VHANDLER_LOG;
     int update;
+    int overflow = mCompassOverFlow;
 #if defined ANDROID_LOLLIPOP
     update = inv_get_sensor_type_magnetic_field(
         s->magnetic.v, &s->magnetic.status, (inv_time_t *)(&s->timestamp));
@@ -2671,17 +2704,24 @@ int MPLSensor::compassHandler(sensors_event_t* s)
     update = inv_get_sensor_type_magnetic_field(
         s->magnetic.v, &s->magnetic.status, &s->timestamp);
 #endif
+    if (!mEnabledTime[MagneticField] || !(s->timestamp > mEnabledTime[MagneticField])) {
+        LOGV_IF(ENG_VERBOSE, "HAL:compass incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+                mEnabledTime[MagneticField], s->timestamp, android::elapsedRealtimeNano());
+        overflow = 0;
+        update = 0;
+    }
     LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d",
             s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2],
             s->timestamp, update);
     mCompassAccuracy = s->magnetic.status;
-    return update | mCompassOverFlow;
+    return update | overflow;
 }
 
 int MPLSensor::rawCompassHandler(sensors_event_t* s)
 {
     VHANDLER_LOG;
     int update;
+    int overflow = mCompassOverFlow;
     //TODO: need to handle uncalib data and bias for 3rd party compass
 #if defined ANDROID_LOLLIPOP
     if(mCompassSensor->providesCalibration()) {
@@ -2700,6 +2740,12 @@ int MPLSensor::rawCompassHandler(sensors_event_t* s)
                      &s->magnetic.status, &s->timestamp);
     }
 #endif
+    if (!mEnabledTime[RawMagneticField] || !(s->timestamp > mEnabledTime[RawMagneticField])) {
+        LOGV_IF(ENG_VERBOSE, "HAL:raw compass incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+                mEnabledTime[RawMagneticField], s->timestamp, android::elapsedRealtimeNano());
+        overflow = 0;
+        update = 0;
+    }
     if(update) {
         memcpy(s->uncalibrated_magnetic.bias, mCompassBias, sizeof(mCompassBias));
         LOGV_IF(HANDLER_DATA, "HAL:compass bias data: %+f %+f %+f -- %lld - %d",
@@ -2710,7 +2756,7 @@ int MPLSensor::rawCompassHandler(sensors_event_t* s)
     LOGV_IF(HANDLER_DATA, "HAL:compass raw data: %+f %+f %+f %d -- %lld - %d",
         s->uncalibrated_magnetic.uncalib[0], s->uncalibrated_magnetic.uncalib[1],
                     s->uncalibrated_magnetic.uncalib[2], s->magnetic.status, s->timestamp, update);
-    return update | mCompassOverFlow;
+    return update | overflow;
 }
 
 /*
@@ -2730,7 +2776,15 @@ int MPLSensor::rvHandler(sensors_event_t* s)
                                                  &s->timestamp);
 #endif
     s->orientation.status = status;
+
     update |= isCompassDisabled();
+
+    if (!mEnabledTime[RotationVector] || !(s->timestamp > mEnabledTime[RotationVector])) {
+        LOGV_IF(ENG_VERBOSE, "HAL:rv incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+                mEnabledTime[RotationVector], s->timestamp, android::elapsedRealtimeNano());
+        update = 0;
+    }
+
     LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f %+f %d- %+lld - %d",
             s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->orientation.status, s->timestamp,
             update);
@@ -2756,6 +2810,12 @@ int MPLSensor::grvHandler(sensors_event_t* s)
 #endif
     s->orientation.status = status;
 
+    if (!mEnabledTime[GameRotationVector] || !(s->timestamp > mEnabledTime[GameRotationVector])) {
+        LOGV_IF(ENG_VERBOSE, "HAL:grv incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+                mEnabledTime[GameRotationVector], s->timestamp, android::elapsedRealtimeNano());
+        update = 0;
+    }
+
     LOGV_IF(HANDLER_DATA, "HAL:grv data: %+f %+f %+f %+f %+f %d- %+lld - %d",
             s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->orientation.status, s->timestamp,
             update);
@@ -2774,6 +2834,13 @@ int MPLSensor::laHandler(sensors_event_t* s)
             s->gyro.v, &s->gyro.status, &s->timestamp);
 #endif
     update |= isCompassDisabled();
+
+    if (!mEnabledTime[LinearAccel] || !(s->timestamp > mEnabledTime[LinearAccel])) {
+        LOGV_IF(ENG_VERBOSE, "HAL:la incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+                mEnabledTime[LinearAccel], s->timestamp, android::elapsedRealtimeNano());
+        update = 0;
+    }
+
     LOGV_IF(HANDLER_DATA, "HAL:la data: %+f %+f %+f - %lld - %d",
             s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
     return update;
@@ -2791,6 +2858,13 @@ int MPLSensor::gravHandler(sensors_event_t* s)
                                          &s->timestamp);
 #endif
     update |= isCompassDisabled();
+
+    if (!mEnabledTime[Gravity] || !(s->timestamp > mEnabledTime[Gravity])) {
+        LOGV_IF(ENG_VERBOSE, "HAL:gr incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+                mEnabledTime[Gravity], s->timestamp, android::elapsedRealtimeNano());
+        update = 0;
+    }
+
     LOGV_IF(HANDLER_DATA, "HAL:gr data: %+f %+f %+f - %lld - %d",
             s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
     return update;
@@ -2809,6 +2883,13 @@ int MPLSensor::orienHandler(sensors_event_t* s)
 
 #endif
     update |= isCompassDisabled();
+
+    if (!mEnabledTime[Orientation] || !(s->timestamp > mEnabledTime[Orientation])) {
+        LOGV_IF(ENG_VERBOSE, "HAL:or incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+                mEnabledTime[Orientation], s->timestamp, android::elapsedRealtimeNano());
+        update = 0;
+    }
+
     LOGV_IF(HANDLER_DATA, "HAL:or data: %f %f %f - %lld - %d",
             s->orientation.v[0], s->orientation.v[1], s->orientation.v[2],
             s->timestamp, update);
@@ -2826,9 +2907,13 @@ int MPLSensor::smHandler(sensors_event_t* s)
     s->data[2] = 0.f;
 
     /* Capture timestamp in HAL */
-    struct timespec ts;
-    clock_gettime(CLOCK_MONOTONIC, &ts);
-    s->timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
+    s->timestamp = android::elapsedRealtimeNano();
+
+    if (!mEnabledTime[SignificantMotion] || !(s->timestamp > mEnabledTime[SignificantMotion])) {
+        LOGV_IF(ENG_VERBOSE, "HAL:sm incorrect timestamp Enabled=%lld, Timestamp=%lld",
+                mEnabledTime[SignificantMotion], s->timestamp);
+        update = 0;
+    }
 
     LOGV_IF(HANDLER_DATA, "HAL:sm data: %f - %lld - %d",
             s->data[0], s->timestamp, update);
@@ -2840,6 +2925,7 @@ int MPLSensor::gmHandler(sensors_event_t* s)
     VHANDLER_LOG;
     int8_t status;
     int update = 0;
+
 #if defined ANDROID_LOLLIPOP
     update = inv_get_sensor_type_geomagnetic_rotation_vector(s->data, &status,
                                                              (inv_time_t *)(&s->timestamp));
@@ -2848,6 +2934,13 @@ int MPLSensor::gmHandler(sensors_event_t* s)
                                                              &s->timestamp);
 #endif
     s->orientation.status = status;
+
+    if (!mEnabledTime[GeomagneticRotationVector] || !(s->timestamp > mEnabledTime[GeomagneticRotationVector])) {
+        LOGV_IF(ENG_VERBOSE, "HAL:gm incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+                mEnabledTime[GeomagneticRotationVector], s->timestamp, android::elapsedRealtimeNano());
+        update = 0;
+    }
+
     LOGV_IF(HANDLER_DATA, "HAL:gm data: %+f %+f %+f %+f %+f %d- %+lld - %d",
             s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->orientation.status, s->timestamp, update);
     return update < 1 ? 0 :1;
@@ -2868,6 +2961,14 @@ int MPLSensor::psHandler(sensors_event_t* s)
     update = mPressureUpdate;
     mPressureUpdate = 0;
 
+#ifdef ENABLE_PRESSURE
+    if (!mEnabledTime[Pressure] || !(s->timestamp > mEnabledTime[Pressure])) {
+        LOGV_IF(ENG_VERBOSE, "HAL:ps incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+                mEnabledTime[Pressure], s->timestamp, android::elapsedRealtimeNano());
+        update = 0;
+    }
+#endif
+
     LOGV_IF(HANDLER_DATA, "HAL:ps data: %+f %+f %+f %+f- %+lld - %d",
             s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update);
     return update < 1 ? 0 :1;
@@ -2885,9 +2986,7 @@ int MPLSensor::sdHandler(sensors_event_t* s)
     s->data[2] = 0.f;
 
     /* get current timestamp */
-    struct timespec ts;
-    clock_gettime(CLOCK_MONOTONIC, &ts) ;
-    s->timestamp = (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec;
+    s->timestamp =  android::elapsedRealtimeNano();
 
     LOGV_IF(HANDLER_DATA, "HAL:sd data: %f - %lld - %d",
             s->data[0], s->timestamp, update);
@@ -2911,14 +3010,7 @@ int MPLSensor::scHandler(sensors_event_t* s)
 #endif
 
     if (s->timestamp == 0 && update) {
-        struct timespec ts;
-        clock_gettime(CLOCK_MONOTONIC, &ts);
-        s->timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
-                // workaround for some platform which has gap between monotonic clock
-                // and Android SystemClock.
-                // Subtract 100ms not to point the future for SystemClock.
-                // s->timestamp -= 100000000LL;
-        LOGV_IF(0, "HAL:sc timestamp %lld", s->timestamp);
+        s->timestamp = android::elapsedRealtimeNano();
     }
 
     return update;
@@ -2939,10 +3031,10 @@ int MPLSensor::metaHandler(sensors_event_t* s, int flags)
     switch(flags) {
     case META_DATA_FLUSH_COMPLETE:
         s->type = SENSOR_TYPE_META_DATA;
+        s->version = META_DATA_VERSION;
         s->meta_data.what = flags;
         s->meta_data.sensor = mFlushSensorEnabledVector[0];
         mFlushSensorEnabledVector.removeAt(0);
-        //mFlushBatchSet = 0;
         LOGV_IF(HANDLER_DATA,
                 "HAL:flush complete data: type=%d what=%d, "
                 "sensor=%d - %lld - %d",
@@ -2975,7 +3067,7 @@ int MPLSensor::enable(int32_t handle, int en)
     if (!en)
         mBatchEnabled &= ~(1 << handle);
 
-    LOGV_IF(ENG_VERBOSE, "HAL:handle = %d", handle);
+    LOGV_IF(ENG_VERBOSE, "HAL: MPLSensor::enable(handle = %d, en = %d)", handle, en);
 
     switch (handle) {
     case ID_SC:
@@ -2987,8 +3079,16 @@ int MPLSensor::enable(int32_t handle, int en)
                 (en? "en" : "dis"));
         enableDmpPedometer(en, 0);
         mDmpStepCountEnabled = !!en;
+        if (en)
+            mEnabledTime[StepCounter] = android::elapsedRealtimeNano();
+        else
+            mEnabledTime[StepCounter] = 0;
+
+        if (!en)
+            mBatchDelays[what] = 1000000000LL;
         return 0;
     case ID_P:
+        what = StepDetector;
         sname = "StepDetector";
         LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
                 sname.string(), handle,
@@ -3002,6 +3102,13 @@ int MPLSensor::enable(int32_t handle, int en)
             setBatch(batchMode,1);
         }
         mOldBatchEnabledMask = batchMode;
+        if (en)
+            mEnabledTime[StepDetector] = android::elapsedRealtimeNano();
+        else
+            mEnabledTime[StepDetector] = 0;
+
+        if (!en)
+            mBatchDelays[what] = 1000000000LL;
         return 0;
     case ID_SM:
         sname = "Significant Motion";
@@ -3011,6 +3118,10 @@ int MPLSensor::enable(int32_t handle, int en)
                 (en? "en" : "dis"));
         enableDmpSignificantMotion(en);
         mDmpSignificantMotionEnabled = !!en;
+        if (en)
+            mEnabledTime[SignificantMotion] = android::elapsedRealtimeNano();
+        else
+            mEnabledTime[SignificantMotion] = 0;
         return 0;
     case ID_SO:
         sname = "Screen Orientation";
@@ -3097,6 +3208,11 @@ int MPLSensor::enable(int32_t handle, int en)
 
         mEnabled &= ~(1 << what);
         mEnabled |= (uint32_t(flags) << what);
+        if (lastEnabled > mEnabled) {
+            mEnabledCached = mEnabled;
+        } else {
+            mEnabledCached = lastEnabled;
+        }
 
         LOGV_IF(ENG_VERBOSE, "HAL:flags = %d", flags);
         computeLocalSensorMask(mEnabled);
@@ -3187,6 +3303,16 @@ int MPLSensor::enable(int32_t handle, int en)
         }
         LOGV_IF(ENG_VERBOSE, "HAL:changed = %d", changed);
         enableSensors(sen_mask, flags, changed);
+        // update mEnabledCached afer all configurations done
+        mEnabledCached = mEnabled;
+
+        if (en)
+            mEnabledTime[what] = android::elapsedRealtimeNano();
+        else
+            mEnabledTime[what] = 0;
+
+        if (!en)
+            mBatchDelays[what] = 1000000000LL;
     }
 
     // pthread_mutex_unlock(&mMplMutex);
@@ -3717,7 +3843,8 @@ int MPLSensor::readEvents(sensors_event_t* data, int count)
 {
     VHANDLER_LOG;
 
-    inv_execute_on_data();
+    if (!mSkipExecuteOnData)
+        inv_execute_on_data();
 
     int numEventReceived = 0;
 
@@ -3771,9 +3898,16 @@ int MPLSensor::readEvents(sensors_event_t* data, int count)
                     update = readDmpPedometerEvents(data, count, ID_P, 1);
                     mPedUpdate = 0;
                     if(update == 1 && count > 0) {
-                        data->timestamp = mStepSensorTimestamp;
-                        count--;
-                        numEventReceived++;
+                        if (mLastTimestamp[i] != mStepSensorTimestamp) {
+                            count--;
+                            numEventReceived++;
+                            data->timestamp = mStepSensorTimestamp;
+                            data++;
+                            mLastTimestamp[i] = mStepSensorTimestamp;
+                        } else {
+                            ALOGE("Event from type=%d with duplicate timestamp %lld discarded",
+                                    mPendingEvents[i].type, mStepSensorTimestamp);
+                        }
                         continue;
                     }
                 } else {
@@ -3784,14 +3918,21 @@ int MPLSensor::readEvents(sensors_event_t* data, int count)
             }
 
             // load up virtual sensors
-            if (mEnabled & (1 << i)) {
+            if (mEnabledCached & (1 << i)) {
                 update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i);
                 mPendingMask |= (1 << i);
 
                 if (update && (count > 0)) {
-                    *data++ = mPendingEvents[i];
-                    count--;
-                    numEventReceived++;
+                    // Discard any events with duplicate timestamps
+                    if (mLastTimestamp[i] != mPendingEvents[i].timestamp) {
+                        mLastTimestamp[i] = mPendingEvents[i].timestamp;
+                        *data++ = mPendingEvents[i];
+                        count--;
+                        numEventReceived++;
+                    } else {
+                        ALOGE("Event from type=%d with duplicate timestamp %lld discarded",
+                                    mPendingEvents[i].type, mStepSensorTimestamp);
+                    }
                 }
             }
         }
@@ -3800,32 +3941,33 @@ int MPLSensor::readEvents(sensors_event_t* data, int count)
 
     // handle partial packet read and end marker
     // skip readEvents from hal_outputs
-    int flush_vec_size = mFlushSensorEnabledVector.size();
-    if (flush_vec_size && mDataMarkerDetected && mFlushBatchSet) {
-        // handle flush complete event
-        for(int k = 0; k < flush_vec_size; k++) {
-            int sendEvent = metaHandler(&mPendingFlushEvents[k], META_DATA_FLUSH_COMPLETE);
-            if(sendEvent && count > 0) {
-                *data++ = mPendingFlushEvents[k];
+    if (mFlushBatchSet && count>0 && !mFlushSensorEnabledVector.isEmpty()) {
+        while (mFlushBatchSet && count>0 && !mFlushSensorEnabledVector.isEmpty()) {
+            int sendEvent = metaHandler(&mPendingFlushEvents[0], META_DATA_FLUSH_COMPLETE);
+            if (sendEvent) {
+                LOGV_IF(ENG_VERBOSE, "Queueing flush complete for handle=%d",
+                        mPendingFlushEvents[0].meta_data.sensor);
+                *data++ = mPendingFlushEvents[0];
                 count--;
                 numEventReceived++;
+            } else {
+                LOGV_IF(ENG_VERBOSE, "sendEvent false, NOT queueing flush complete for handle=%d",
+                        mPendingFlushEvents[0].meta_data.sensor);
             }
-        }
-
-        if (!mEmptyDataMarkerDetected) {
-            // turn off sensors in data_builder
-            resetMplStates();
+            mFlushBatchSet--;
         }
 
         // Double check flush status
         if (mFlushSensorEnabledVector.isEmpty()) {
-			mEmptyDataMarkerDetected = 0;
+            mEmptyDataMarkerDetected = 0;
             mDataMarkerDetected = 0;
-			mFlushBatchSet = 0;
-            LOGV_IF(0, "Flush completed");
+            mFlushBatchSet = 0;
+            LOGV_IF(ENG_VERBOSE, "Flush completed");
         } else {
-            LOGV_IF(0, "Flush is still active");
+            LOGV_IF(ENG_VERBOSE, "Flush is still active");
         }
+    } else if (mFlushBatchSet && mFlushSensorEnabledVector.isEmpty()) {
+        mFlushBatchSet = 0;
     }
 
     return numEventReceived;
@@ -3855,6 +3997,18 @@ void MPLSensor::buildMpuEvent(void)
     char *rdataP = NULL;
     bool doneFlag = 0;
 
+    /* flush buffer when no sensors are enabled */
+    if (mEnabledCached == 0 && mBatchEnabled == 0 && mDmpPedometerEnabled == 0) {
+        rsize = read(iio_fd, rdata, MAX_SUSPEND_BATCH_PACKET_SIZE);
+        if(rsize > 0) {
+            LOGV_IF(ENG_VERBOSE, "HAL:input data flush rsize=%d", (int)rsize);
+        }
+        mLeftOverBufferSize = 0;
+        mDataMarkerDetected = 0;
+        mEmptyDataMarkerDetected = 0;
+        return;
+    }
+
     lp_quaternion_on = isLowPowerQuatEnabled() && checkLPQuaternion();
     sixAxis_quaternion_on = check6AxisQuatEnabled();
     ped_quaternion_on = checkPedQuatEnabled();
@@ -3943,7 +4097,7 @@ LOGV_IF(INPUT_DATA,
                 readCounter -= BYTES_PER_SENSOR;
                 rdata += BYTES_PER_SENSOR;
                 if (!mFlushSensorEnabledVector.isEmpty()) {
-                    mFlushBatchSet = 1;
+                    mFlushBatchSet++;
                 }
                 mDataMarkerDetected = 1;
             }
@@ -3952,9 +4106,10 @@ LOGV_IF(INPUT_DATA,
                 readCounter -= BYTES_PER_SENSOR;
                 rdata += BYTES_PER_SENSOR;
                 if (!mFlushSensorEnabledVector.isEmpty()) {
-                    mFlushBatchSet = 1;
+                    mFlushBatchSet++;
                 }
                 mEmptyDataMarkerDetected = 1;
+                mDataMarkerDetected = 1;
             }
         }
 
@@ -3986,6 +4141,7 @@ LOGV_IF(INPUT_DATA,
             sensors, lp_quaternion_on, sixAxis_quaternion_on, ped_quaternion_on,
             ped_standalone_on);
 
+    mSkipExecuteOnData = 1;
     while (readCounter > 0) {
         // since copied buffer is already accounted for, reset left over size
         mLeftOverBufferSize = 0;
@@ -4019,19 +4175,18 @@ LOGV_IF(INPUT_DATA,
             LOGV_IF(ENG_VERBOSE && INPUT_DATA, "MARKER DETECTED:0x%x", data_format);
             readCounter -= BYTES_PER_SENSOR;
             if (!mFlushSensorEnabledVector.isEmpty()) {
-                mFlushBatchSet = 1;
+                mFlushBatchSet++;
             }
             mDataMarkerDetected = 1;
-            mSkipReadEvents = 1;
         }
         else if (data_format == DATA_FORMAT_EMPTY_MARKER) {
             LOGV_IF(ENG_VERBOSE && INPUT_DATA, "EMPTY MARKER DETECTED:0x%x", data_format);
             readCounter -= BYTES_PER_SENSOR;
             if (!mFlushSensorEnabledVector.isEmpty()) {
-                mFlushBatchSet = 1;
+                mFlushBatchSet++;
             }
             mEmptyDataMarkerDetected = 1;
-            mSkipReadEvents = 1;
+            mDataMarkerDetected = 1;
         }
         else if (data_format == DATA_FORMAT_QUAT) {
             LOGV_IF(ENG_VERBOSE && INPUT_DATA, "QUAT DETECTED:0x%x", data_format);
@@ -4197,7 +4352,7 @@ LOGV_IF(INPUT_DATA,
 				rdata += BYTES_PER_SENSOR;
 				readCounter -= BYTES_PER_SENSOR;
 				if (!mFlushSensorEnabledVector.isEmpty()) {
-					mFlushBatchSet = 1;
+					mFlushBatchSet++;
 				}
 				mDataMarkerDetected = 1;
 				if (readCounter == 0) {
@@ -4240,6 +4395,7 @@ LOGV_IF(INPUT_DATA,
                         "HAL:input inv_read_temperature = %lld, timestamp= %lld",
                         temperature[0], temperature[1]);
                         inv_build_temp(temperature[0], temperature[1]);
+                        mSkipExecuteOnData = 0;
                      }
 #ifdef TESTING
                     long bias[3], temp, temp_slope[3];
@@ -4267,30 +4423,30 @@ LOGV_IF(INPUT_DATA,
                    "HAL:input inv_build_gyro: %+8d %+8d %+8d - %lld",
                     mCachedGyroData[0], mCachedGyroData[1],
                     mCachedGyroData[2], mGyroSensorTimestamp);
+            mSkipExecuteOnData = 0;
             latestTimestamp = mGyroSensorTimestamp;
         }
 
         if (mask == DATA_FORMAT_ACCEL) {
-            if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
-                mPendingMask |= 1 << Accelerometer;
-                inv_build_accel(mCachedAccelData, 0, mAccelSensorTimestamp);
-                LOGV_IF(INPUT_DATA,
-                   "HAL:input inv_build_accel: %+8ld %+8ld %+8ld - %lld",
-                    mCachedAccelData[0], mCachedAccelData[1],
-                    mCachedAccelData[2], mAccelSensorTimestamp);
-                    /* remember inital 6 axis quaternion */
-                    inv_time_t tempTimestamp;
-                    inv_get_6axis_quaternion(mInitial6QuatValue, &tempTimestamp);
-                    if (mInitial6QuatValue[0] != 0 && mInitial6QuatValue[1] != 0 &&
-                            mInitial6QuatValue[2] != 0 && mInitial6QuatValue[3] != 0) {
-                        mInitial6QuatValueAvailable = 1;
-                        LOGV_IF(INPUT_DATA && ENG_VERBOSE,
-                            "HAL:input build 6q init: %+8ld %+8ld %+8ld %+8ld",
-                            mInitial6QuatValue[0], mInitial6QuatValue[1],
-                            mInitial6QuatValue[2], mInitial6QuatValue[3]);
-                    }
-                latestTimestamp = mAccelSensorTimestamp;
+            mPendingMask |= 1 << Accelerometer;
+            inv_build_accel(mCachedAccelData, 0, mAccelSensorTimestamp);
+            LOGV_IF(INPUT_DATA,
+               "HAL:input inv_build_accel: %+8ld %+8ld %+8ld - %lld",
+                mCachedAccelData[0], mCachedAccelData[1],
+                mCachedAccelData[2], mAccelSensorTimestamp);
+            mSkipExecuteOnData = 0;
+            /* remember inital 6 axis quaternion */
+            inv_time_t tempTimestamp;
+            inv_get_6axis_quaternion(mInitial6QuatValue, &tempTimestamp);
+            if (mInitial6QuatValue[0] != 0 && mInitial6QuatValue[1] != 0 &&
+                    mInitial6QuatValue[2] != 0 && mInitial6QuatValue[3] != 0) {
+                mInitial6QuatValueAvailable = 1;
+                LOGV_IF(INPUT_DATA && ENG_VERBOSE,
+                    "HAL:input build 6q init: %+8ld %+8ld %+8ld %+8ld",
+                    mInitial6QuatValue[0], mInitial6QuatValue[1],
+                    mInitial6QuatValue[2], mInitial6QuatValue[3]);
             }
+            latestTimestamp = mAccelSensorTimestamp;
         }
 
         if (mask  == DATA_FORMAT_COMPASS_OF) {
@@ -4303,6 +4459,7 @@ LOGV_IF(INPUT_DATA,
                     "HAL:input inv_build_compass_of: %+8ld %+8ld %+8ld - %lld",
                     mCachedCompassData[0], mCachedCompassData[1],
                     mCachedCompassData[2], mCompassTimestamp);
+            mSkipExecuteOnData = 0;
             resetCompass();
         }
 
@@ -4318,6 +4475,7 @@ LOGV_IF(INPUT_DATA,
                     "HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld",
                     mCachedCompassData[0], mCachedCompassData[1],
                     mCachedCompassData[2], mCompassTimestamp);
+            mSkipExecuteOnData = 0;
             latestTimestamp = mCompassTimestamp;
         }
 
@@ -4338,6 +4496,7 @@ LOGV_IF(INPUT_DATA,
                     mCachedQuaternionData[0], mCachedQuaternionData[1],
                     mCachedQuaternionData[2],
                     mQuatSensorTimestamp);
+            mSkipExecuteOnData = 0;
             latestTimestamp = mQuatSensorTimestamp;
         }
 
@@ -4357,6 +4516,7 @@ LOGV_IF(INPUT_DATA,
                     "HAL:input inv_build_quat-6x: %+8ld %+8ld %+8ld - %lld",
                     mCached6AxisQuaternionData[0], mCached6AxisQuaternionData[1],
                     mCached6AxisQuaternionData[2], mQuatSensorTimestamp);
+            mSkipExecuteOnData = 0;
             latestTimestamp = mQuatSensorTimestamp;
         }
 
@@ -4376,27 +4536,26 @@ LOGV_IF(INPUT_DATA,
             inv_build_quat(mCachedPedQuaternionData,
                        status,
                        mQuatSensorTimestamp);
-
             LOGV_IF(INPUT_DATA,
                     "HAL:HAL:input inv_build_quat-ped_6x: %+8ld %+8ld %+8ld - %lld",
                     mCachedPedQuaternionData[0], mCachedPedQuaternionData[1],
                     mCachedPedQuaternionData[2], mQuatSensorTimestamp);
+            mSkipExecuteOnData = 0;
             latestTimestamp = mQuatSensorTimestamp;
         }
 
 #ifdef ENABLE_PRESSURE
         if ((mask ==DATA_FORMAT_PRESSURE) && mPressureSensor->isIntegrated()) {
             int status = 0;
-            if (mLocalSensorMask & INV_ONE_AXIS_PRESSURE) {
-                latestTimestamp = mPressureTimestamp;
-                mPressureUpdate = 1;
-                inv_build_pressure(mCachedPressureData,
-                            status,
-                            mPressureTimestamp);
-                LOGV_IF(INPUT_DATA,
-                    "HAL:input inv_build_pressure: %+8ld - %lld",
-                    mCachedPressureData, mPressureTimestamp);
-            }
+            latestTimestamp = mPressureTimestamp;
+            mPressureUpdate = 1;
+            inv_build_pressure(mCachedPressureData,
+                        status,
+                        mPressureTimestamp);
+            LOGV_IF(INPUT_DATA,
+                "HAL:input inv_build_pressure: %+8ld - %lld",
+                mCachedPressureData, mPressureTimestamp);
+            mSkipExecuteOnData = 0;
         }
 #endif
         /* take the latest timestamp */
@@ -4407,6 +4566,9 @@ LOGV_IF(INPUT_DATA,
                 LOGV_IF(INPUT_DATA,
                     "HAL:input build step: 1 - %lld", mStepSensorTimestamp);
             } else {
+                LOGV_IF(ENG_VERBOSE, "Step data OUT OF ORDER, "
+                        "mPedUpdate = 0x%x last = %lld, ts = %lld",
+                        mPedUpdate, mStepSensorTimestamp, latestTimestamp);
                 mPedUpdate = 0;
             }
         }
@@ -4465,15 +4627,14 @@ void MPLSensor::buildCompassEvent(void)
             status = mCompassSensor->getAccuracy();
             status |= INV_CALIBRATED;
         }
-        if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
-            inv_build_compass(mCachedCompassData, status,
-                              mCompassTimestamp);
-            LOGV_IF(INPUT_DATA,
-                    "HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld",
-                    mCachedCompassData[0], mCachedCompassData[1],
-                    mCachedCompassData[2], mCompassTimestamp);
-            mSkipReadEvents = 0;
-        }
+        inv_build_compass(mCachedCompassData, status,
+                          mCompassTimestamp);
+        LOGV_IF(INPUT_DATA,
+                "HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld",
+                mCachedCompassData[0], mCachedCompassData[1],
+                mCachedCompassData[2], mCompassTimestamp);
+        mSkipReadEvents = 0;
+        mSkipExecuteOnData = 0;
     }
 
     // pthread_mutex_unlock(&mMplMutex);
@@ -4694,9 +4855,7 @@ int MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count)
         temp.type = SENSOR_TYPE_SCREEN_ORIENTATION;
         temp.screen_orientation = screen_orientation;
 #endif
-        struct timespec ts;
-        clock_gettime(CLOCK_MONOTONIC, &ts);
-        temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
+        temp.timestamp = android::elapsedRealtimeNano();
 
         *data++ = temp;
         count--;
@@ -4764,12 +4923,11 @@ bool MPLSensor::hasStepCountPendingEvents(void)
 {
     VFUNC_LOG;
     if (mDmpStepCountEnabled) {
-        struct timespec t_now;
+        int64_t t_now_ns;
         int64_t interval = 0;
 
-        clock_gettime(CLOCK_MONOTONIC, &t_now);
-        interval = ((int64_t(t_now.tv_sec) * 1000000000LL + t_now.tv_nsec) -
-                    (int64_t(mt_pre.tv_sec) * 1000000000LL + mt_pre.tv_nsec));
+        t_now_ns = android::elapsedRealtimeNano();
+        interval = t_now_ns - mt_pre_ns;
 
         if (interval < mStepCountPollTime) {
             LOGV_IF(0,
@@ -4777,9 +4935,9 @@ bool MPLSensor::hasStepCountPendingEvents(void)
                     interval, mStepCountPollTime);
             return false;
         } else {
-            clock_gettime(CLOCK_MONOTONIC, &mt_pre);
-            LOGV_IF(0, "Step Count previous time: %ld ms",
-                    mt_pre.tv_nsec / 1000);
+            mt_pre_ns = android::elapsedRealtimeNano();
+            LOGV_IF(0, "Step Count previous time: %lld ms",
+                    mt_pre_ns / 1000000);
             return true;
         }
     }
@@ -5759,7 +5917,11 @@ int MPLSensor::batch(int handle, int flags, int64_t period_ns, int64_t timeout)
 
     LOGV_IF(PROCESS_VERBOSE,
             "HAL:batch after applying upper and lower limit: %llu ns, (%.2f Hz)",
-			period_ns, 1000000000.f / period_ns);
+            period_ns, 1000000000.f / period_ns);
+
+    LOGV_IF(PROCESS_VERBOSE,
+            "HAL:batch after applying upper and lower limit: %llu ns, (%.2f Hz)",
+            period_ns, 1000000000.f / period_ns);
 
     switch (what) {
     case Gyro:
@@ -5841,7 +6003,7 @@ int MPLSensor::batch(int handle, int flags, int64_t period_ns, int64_t timeout)
 
     if (!timeout) {
         mBatchEnabled &= ~(1 << what);
-        mBatchDelays[what] = 1000000000L;
+        mBatchDelays[what] = 1000000000LL;
         mDelays[what] = period_ns;
         mBatchTimeouts[what] = 100000000000LL;
     } else {
@@ -6101,7 +6263,6 @@ int MPLSensor::flush(int handle)
     }
 
     mFlushSensorEnabledVector.push_back(handle);
-    mFlushBatchSet = 0;
 
     /*write sysfs */
     LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
@@ -6361,29 +6522,46 @@ int MPLSensor::readDmpSignificantMotionEvents(sensors_event_t* data, int count)
 
     int res = 0;
     char dummy[4];
-    int significantMotion;
+    int vibrator = 0;
     FILE *fp;
     int sensors = mEnabled;
     int numEventReceived = 0;
     int update = 0;
+    static int64_t lastVibTrigger = 0;
 
-    /* Technically this step is not necessary for now  */
-    /* In the future, we may have meaningful values */
-    fp = fopen(mpu.event_smd, "r");
-    if (fp == NULL) {
-        LOGE("HAL:cannot open event_smd");
-        return 0;
-    } else {
-        if (fscanf(fp, "%d\n", &significantMotion) < 0) {
-            LOGE("HAL:cannot read event_smd");
-        }
-        if (fclose(fp) < 0) {
-            LOGE("HAL:cannot close event_smd");
+    if (mDmpSignificantMotionEnabled && count > 0) {
+
+        // If vibrator is going off, ignore this event
+        fp = fopen(VIBRATOR_ENABLE_FILE, "r");
+        if (fp != NULL) {
+            if (fscanf(fp, "%d\n", &vibrator) < 0) {
+                LOGE("HAL:cannot read %s", VIBRATOR_ENABLE_FILE);
+            }
+            if (fclose(fp) < 0) {
+                LOGE("HAL:cannot close %s", VIBRATOR_ENABLE_FILE);
+            }
+            if (vibrator != 0) {
+                lastVibTrigger = android::elapsedRealtimeNano();
+                LOGV_IF(ENG_VERBOSE, "SMD triggered by vibrator, ignoring SMD event");
+                return 0;
+            } else if (lastVibTrigger) {
+                // vibrator recently triggered SMD, discard related events
+                int64_t now = android::elapsedRealtimeNano();
+		if ((now - lastVibTrigger) < MIN_TRIGGER_TIME_AFTER_VIBRATOR_NS) {
+                    LOGV_IF(ENG_VERBOSE, "HAL: SMD triggered too close to vibrator (delta %lldnS), ignoring",
+                            (now-lastVibTrigger));
+                    return 0;
+                } else {
+                    LOGV_IF(ENG_VERBOSE, "HAL: SMD triggered %lld after vibrator (last %lld now %lld)",
+                            now-lastVibTrigger, lastVibTrigger, now);
+                    lastVibTrigger = 0;
+                }
+            }
+        } else {
+            LOGE("HAL:cannot open %s", VIBRATOR_ENABLE_FILE);
         }
-    }
 
-    if(mDmpSignificantMotionEnabled && count > 0) {
-       /* By implementation, smd is disabled once an event is triggered */
+        /* By implementation, smd is disabled once an event is triggered */
         sensors_event_t temp;
 
         /* Handles return event */
@@ -6543,7 +6721,7 @@ int MPLSensor::calcBatchDataRates(int64_t *gyro_rate, int64_t *accel_rate, int64
 #ifdef ENABLE_PRESSURE
     int64_t pressureRate;
 #endif
-    int64_t quatRate;
+    int64_t quatRate = 0;
 
     int mplGyroRate;
     int mplAccelRate;
diff --git a/6515/libsensors_iio/MPLSensor.h b/6515/libsensors_iio/MPLSensor.h
index 13ba301..04a35ee 100644
--- a/6515/libsensors_iio/MPLSensor.h
+++ b/6515/libsensors_iio/MPLSensor.h
@@ -355,6 +355,7 @@ protected:
     int mDmpStepCountEnabled;
 
     uint32_t mEnabled;
+    uint32_t mEnabledCached;
     uint32_t mBatchEnabled;
     android::Vector<int> mFlushSensorEnabledVector;
     uint32_t mOldBatchEnabledMask;
@@ -368,6 +369,8 @@ protected:
     int64_t mBatchDelays[NumSensors];
     int64_t mBatchTimeouts[NumSensors];
     hfunc_t mHandlers[NumSensors];
+    int64_t mEnabledTime[NumSensors];
+    int64_t mLastTimestamp[NumSensors];
     short mCachedGyroData[3];
     long mCachedAccelData[3];
     long mCachedCompassData[3];
@@ -535,8 +538,9 @@ protected:
     char mLeftOverBuffer[1024];
     bool mInitial6QuatValueAvailable;
     long mInitial6QuatValue[4];
-    bool mFlushBatchSet;
+    int mFlushBatchSet;
     uint32_t mSkipReadEvents;
+    uint32_t mSkipExecuteOnData;
     bool mDataMarkerDetected;
     bool mEmptyDataMarkerDetected;
     int mDmpState;
diff --git a/6515/libsensors_iio/MPLSupport.cpp b/6515/libsensors_iio/MPLSupport.cpp
index 2954df3..dc40fab 100755
--- a/6515/libsensors_iio/MPLSupport.cpp
+++ b/6515/libsensors_iio/MPLSupport.cpp
@@ -150,7 +150,7 @@ int read_sysfs_int(char *filename, int *var)
 
     sysfsfp = fopen(filename, "r");
     if (sysfsfp != NULL) {
-        if (fscanf(sysfsfp, "%d\n", var) < 0 || fclose(sysfsfp) < 0) {    
+        if (fscanf(sysfsfp, "%d\n", var) < 0 || fclose(sysfsfp) < 0) {
             res = errno;
             LOGE("HAL:ERR open file %s to read with error %d", filename, res);
         }
@@ -165,7 +165,7 @@ int read_sysfs_int64(char *filename, int64_t *var)
 
     sysfsfp = fopen(filename, "r");
     if (sysfsfp != NULL) {
-        if (fscanf(sysfsfp, "%lld\n", var) < 0 || fclose(sysfsfp) < 0) {    
+        if (fscanf(sysfsfp, "%lld\n", var) < 0 || fclose(sysfsfp) < 0) {
             res = errno;
             LOGE("HAL:ERR open file %s to read with error %d", filename, res);
         }
@@ -178,7 +178,7 @@ void convert_long_to_hex_char(long* quat, unsigned char* hex, int numElement)
     int bytePosition = 0;
     for (int index = 0; index < numElement; index++) {
         for (int i = 0; i < 4; i++) {
-            hex[bytePosition] = (int) ((quat[index] >> (4-1-i) * 8) & 0xFF);                                 
+            hex[bytePosition] = (int) ((quat[index] >> (4-1-i) * 8) & 0xFF);
             //LOGI("e%d quat[%d]: %x", index, bytePosition, hex[bytePosition]);
             bytePosition++;
         }
@@ -208,7 +208,7 @@ int write_sysfs_longlong(char *filename, int64_t var)
 
     sysfsfp = fopen(filename, "w");
     if (sysfsfp != NULL) {
-        if (fprintf(sysfsfp, "%lld\n", var) < 0 || fclose(sysfsfp) < 0) {   
+        if (fprintf(sysfsfp, "%lld\n", var) < 0 || fclose(sysfsfp) < 0) {
             res = errno;
             LOGE("HAL:ERR open file %s to write with error %d", filename, res);
         }
@@ -216,7 +216,7 @@ int write_sysfs_longlong(char *filename, int64_t var)
     return -res;
 }
 
-int fill_dev_full_name_by_prefix(const char* dev_prefix, 
+int fill_dev_full_name_by_prefix(const char* dev_prefix,
                                  char *dev_full_name, int len)
 {
     char cand_name[20];
@@ -226,34 +226,34 @@ int fill_dev_full_name_by_prefix(const char* dev_prefix,
     // try adding a number, 0-9
     for(int cand_postfix = 0; cand_postfix < 10; cand_postfix++) {
         snprintf(&cand_name[prefix_len],
-                 sizeof(cand_name) / sizeof(cand_name[0]), 
+                 sizeof(cand_name) / sizeof(cand_name[0]),
                  "%d", cand_postfix);
         int dev_num = find_type_by_name(cand_name, "iio:device");
         if (dev_num != -ENODEV) {
             strncpy(dev_full_name, cand_name, len);
-            return 0; 
+            return 0;
         }
     }
     // try adding a small letter, a-z
     for(char cand_postfix = 'a'; cand_postfix <= 'z'; cand_postfix++) {
-        snprintf(&cand_name[prefix_len], 
-                 sizeof(cand_name) / sizeof(cand_name[0]), 
+        snprintf(&cand_name[prefix_len],
+                 sizeof(cand_name) / sizeof(cand_name[0]),
                  "%c", cand_postfix);
         int dev_num = find_type_by_name(cand_name, "iio:device");
         if (dev_num != -ENODEV) {
             strncpy(dev_full_name, cand_name, len);
-            return 0; 
+            return 0;
         }
     }
     // try adding a capital letter, A-Z
     for(char cand_postfix = 'A'; cand_postfix <= 'Z'; cand_postfix++) {
         snprintf(&cand_name[prefix_len],
-                 sizeof(cand_name) / sizeof(cand_name[0]), 
+                 sizeof(cand_name) / sizeof(cand_name[0]),
                  "%c", cand_postfix);
         int dev_num = find_type_by_name(cand_name, "iio:device");
         if (dev_num != -ENODEV) {
             strncpy(dev_full_name, cand_name, len);
-            return 0; 
+            return 0;
         }
     }
     return 1;
@@ -286,10 +286,10 @@ int read_sysfs_dir(bool fileMode, char *sysfs_path)
     int fd;
     char buf[sizeof(long) *4];
     long data;
-  
+
     DIR *dp;
     struct dirent *ep;
-      
+
     dp = opendir (sysfs_path);
 
     if (dp != NULL)
@@ -316,7 +316,7 @@ int read_sysfs_dir(bool fileMode, char *sysfs_path)
                             LOGI("HAL DEBUG:sysfs:cat %s = %ld", full_path, data);
                     } else {
                          LOGV_IF(0,"HAL DEBUG: error reading %s", full_path);
-                    } 
+                    }
                 } else {
                     LOGV_IF(0,"HAL DEBUG: error opening %s", full_path);
                 }
diff --git a/6515/libsensors_iio/SensorBase.cpp b/6515/libsensors_iio/SensorBase.cpp
index 544ac15..f9825ad 100755
--- a/6515/libsensors_iio/SensorBase.cpp
+++ b/6515/libsensors_iio/SensorBase.cpp
@@ -24,6 +24,8 @@
 #include <sys/select.h>
 #include <cutils/log.h>
 #include <linux/input.h>
+#include <string.h>
+#include <utils/SystemClock.h>
 
 #include <cutils/properties.h>
 
@@ -141,10 +143,7 @@ bool SensorBase::hasPendingEvents() const
 
 int64_t SensorBase::getTimestamp()
 {
-    struct timespec t;
-    t.tv_sec = t.tv_nsec = 0;
-    clock_gettime(CLOCK_MONOTONIC, &t);
-    return int64_t(t.tv_sec) * 1000000000LL + t.tv_nsec;
+    return android::elapsedRealtimeNano();
 }
 
 int SensorBase::openInput(const char *inputName)
diff --git a/6515/libsensors_iio/sensors_mpl.cpp b/6515/libsensors_iio/sensors_mpl.cpp
index 7e7b06b..b277e3b 100755
--- a/6515/libsensors_iio/sensors_mpl.cpp
+++ b/6515/libsensors_iio/sensors_mpl.cpp
@@ -32,16 +32,17 @@
 
 #include <utils/Atomic.h>
 #include <utils/Log.h>
+#include <utils/SystemClock.h>
 
 #include "sensors.h"
 #include "MPLSensor.h"
 
-/* 
- * Vendor-defined Accel Load Calibration File Method 
+/*
+ * Vendor-defined Accel Load Calibration File Method
  * @param[out] Accel bias, length 3.  In HW units scaled by 2^16 in body frame
  * @return '0' for a successful load, '1' otherwise
  * example: int AccelLoadConfig(long* offset);
- * End of Vendor-defined Accel Load Cal Method 
+ * End of Vendor-defined Accel Load Cal Method
  */
 
 /*****************************************************************************/
@@ -81,6 +82,14 @@ static struct hw_module_methods_t sensors_module_methods = {
         open: open_sensors
 };
 
+static int sensors_set_operation_mode(unsigned int mode)
+{
+    LOGI("%s", __FUNCTION__);
+    LOGI("%s: stub function: ignoring mode request (%d)", __FUNCTION__,
+                 mode);
+    return 0;
+}
+
 struct sensors_module_t HAL_MODULE_INFO_SYM = {
         common: {
                 tag: HARDWARE_MODULE_TAG,
@@ -94,6 +103,7 @@ struct sensors_module_t HAL_MODULE_INFO_SYM = {
                 reserved: {0}
         },
         get_sensors_list: sensors__get_sensors_list,
+        set_operation_mode: sensors_set_operation_mode
 };
 
 struct sensors_poll_context_t {
@@ -109,6 +119,7 @@ struct sensors_poll_context_t {
 #if defined ANDROID_KITKAT || defined ANDROID_LOLLIPOP
     int flush(int handle);
 #endif
+    int64_t getTimestamp();
 
 private:
     enum {
@@ -127,7 +138,6 @@ private:
 
     /* Significant Motion wakelock support */
     bool mSMDWakelockHeld;
-
 };
 
 /******************************************************************************/
@@ -144,7 +154,7 @@ sensors_poll_context_t::sensors_poll_context_t() {
 
    /* For Vendor-defined Accel Calibration File Load
     * Use the Following Constructor and Pass Your Load Cal File Function
-    * 
+    *
     * MPLSensor *mplSensor = new MPLSensor(mCompassSensor, AccelLoadConfig);
     */
 
@@ -174,7 +184,7 @@ sensors_poll_context_t::sensors_poll_context_t() {
 
     mPollFds[dmpPed].fd = ((MPLSensor*) mSensor)->getDmpPedometerFd();
     mPollFds[dmpPed].events = POLLPRI;
-    mPollFds[dmpPed].revents = 0;   
+    mPollFds[dmpPed].revents = 0;
 }
 
 sensors_poll_context_t::~sensors_poll_context_t() {
@@ -190,7 +200,7 @@ int sensors_poll_context_t::activate(int handle, int enabled) {
     FUNC_LOG;
 
     int err;
-    err = mSensor->enable(handle, enabled);   
+    err = mSensor->enable(handle, enabled);
     return err;
 }
 
@@ -200,6 +210,11 @@ int sensors_poll_context_t::setDelay(int handle, int64_t ns)
     return mSensor->setDelay(handle, ns);
 }
 
+int64_t sensors_poll_context_t::getTimestamp()
+{
+    return android::elapsedRealtimeNano();
+}
+
 int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count)
 {
     VHANDLER_LOG;
@@ -234,7 +249,7 @@ int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count)
 
     // look for new events
     nb = poll(mPollFds, numSensorDrivers, polltime);
-    LOGI_IF(0, "poll nb=%d, count=%d, pt=%d", nb, count, polltime);
+    LOGI_IF(0, "poll nb=%d, count=%d, pt=%d ts=%lld", nb, count, polltime, getTimestamp());
     if (nb > 0) {
         for (int i = 0; count && i < numSensorDrivers; i++) {
             if (mPollFds[i].revents & (POLLIN | POLLPRI)) {
@@ -283,7 +298,7 @@ int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count)
                     LOGI_IF(0, "sensors_mpl:readEvents() - "
                             "i=%d, nb=%d, count=%d, nbEvents=%d, "
                             "data->timestamp=%lld, data->data[0]=%f,",
-                            i, nb, count, nbEvents, data->timestamp, 
+                            i, nb, count, nbEvents, data->timestamp,
                             data->data[0]);
                     if (nb > 0) {
                         count -= nb;
@@ -333,7 +348,7 @@ int sensors_poll_context_t::query(int what, int* value)
     return mSensor->query(what, value);
 }
 
-int sensors_poll_context_t::batch(int handle, int flags, int64_t period_ns, 
+int sensors_poll_context_t::batch(int handle, int flags, int64_t period_ns,
                                   int64_t timeout)
 {
     FUNC_LOG;