sensor: generate flush_complete event when data timestamps matches

Generating a flush_complete event right after receiving
 a flush_request from the sensor services breaks the flow
 as the client stops acquiring data.

Solution is to store the timestamp of the flush request
 and issue the flush_complete event when the timestamp
 of the sensor data matches

Change-Id: Idb273db3e69e6e2593c00298d3d458fc7c965830
Signed-off-by: Fei Li <feix.f.li@intel.com>
Signed-off-by: Guillaume Ranquet <guillaumex.ranquet@intel.com>

13 files changed, 182 insertions, 34 deletions

diff --git a/libsensors_iio/src/Accelerometer.cpp b/libsensors_iio/src/Accelerometer.cpp
index dc9d8c7..5c9cadc 100644
--- a/libsensors_iio/src/Accelerometer.cpp
+++ b/libsensors_iio/src/Accelerometer.cpp
@@ -27,7 +27,7 @@ Accelerometer::Accelerometer(HWSensorBaseCommonData *data, const char *name,
 		sensor_t_data.flags |= SENSOR_FLAG_WAKE_UP;
 
 	sensor_t_data.resolution = data->channels[0].scale;
-	sensor_t_data.maxRange = sensor_t_data.resolution * (pow(2, data->channels[0].bits_used - 1) - 1);
+	sensor_t_data.maxRange = sensor_t_data.resolution * (pow(2.0, data->channels[0].bits_used - 1.0) - 1);
 }
 
 Accelerometer::~Accelerometer()
diff --git a/libsensors_iio/src/Android.mk b/libsensors_iio/src/Android.mk
index 39eca67..b5b7f9e 100644
--- a/libsensors_iio/src/Android.mk
+++ b/libsensors_iio/src/Android.mk
@@ -35,7 +35,7 @@ LOCAL_MODULE_RELATIVE_PATH := hw
 
 LOCAL_MODULE_OWNER := STMicroelectronics
 
-LOCAL_C_INCLUDES := $(LOCAL_PATH)/
+LOCAL_C_INCLUDES := $(LOCAL_PATH)/ external/stlport/stlport bionic
 
 LOCAL_CFLAGS += -DLOG_TAG=\"SensorHAL\"
 
@@ -162,7 +162,7 @@ LOCAL_SRC_FILES += SWLinearAccel.cpp
 endif
 
 
-LOCAL_SHARED_LIBRARIES := liblog libcutils libutils libdl libc
+LOCAL_SHARED_LIBRARIES := liblog libcutils libutils libdl libc libstlport
 
 LOCAL_MODULE_TAGS := optional
 
diff --git a/libsensors_iio/src/Gyroscope.cpp b/libsensors_iio/src/Gyroscope.cpp
index 55011bc..0abf236 100644
--- a/libsensors_iio/src/Gyroscope.cpp
+++ b/libsensors_iio/src/Gyroscope.cpp
@@ -33,7 +33,7 @@ Gyroscope::Gyroscope(HWSensorBaseCommonData *data, const char *name,
 		sensor_t_data.flags |= SENSOR_FLAG_WAKE_UP;
 
 	sensor_t_data.resolution = data->channels[0].scale;
-	sensor_t_data.maxRange = sensor_t_data.resolution * (pow(2, data->channels[0].bits_used - 1) - 1);
+	sensor_t_data.maxRange = sensor_t_data.resolution * (pow(2.0, data->channels[0].bits_used - 1.0) - 1);
 
 #ifdef CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED
 	iNemoEngine_API_gbias_Initialization(NULL);
diff --git a/libsensors_iio/src/HWSensorBase.cpp b/libsensors_iio/src/HWSensorBase.cpp
index 1b487c5..c3c941e 100644
--- a/libsensors_iio/src/HWSensorBase.cpp
+++ b/libsensors_iio/src/HWSensorBase.cpp
@@ -313,11 +313,51 @@ restore_status_enable:
 	return err;
 }
 
