Sensor: Invensense: 6515: fix step counter corrupting event queue

Fixed a sensor event queue corruption bug caused in some cases
when a step event is read.  The bug would either cause the step
event to get overwritten before being delivered to Sensor Services,
resulting in a lost sensor event and an older, already sent sensor
event to be resent to sensor services.

Bug: 25618874
Change-Id: I9d4c87b42f88113275a8ac613ae87ae9ec20d6ea
diff --git a/6515/libsensors_iio/MPLSensor.cpp b/6515/libsensors_iio/MPLSensor.cpp
index bf0fdb9..5a57d0b 100644
--- a/6515/libsensors_iio/MPLSensor.cpp
+++ b/6515/libsensors_iio/MPLSensor.cpp
@@ -3902,6 +3902,7 @@
                             count--;
                             numEventReceived++;
                             data->timestamp = mStepSensorTimestamp;
+                            data++;
                             mLastTimestamp[i] = mStepSensorTimestamp;
                         } else {
                             ALOGE("Event from type=%d with duplicate timestamp %lld discarded",
@@ -4565,6 +4566,9 @@
                 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;
             }
         }