sensor: poll for POLLOUT event before writing pipe

Poll with timeout = -1 for flush complete event and timeout = 1000
for regular sensor event

flush requests are tracked with vector. This requires to include stlport.
As stlport olverloads pow(), all calls to this function are also modified
to avoid ambiguous calls.

Change-Id: I4f8bd97289dbc3e18b5fb0911aa84bde4f401ad8
Signed-off-by: Like Yan <like.yan@intel.com>

4 files changed, 96 insertions, 33 deletions

diff --git a/libsensors_iio/src/HWSensorBase.cpp b/libsensors_iio/src/HWSensorBase.cpp
index c3c941e..0e7c383 100644
--- a/libsensors_iio/src/HWSensorBase.cpp
+++ b/libsensors_iio/src/HWSensorBase.cpp
@@ -18,6 +18,7 @@
 #include "HWSensorBase.h"
 
 #define DEFAULT_HRTIMER_PERIOD_NS		(200000000)
+#define LOCAL_REPORTING_MODE_MASK		6
 
 
 /**
@@ -328,7 +329,7 @@ int HWSensorBase::FlushData(bool need_report_event)
 	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))
+	if (SENSOR_FLAG_ONE_SHOT_MODE == (flags & LOCAL_REPORTING_MODE_MASK))
 		return -EINVAL;
 
 	if (GetStatus()) {
@@ -373,9 +374,9 @@ int HWSensorBase::FlushData(bool need_report_event)
 		 */
 		if (need_report_event && (report_at_once || ((type != SENSOR_TYPE_ACCELEROMETER) &&
 				(type != SENSOR_TYPE_GYROSCOPE))))
-			return SensorBase::FlushData(true);
-		else
-			return 0;
+			SensorBase::FlushData(true);
+
+		return 0;
 
 	} else
 		return -EINVAL;
@@ -507,7 +508,7 @@ int HWSensorBaseWithPollrate::SetDelay(int handle, int64_t period_ns, int64_t ti
 
 void HWSensorBaseWithPollrate::WriteDataToPipe()
 {
-	int err;
+	int err, retry = 3;
 	std::vector<int64_t>::iterator it;
 
 	if (!GetStatusOfHandle(sensor_t_data.handle))
@@ -530,16 +531,23 @@ void HWSensorBaseWithPollrate::WriteDataToPipe()
 				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));
+				while (retry) {
+					err = SensorBase::WritePipeWithPoll(&flush_event_data, sizeof(sensor_event),
+											POLL_TIMEOUT_FLUSH_EVENT);
+					if (err > 0)
+						break;
 
-				if (err < 0) {
-					ALOGE("%s: Writing flush_complete event failed, errno=%d", android_name, errno);
-					return;
+					retry--;
+					ALOGI("%s: Retry writing flush event data to pipe, retry_cnt: %d.", android_name, 3-retry);
 				}
 
+				if (retry ==  0)
+					ALOGE("%s: Failed to write HW flush_complete, err=%d", android_name, err);
+				else
+					ALOGD("write hw flush complete event to pipe succeed.");
+
 				last_timestamp = *it;
 				it = SensorBase::timestamp.erase(it);
-				ALOGD("write hw flush complete event to pipe succeed.");
 			} else
 				break;
 		}
@@ -547,9 +555,9 @@ void HWSensorBaseWithPollrate::WriteDataToPipe()
 
 	/* 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: Write sensor data failed, errno=%d", android_name, errno);
+		err =  SensorBase::WritePipeWithPoll(&sensor_event, sizeof(sensor_event), POLL_TIMEOUT_DATA_EVENT);
+		if (err <= 0) {
+			ALOGE("%s: Write sensor data failed.", android_name);
 			return;
 		}
 
diff --git a/libsensors_iio/src/SWSensorBase.cpp b/libsensors_iio/src/SWSensorBase.cpp
index 97ad409..83da79f 100644
--- a/libsensors_iio/src/SWSensorBase.cpp
+++ b/libsensors_iio/src/SWSensorBase.cpp
@@ -97,9 +97,9 @@ int SWSensorBase::FlushData(bool /*need_report_event*/)
 		}
 
 		if (report_event_at_once)
-			return SensorBase::FlushData(false);
-		else
-			return 0;
+			SensorBase::FlushData(false);
+
+		return 0;
 	} else
 		return -EINVAL;
 }
@@ -147,7 +147,7 @@ int SWSensorBaseWithPollrate::SetDelay(int handle, int64_t period_ns, int64_t ti
 
 void SWSensorBaseWithPollrate::WriteDataToPipe()
 {
-	int err;
+	int err, retry = 3;
 	std::vector<int64_t>::iterator it;
 
 	if (!GetStatusOfHandle(sensor_t_data.handle))
@@ -170,24 +170,31 @@ void SWSensorBaseWithPollrate::WriteDataToPipe()
 				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));