-int HWSensorBase::FlushData(int base)
+int HWSensorBase::FlushData(bool need_report_event)
 {
-	int err;
+	bool report_at_once = false;
+	int err = -1;
+	int32_t type = SensorBase::sensor_t_data.type;
+#ifdef __LP64__
+       uint64_t flags;
+#else
+       uint32_t flags;
+#endif
+
+	flags = SensorBase::sensor_t_data.flags;
+	ALOGD("HWSensorBase::FlushData type=%d, flags=%lld", type, flags);
+
+	/* No flush events for One-shot sensors */
+	if (SENSOR_FLAG_ONE_SHOT_MODE == (flags & REPORTING_MODE_MASK))
+		return -EINVAL;
 
 	if (GetStatus()) {
+		/* Sensors used fifo would report flush complete event after data in fifo reported,
+		 * so we store the flush timestamp here. When we write fifo data to pipe,
+		 * we compare them. If the fifo data wrote done, then write the flush complete event.
+		 * One exception: if the flush call come after a fifo parse, there would no data
+		 * in the fifo. If we still sent flush complete event like before, that should wait for
+		 * another fifo parse, that would be a very long time (unit seconds). In this case,
+		 * we would sent the flush complete event here.
+		 */
+		if (need_report_event && ((type == SENSOR_TYPE_ACCELEROMETER) ||
+				(type == SENSOR_TYPE_GYROSCOPE))) {
+			int64_t flush_timestamp = get_monotonic_time();
+			if (flush_timestamp <= real_pollrate) {
+				ALOGE("HWSensorBase get flush base timestamp failed");
+				return err;
+			}
+			ALOGD("hw flush timestamp %lld", flush_timestamp);
+			/* Scale the real_pollrate by 11/10 because LSM6DS3 ODR has +/-10% skew */
+			if (flush_timestamp <= (last_data_timestamp + real_pollrate * 11 / 10))
+				report_at_once = true;
+			else {
+				flush_timestamp -= real_pollrate * 11 /10;
+				(SensorBase::timestamp).push_back(flush_timestamp);
+			}
+		}
+
+		/* Sensors used fifo would trigger read fifo here */
 		if (current_fifo_len > HW_SENSOR_BASE_DEFAULT_IIO_BUFFER_LEN) {
 			err = write_sysfs_int((char *)FILENAME_FLUSH, common_data.iio_sysfs_path, 1);
 			if (err < 0) {
@@ -326,13 +366,20 @@ int HWSensorBase::FlushData(int base)
 				return -EINVAL;
 			}
 		}
+
+		/* Sensors which not use fifo would sent a flush complete event here.
+		 * Sensors used fifo would sent a flush complete event here too if the fifo
+		 * is empty now.
+		 */
+		if (need_report_event && (report_at_once || ((type != SENSOR_TYPE_ACCELEROMETER) &&
+				(type != SENSOR_TYPE_GYROSCOPE))))
+			return SensorBase::FlushData(true);
+		else
+			return 0;
+
 	} else
 		return -EINVAL;
 
-	if (base)
-		return SensorBase::FlushData(0);
-	else
-		return 0;
 }
 
 void HWSensorBase::ThreadTask()
@@ -461,17 +508,53 @@ int HWSensorBaseWithPollrate::SetDelay(int handle, int64_t period_ns, int64_t ti
 void HWSensorBaseWithPollrate::WriteDataToPipe()
 {
 	int err;
+	std::vector<int64_t>::iterator it;
 
 	if (!GetStatusOfHandle(sensor_t_data.handle))
 		return;
 
-	if (sensor_event.timestamp >= (last_data_timestamp + real_pollrate)) {
+	if (!(SensorBase::timestamp.empty())) {
+		int64_t last_timestamp = 0;
+		for (it = SensorBase::timestamp.begin(); it != SensorBase::timestamp.end(); ) {
+			/* If two flush event come within 1 odr, there may not have data in hw fifo,
+			 * so report corresponding flush complete events here.
+			 */
+			if ((sensor_event.timestamp >= *it) ||
+					(last_timestamp != 0 && (*it - last_timestamp < real_pollrate * 11 / 10))) {
+				sensors_event_t flush_event_data;
+
+				flush_event_data.sensor = 0;
+				flush_event_data.timestamp = 0;
+				flush_event_data.meta_data.sensor = sensor_t_data.handle;
+				flush_event_data.meta_data.what = META_DATA_FLUSH_COMPLETE;
+				flush_event_data.type = SENSOR_TYPE_META_DATA;
+				flush_event_data.version = META_DATA_VERSION;
+
+				err = write(android_pipe_fd, &flush_event_data, sizeof(sensor_event));
+
+				if (err < 0) {
+					ALOGE("%s: Writing flush_complete event failed, errno=%d", android_name, errno);
+					return;
+				}
+
+				last_timestamp = *it;
+				it = SensorBase::timestamp.erase(it);
+				ALOGD("write hw flush complete event to pipe succeed.");
+			} else
+				break;
+		}
+	}
+
+	/* Scale the real_pollrate by 9/10 because LSM6DS3 ODR has +/-10% skew */
+	if (sensor_event.timestamp >= (last_data_timestamp + real_pollrate * 9 / 10)) {
 		err = write(android_pipe_fd, &sensor_event, sizeof(sensor_event));
 		if (err < 0) {
-			ALOGE("%s: Failed to write sensor data to pipe.", android_name);
+			ALOGE("%s: Write sensor data failed, errno=%d", android_name, errno);
 			return;
 		}
 
 		last_data_timestamp = sensor_event.timestamp;
-	}
+	} else
+		ALOGE("%s: Dropping event type=%d because ts %lld < %lld (%lld + %lld * 9 / 10)", android_name,
+					(last_data_timestamp + real_pollrate * 9 / 10), last_data_timestamp, real_pollrate);
 }
diff --git a/libsensors_iio/src/HWSensorBase.h b/libsensors_iio/src/HWSensorBase.h
index 4996cad..f9e1113 100644
--- a/libsensors_iio/src/HWSensorBase.h
+++ b/libsensors_iio/src/HWSensorBase.h
@@ -77,7 +77,7 @@ public:
 	virtual ~HWSensorBase();
 
 	virtual int Enable(int handle, bool enable);
-	virtual int FlushData(int base);
+	virtual int FlushData(bool need_report_event);
 	virtual void ThreadTask();
 };
 
