diff options
-rw-r--r-- | libsensors_iio/src/Accelerometer.cpp | 2 | ||||
-rw-r--r-- | libsensors_iio/src/Android.mk | 4 | ||||
-rw-r--r-- | libsensors_iio/src/Gyroscope.cpp | 2 | ||||
-rw-r--r-- | libsensors_iio/src/HWSensorBase.cpp | 115 | ||||
-rw-r--r-- | libsensors_iio/src/HWSensorBase.h | 2 | ||||
-rw-r--r-- | libsensors_iio/src/Magnetometer.cpp | 2 | ||||
-rw-r--r-- | libsensors_iio/src/Pressure.cpp | 2 | ||||
-rw-r--r-- | libsensors_iio/src/SWSensorBase.cpp | 76 | ||||
-rw-r--r-- | libsensors_iio/src/SWSensorBase.h | 2 | ||||
-rw-r--r-- | libsensors_iio/src/SensorBase.cpp | 76 | ||||
-rw-r--r-- | libsensors_iio/src/SensorBase.h | 11 | ||||
-rw-r--r-- | libsensors_iio/src/SensorHAL.cpp | 9 | ||||
-rw-r--r-- | libsensors_iio/src/StepCounter.cpp | 2 |
13 files changed, 258 insertions, 47 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 924dc65..a910bd0 100644 --- a/libsensors_iio/src/HWSensorBase.cpp +++ b/libsensors_iio/src/HWSensorBase.cpp @@ -19,6 +19,7 @@ #include "HWSensorBase.h" #define DEFAULT_HRTIMER_PERIOD_NS (200000000) +#define LOCAL_REPORTING_MODE_MASK 6 /** @@ -314,11 +315,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 & LOCAL_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) { @@ -327,13 +368,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)))) + SensorBase::FlushData(true); + + return 0; + } else return -EINVAL; - if (base) - return SensorBase::FlushData(0); - else - return 0; } void HWSensorBase::ThreadTask() @@ -461,18 +509,61 @@ 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)) return; - if (sensor_event.timestamp >= (last_data_timestamp + real_pollrate)) { - err = write(android_pipe_fd, &sensor_event, sizeof(sensor_event)); - if (err < 0) { - ALOGE("%s: Failed to write sensor data to pipe.", android_name); + 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; + + while (retry) { + err = SensorBase::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); + } + + 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); + } 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 = SensorBase::WritePipeWithPoll(&sensor_event, sizeof(sensor_event), POLL_TIMEOUT_DATA_EVENT); + if (err <= 0) { + ALOGE("%s: Write sensor data failed.", android_name); 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..83da79f 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) + SensorBase::FlushData(false); + + return 0; } else return -EINVAL; - - return SensorBase::FlushData(0); } void SWSensorBase::ThreadTask() @@ -131,15 +147,55 @@ 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)) return; - if (sensor_event.timestamp >= (last_data_timestamp + real_pollrate)) { - err = write(android_pipe_fd, &sensor_event, sizeof(sensor_event)); - if (err < 0) { - ALOGE("%s: Failed to write sensor data to pipe.", android_name); + 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; + + while (retry) { + err = SensorBase::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); + } + + 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); + } else + break; + } + } + + if (sensor_event.timestamp >= (last_data_timestamp + real_pollrate * 9 / 10)) { + 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/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..bfee207 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,9 +369,43 @@ bool SensorBase::FillSensor_tData(struct sensor_t *data) return true; } -int SensorBase::FlushData(int) +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; @@ -367,13 +415,22 @@ int SensorBase::FlushData(int) 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); } - 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() @@ -384,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 9b73dcc..b20dea2 100644 --- a/libsensors_iio/src/SensorBase.h +++ b/libsensors_iio/src/SensorBase.h @@ -24,6 +24,8 @@ #include <time.h> #include <pthread.h> #include <errno.h> +#include <poll.h> +#include <vector> #include <hardware/sensors.h> #include <cutils/log.h> @@ -45,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) @@ -121,6 +126,7 @@ public: int GetType(); int GetMaxFifoLenght(); + std::vector<int64_t> timestamp; char* GetName(); virtual int Enable(int handle, bool enable); @@ -140,7 +146,8 @@ public: bool FillSensor_tData(struct sensor_t *data); - virtual int FlushData(int); + virtual int WritePipeWithPoll(sensors_event_t *event_data, int size, int timeout); + virtual int FlushData(bool); virtual void ProcessData(SensorBaseData *data); virtual void ProcessEvent(struct iio_event_data *event_data); @@ -154,4 +161,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; } |