+				while (retry) {
+					err = SensorBase::WritePipeWithPoll(&flush_event_data, sizeof(sensor_event),
+											POLL_TIMEOUT_FLUSH_EVENT);
+					if (err > 0)
+						break;
 
-				if (err < 0) {
-					ALOGE("%s: Failed to write SW flush_complete, errno=%d", android_name, errno);
-					return;
+					retry--;
+					ALOGI("%s: Retry writing flush event data to pipe, retry_cnt: %d.", android_name, 3-retry);
 				}
 
+				if (retry ==  0)
+					ALOGE("%s: Failed to write SW flush_complete, err=%d", android_name, err);
+				else
+					ALOGD("SW flush_complete sent");
+
 				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) {
+		err = SensorBase::WritePipeWithPoll(&sensor_event, sizeof(sensor_event), POLL_TIMEOUT_DATA_EVENT);
+		if (err <= 0) {
 			ALOGE("%s: Failed to write sensor data - err=%d.", android_name, err);
 			return;
 		}
diff --git a/libsensors_iio/src/SensorBase.cpp b/libsensors_iio/src/SensorBase.cpp
index 992d3d4..bfee207 100644
--- a/libsensors_iio/src/SensorBase.cpp
+++ b/libsensors_iio/src/SensorBase.cpp
@@ -369,9 +369,43 @@ bool SensorBase::FillSensor_tData(struct sensor_t *data)
 	return true;
 }
 
-int SensorBase::FlushData(bool)
+int SensorBase::WritePipeWithPoll(sensors_event_t *event_data, int size, int timeout)
 {
 	int err;
+	struct pollfd poll_fd;
+
+	poll_fd.fd = android_pipe_fd;
+	poll_fd.events = POLLOUT;
+
+	err = poll(&poll_fd, (unsigned long)1, timeout);
+	if (err < 0) {
+		ALOGE("%s: error happened when polling pipe, errno: %d.", android_name, errno);
+		return err;
+	}
+
+	if (err == 0) {
+		ALOGE("%s: polling pipe timeout, timeout = %d.", android_name, timeout);
+		return err;
+	}
+
+	if (poll_fd.revents&POLLOUT) {
+		err = write(android_pipe_fd, event_data, size);
+		if (err <= 0) {
+			ALOGE("%s: Failed to write to pipe, timeout: %d, errno: %d.",
+				android_name, timeout, errno);
+			return err;
+		}
+	} else {
+		ALOGE("%s: polling was breaked by unexpected event: %d", android_name, poll_fd.revents);
+		return -EAGAIN;
+	}
+
+	return err;
+}
+
+int SensorBase::FlushData(bool)
+{
+	int err = 0, retry = 3;
 	sensors_event_t flush_event_data;
 
 	flush_event_data.sensor = 0;
@@ -381,14 +415,22 @@ int SensorBase::FlushData(bool)
 	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 flush event data to pipe.", android_name);
-		return err;
+	while (retry) {
+		err = WritePipeWithPoll(&flush_event_data, sizeof(sensor_event),
+						POLL_TIMEOUT_FLUSH_EVENT);
+		if (err > 0)
+			break;
+
+		retry--;
+		ALOGI("%s: Retry writing flush event data to pipe, retry_cnt: %d.", android_name, 3-retry);
 	}
 
-	ALOGD("SensorBase::FlushData completed.");
-	return 0;
+	if (retry == 0)
+		ALOGE("%s: Failed to write flush event data to pipe, err=%d.", android_name, err);
+	else
+		ALOGD("SensorBase::FlushData completed.");
+
+	return err;
 }
 
 void SensorBase::WriteDataToPipe()
@@ -399,8 +441,9 @@ void SensorBase::WriteDataToPipe()
 		return;
 
 	if (sensor_event.timestamp > last_data_timestamp) {
-		err = write(android_pipe_fd, &sensor_event, sizeof(sensor_event));
-		if (err < 0) {
+		err = WritePipeWithPoll(&sensor_event, sizeof(sensor_event),
+						POLL_TIMEOUT_DATA_EVENT);
+		if (err <= 0) {
 			ALOGE("%s: Failed to write sensor data to pipe.", android_name);
 			return;
 		}
diff --git a/libsensors_iio/src/SensorBase.h b/libsensors_iio/src/SensorBase.h
index 25cb79e..b20dea2 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 <poll.h>
 #include <vector>
 
 #include <hardware/sensors.h>
@@ -46,6 +47,9 @@
 
 #define SENSOR_BASE_ANDROID_NAME_MAX		(40)
 
+#define POLL_TIMEOUT_FLUSH_EVENT		(-1)
+#define POLL_TIMEOUT_DATA_EVENT		(1000)
+
 #define GAUSS_TO_UTESLA(x)			((x) * 100.0f)
 #define NS_TO_FREQUENCY(x)			(1E9 / x)
 #define FREQUENCY_TO_NS(x)			(1E9 / x)
@@ -142,6 +146,7 @@ public:
 
 	bool FillSensor_tData(struct sensor_t *data);
 
+	virtual int WritePipeWithPoll(sensors_event_t *event_data, int size, int timeout);
 	virtual int FlushData(bool);
 
 	virtual void ProcessData(SensorBaseData *data);