diff --git a/libsensors_iio/src/Magnetometer.cpp b/libsensors_iio/src/Magnetometer.cpp
index 4ba8d64..a649fc8 100644
--- a/libsensors_iio/src/Magnetometer.cpp
+++ b/libsensors_iio/src/Magnetometer.cpp
@@ -32,7 +32,7 @@ Magnetometer::Magnetometer(HWSensorBaseCommonData *data, const char *name,
 		sensor_t_data.flags |= SENSOR_FLAG_WAKE_UP;
 
 	sensor_t_data.resolution = GAUSS_TO_UTESLA(data->channels[0].scale);
-	sensor_t_data.maxRange = sensor_t_data.resolution * (pow(2, data->channels[0].bits_used - 1) - 1);
+	sensor_t_data.maxRange = sensor_t_data.resolution * (pow(2.0, data->channels[0].bits_used - 1.0) - 1);
 
 #ifdef CONFIG_ST_HAL_MAGN_CALIB_ENABLED
 	type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_ACCELEROMETER;
diff --git a/libsensors_iio/src/Pressure.cpp b/libsensors_iio/src/Pressure.cpp
index 3fe8820..6af3691 100644
--- a/libsensors_iio/src/Pressure.cpp
+++ b/libsensors_iio/src/Pressure.cpp
@@ -26,7 +26,7 @@ Pressure::Pressure(HWSensorBaseCommonData *data, const char *name,
 		sensor_t_data.flags |= SENSOR_FLAG_WAKE_UP;
 
 	sensor_t_data.resolution = data->channels[0].scale;
-	sensor_t_data.maxRange = sensor_t_data.resolution * (pow(2, data->channels[0].bits_used) - 1);
+	sensor_t_data.maxRange = sensor_t_data.resolution * (pow(2.0, (double)data->channels[0].bits_used) - 1);
 
 	num_data_axis = SENSOR_BASE_1AXIS;
 }
diff --git a/libsensors_iio/src/SWSensorBase.cpp b/libsensors_iio/src/SWSensorBase.cpp
index 0acd26d..97ad409 100644
--- a/libsensors_iio/src/SWSensorBase.cpp
+++ b/libsensors_iio/src/SWSensorBase.cpp
@@ -72,20 +72,36 @@ int SWSensorBase::AddSensorDependency(SensorBase *p)
 	return SensorBase::AddSensorDependency(p);
 }
 
-int SWSensorBase::FlushData(int)
+int SWSensorBase::FlushData(bool /*need_report_event*/)
 {
-	int err, i;
+	int err = -1, i;
+	bool report_event_at_once = false;
 
 	if (GetStatus() && (GetMinTimeout() > 0)) {
+		int64_t flush_timestamp = get_monotonic_time();
+		if (flush_timestamp <= real_pollrate) {
+			ALOGE("HWSensorBase get flush base timestamp failed");
+			return err;
+		}
+		ALOGD("sw flush timestamp %lld", flush_timestamp);
+		if (flush_timestamp > (last_data_timestamp + real_pollrate * 11 / 10)) {
+			flush_timestamp -= real_pollrate;
+			(SensorBase::timestamp).push_back(flush_timestamp);
+		} else
+			report_event_at_once = true;
+
 		for (i = 0; i < (int)dependencies_num; i++) {
-			err = dependencies[i]->FlushData(0);
+			err = dependencies[i]->FlushData(false);
 			if (err < 0)
 				return -EINVAL;
 		}
+
+		if (report_event_at_once)
+			return SensorBase::FlushData(false);
+		else
+			return 0;
 	} else
 		return -EINVAL;
-
-	return SensorBase::FlushData(0);
 }
 
 void SWSensorBase::ThreadTask()
@@ -132,14 +148,47 @@ int SWSensorBaseWithPollrate::SetDelay(int handle, int64_t period_ns, int64_t ti
 void SWSensorBaseWithPollrate::WriteDataToPipe()
 {
 	int err;
+	std::vector<int64_t>::iterator it;
 
 	if (!GetStatusOfHandle(sensor_t_data.handle))
 		return;
 
-	if (sensor_event.timestamp >= (last_data_timestamp + real_pollrate)) {
+	if (!(SensorBase::timestamp.empty())) {
+		int64_t last_timestamp = 0;
+		for (it = SensorBase::timestamp.begin(); it != SensorBase::timestamp.end(); ) {
+			/* If two flush event come within 1 odr, there may not have data in hw fifo,
+			 * so report corresponding flush complete events here.
+			 */
+			if ((sensor_event.timestamp >= *it) ||
+					(last_timestamp != 0 && (*it - last_timestamp < real_pollrate * 11 / 10))) {
+				sensors_event_t flush_event_data;
+
+				flush_event_data.sensor = 0;
+				flush_event_data.timestamp = 0;
+				flush_event_data.meta_data.sensor = sensor_t_data.handle;
+				flush_event_data.meta_data.what = META_DATA_FLUSH_COMPLETE;
+				flush_event_data.type = SENSOR_TYPE_META_DATA;
+				flush_event_data.version = META_DATA_VERSION;
+
+				err = write(android_pipe_fd, &flush_event_data, sizeof(sensor_event));
+
+				if (err < 0) {
+					ALOGE("%s: Failed to write SW flush_complete, errno=%d", android_name, errno);
+					return;
+				}
+
+				last_timestamp = *it;
+				it = SensorBase::timestamp.erase(it);
+				ALOGD("SW flush_complete sent");
+			} else
+				break;
+		}
+	}
+
+	if (sensor_event.timestamp >= (last_data_timestamp + real_pollrate * 9 / 10)) {
 		err = write(android_pipe_fd, &sensor_event, sizeof(sensor_event));
 		if (err < 0) {
-			ALOGE("%s: Failed to write sensor data to pipe.", android_name);
+			ALOGE("%s: Failed to write sensor data - err=%d.", android_name, err);
 			return;
 		}
 
diff --git a/libsensors_iio/src/SWSensorBase.h b/libsensors_iio/src/SWSensorBase.h
index 55abb86..3bfa3be 100644
--- a/libsensors_iio/src/SWSensorBase.h
+++ b/libsensors_iio/src/SWSensorBase.h
@@ -42,7 +42,7 @@ public:
 	virtual ~SWSensorBase();
 
 	int AddSensorDependency(SensorBase *p);
-	virtual int FlushData(int base);
+	virtual int FlushData(bool need_report_event);
 	virtual void ThreadTask();
 };
 
diff --git a/libsensors_iio/src/SensorBase.cpp b/libsensors_iio/src/SensorBase.cpp
index e6904e4..992d3d4 100644
--- a/libsensors_iio/src/SensorBase.cpp
+++ b/libsensors_iio/src/SensorBase.cpp
@@ -11,10 +11,24 @@
 #include <assert.h>
 #include <string.h>
 #include <signal.h>
+#include <time.h>
 
 #include "SensorBase.h"
 
-#define ST_SENSOR_BASE_WAIT_US_BEFORE_SEND_FLUSH		(200000)
+int64_t get_monotonic_time(void)
+{
+	int err;
+	struct timespec tm;
+
+	err = clock_gettime(CLOCK_BOOTTIME, &tm);
+	if (err < 0) {
+		ALOGE("get_monotonic_time failed, err=%d", err);
+		return err;
+	}
+
+	ALOGD("get_monotonic_time %ld, %ld", tm.tv_sec, tm.tv_nsec);
+	return (int64_t) tm.tv_sec * 1000000000 + (int64_t) tm.tv_nsec;
+}
 
 SensorBase::SensorBase(const char *name, int handle, int type, int pipe_data_fd)
 {
@@ -355,7 +369,7 @@ bool SensorBase::FillSensor_tData(struct sensor_t *data)
 	return true;
 }
 
-int SensorBase::FlushData(int)
+int SensorBase::FlushData(bool)
 {
 	int err;
 	sensors_event_t flush_event_data;
@@ -373,6 +387,7 @@ int SensorBase::FlushData(int)
 		return err;
 	}
 
+	ALOGD("SensorBase::FlushData completed.");
 	return 0;
 }
 
diff --git a/libsensors_iio/src/SensorBase.h b/libsensors_iio/src/SensorBase.h
index 9b73dcc..25cb79e 100644
--- a/libsensors_iio/src/SensorBase.h
+++ b/libsensors_iio/src/SensorBase.h
@@ -24,6 +24,7 @@
 #include <time.h>
 #include <pthread.h>
 #include <errno.h>
+#include <vector>
 
 #include <hardware/sensors.h>
 #include <cutils/log.h>
@@ -121,6 +122,7 @@ public:
 	int GetType();
 	int GetMaxFifoLenght();
 
+	std::vector<int64_t> timestamp;
 	char* GetName();
 
 	virtual int Enable(int handle, bool enable);
@@ -140,7 +142,7 @@ public:
 
 	bool FillSensor_tData(struct sensor_t *data);
 
-	virtual int FlushData(int);
+	virtual int FlushData(bool);
 
 	virtual void ProcessData(SensorBaseData *data);
 	virtual void ProcessEvent(struct iio_event_data *event_data);
@@ -154,4 +156,6 @@ public:
 	virtual void ThreadTask();
 };
 
+int64_t get_monotonic_time(void);
+
 #endif /* ST_SENSOR_BASE_H */
diff --git a/libsensors_iio/src/SensorHAL.cpp b/libsensors_iio/src/SensorHAL.cpp
index 1bf5018..c0c8443 100644
--- a/libsensors_iio/src/SensorHAL.cpp
+++ b/libsensors_iio/src/SensorHAL.cpp
@@ -439,7 +439,7 @@ static int st_hal_set_fullscale(char *iio_sysfs_path, int sensor_type,
 	}
 
 	for (i = 0; i < (int)sa->num_available; i++) {
-		if ((sa->values[i] * (pow(2, channels[0].bits_used - 1) - 1)) >= max_value)
+		if ((sa->values[i] * ((int)pow(2.0, channels[0].bits_used - 1.0) - 1)) >= max_value)
 			break;
 	}
 	if (i == (int)sa->num_available)
@@ -584,12 +584,9 @@ st_hal_load_free_iio_sysfs_path:
 static int st_hal_dev_flush(struct sensors_poll_device_1 *dev, int handle)
 {
 	STSensorHAL_data *hal_data = (STSensorHAL_data *)dev;
-	ALOGD("st_hal_dev_flush type=%u", ((struct sensor_t) hal_data->sensor_t_list[handle-1]).type);
-	/* One-shot sensor must return -EINVAL and not generate any flush complete metadata event */
-	if (SENSOR_TYPE_SIGNIFICANT_MOTION == ((struct sensor_t) hal_data->sensor_t_list[handle-1]).type)
-		return -EINVAL;
+	ALOGD("st_hal_dev_flush handle=%d", handle);
 
-	return hal_data->sensor_classes[handle]->FlushData(1);
+	return hal_data->sensor_classes[handle]->FlushData(true);
 }
 
 /**
diff --git a/libsensors_iio/src/StepCounter.cpp b/libsensors_iio/src/StepCounter.cpp
index 32575b3..7eecd81 100644
--- a/libsensors_iio/src/StepCounter.cpp
+++ b/libsensors_iio/src/StepCounter.cpp
@@ -26,7 +26,7 @@ StepCounter::StepCounter(HWSensorBaseCommonData *data, const char *name,
 		sensor_t_data.flags |= SENSOR_FLAG_WAKE_UP;
 
 	sensor_t_data.resolution = 1.0f;
-	sensor_t_data.maxRange = pow(2, data->channels[0].bits_used) - 1;
+	sensor_t_data.maxRange = pow(2.0, (double)data->channels[0].bits_used) - 1;
 
 	num_data_axis = SENSOR_BASE_1AXIS;
 }