diff options
Diffstat (limited to 'libsensors_iio/src/SensorBase.cpp')
| -rw-r--r-- | libsensors_iio/src/SensorBase.cpp | 539 |
1 files changed, 0 insertions, 539 deletions
diff --git a/libsensors_iio/src/SensorBase.cpp b/libsensors_iio/src/SensorBase.cpp deleted file mode 100644 index bfee207..0000000 --- a/libsensors_iio/src/SensorBase.cpp +++ /dev/null @@ -1,539 +0,0 @@ -/* - * STMicroelectronics Sensor Base Class - * - * Copyright 2013-2015 STMicroelectronics Inc. - * Author: Denis Ciocca - <denis.ciocca@st.com> - * - * Licensed under the Apache License, Version 2.0 (the "License"). - */ - -#include <fcntl.h> -#include <assert.h> -#include <string.h> -#include <signal.h> -#include <time.h> - -#include "SensorBase.h" - -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) -{ - int i; - - if (strlen(name) + 1 > SENSOR_BASE_ANDROID_NAME_MAX) { - memcpy(android_name, name, SENSOR_BASE_ANDROID_NAME_MAX - 1); - android_name[SENSOR_BASE_ANDROID_NAME_MAX - 1] = '\0'; - } else - memcpy(android_name, name, strlen(name) + 1); - - memset(&sensor_t_data, 0, sizeof(sensor_t)); - memset(&sensor_event, 0, sizeof(sensors_event_t)); - memset(type_dependencies, 0, SENSOR_BASE_MAX_DEPENDENCY * sizeof(int)); - memset(sensors_pollrates, 0, ST_HAL_IIO_MAX_DEVICES * sizeof(int64_t)); - memset(last_timestap_pushed, 0, ST_HAL_IIO_MAX_DEVICES * sizeof(int64_t)); - - for (i = 0; i < ST_HAL_IIO_MAX_DEVICES; i++) - sensors_timeout[i] = INT64_MAX; - - sensor_event.version = sizeof(sensors_event_t); - sensor_event.sensor = handle; - sensor_event.type = type; - - sensor_t_data.name = android_name; - sensor_t_data.handle = handle; - sensor_t_data.type = type; - sensor_t_data.vendor = "STMicroelectronics"; - sensor_t_data.version = 1; - - real_pollrate = 0; - dependencies_num = 0; - last_data_timestamp = 0; - enabled_sensors_mask = 0; - sensors_to_trigger_num = 0; - type_sensor_need_trigger = -ENODEV; - sensors_to_push_data_num = 0; - num_data_axis = SENSOR_BASE_3AXIS; - - android_pipe_fd = pipe_data_fd; - - pthread_mutex_init(&mutext.trigger_mutex, NULL); - pthread_cond_init(&mutext.trigger_data_cond, NULL); - - valid_class = true; -} - -SensorBase::~SensorBase() -{ - int i; - - for (i = 0; i < (int)dependencies_num; i++) - DeAllocateBufferForDependencyData(i); -} - -bool SensorBase::IsValidClass() -{ - return valid_class; -} - -int SensorBase::GetHandle() -{ - return sensor_t_data.handle; -} - -int SensorBase::GetType() -{ - return sensor_t_data.type; -} - -int SensorBase::GetMaxFifoLenght() -{ - return sensor_t_data.fifoMaxEventCount; -} - -void SensorBase::SetBitEnableMask(int handle) -{ - enabled_sensors_mask |= (1ULL << handle); -} - -void SensorBase::ResetBitEnableMask(int handle) -{ - enabled_sensors_mask &= ~(1ULL << handle); -} - -char* SensorBase::GetName() -{ - return (char *)sensor_t_data.name; -} - -int SensorBase::Enable(int handle, bool enable) -{ - int err, i = 0; -#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_INFO) - bool old_status = GetStatus(); -#endif /* CONFIG_ST_HAL_DEBUG_LEVEL */ - - if ((enable && !GetStatus()) || (!enable && !GetStatusExcludeHandle(handle))) { - for (i = 0; i < (int)dependencies_num; i++) { - err = dependencies[i]->Enable(sensor_event.sensor, enable); - if (err < 0) - goto restore_enable_dependencies; - } - } - - if (enable) - SetBitEnableMask(handle); - else { - err = SetDelay(handle, 0, INT64_MAX); - if (err < 0) - goto restore_enable_dependencies; - - ResetBitEnableMask(handle); - } - -#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_INFO) - if (((old_status && !GetStatus()) || (!old_status && GetStatus())) && (sensor_t_data.type < SENSOR_TYPE_ST_CUSTOM_NO_SENSOR)) { - if (GetStatus()) - ALOGI("\"%s\": power-on (sensor type: %d).", sensor_t_data.name, sensor_t_data.type); - else - ALOGI("\"%s\": power-off (sensor type: %d).", sensor_t_data.name, sensor_t_data.type); - } -#endif /* CONFIG_ST_HAL_DEBUG_LEVEL */ - - return 0; - -restore_enable_dependencies: - for (i--; i >= 0; i--) - dependencies[i]->Enable(sensor_event.sensor, !enable); - - return err; -} - -bool SensorBase::GetStatusExcludeHandle(int handle) -{ - return (enabled_sensors_mask & ~(1ULL << handle)) > 0 ? true : false; -} - -bool SensorBase::GetStatusOfHandle(int handle) -{ - return (enabled_sensors_mask & (1ULL << handle)) > 0 ? true : false; -} - -bool SensorBase::GetStatus() -{ - return enabled_sensors_mask > 0 ? true : false; -} - -int SensorBase::SetDelay(int handle, int64_t period_ns, int64_t timeout) -{ - int err, i; - int64_t restore_min_timeout, restore_min_period_ms; - - restore_min_timeout = sensors_timeout[handle]; - restore_min_period_ms = sensors_pollrates[handle]; - - sensors_pollrates[handle] = period_ns; - sensors_timeout[handle] = timeout; - - for (i = 0; i < (int)dependencies_num; i++) { - err = dependencies[i]->SetDelay(sensor_event.sensor, GetMinPeriod(), GetMinTimeout()); - if (err < 0) - goto restore_delay_dependencies; - } - -#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_INFO) - if ((handle == sensor_t_data.handle) && (period_ns > 0)) - ALOGI("\"%s\": changed pollrate to %.2f Hz, timeout %lld ms (sensor type: %d).", - sensor_t_data.name, NS_TO_FREQUENCY((float)(uint64_t)period_ns), - (uint64_t)NS_TO_MS((uint64_t)timeout), sensor_t_data.type); -#endif /* CONFIG_ST_HAL_DEBUG_LEVEL */ - - return 0; - -restore_delay_dependencies: - sensors_pollrates[handle] = restore_min_period_ms; - sensors_timeout[handle] = restore_min_timeout; - - for (i--; i >= 0; i--) - dependencies[i]->SetDelay(sensor_event.sensor, GetMinPeriod(), GetMinTimeout()); - - return err; -} - -int64_t SensorBase::GetDelay() -{ - return sensors_pollrates[sensor_event.sensor]; -} - -int64_t SensorBase::GetRealPollrate() -{ - return real_pollrate; -} - -void SensorBase::GetDepenciesTypeList(int type[SENSOR_BASE_MAX_DEPENDENCY]) -{ - memcpy(type, type_dependencies, SENSOR_BASE_MAX_DEPENDENCY * sizeof(int)); -} - -trigger_mutex* SensorBase::GetMutexForTrigger() -{ - return &mutext; -} - -int SensorBase::GetSensorNeedTriggerType() -{ - return type_sensor_need_trigger; -} - -int SensorBase::AddSensorDependency(SensorBase *p) -{ - int err; - uint32_t sensor_wake_flag; - struct sensor_t dependecy_data; - - if (dependencies_num >= SENSOR_BASE_MAX_DEPENDENCY) { - ALOGE("%s: Failed to add dependency, too many dependencies.", android_name); - return -ENOMEM; - } - - err = AllocateBufferForDependencyData(dependencies_num, p->GetMaxFifoLenght()); - if (err < 0) - return err; - - err = p->AddSensorToDataPush(this); - if (err < 0) { - DeAllocateBufferForDependencyData(dependencies_num); - return err; - } - - p->FillSensor_tData(&dependecy_data); - sensor_t_data.power += dependecy_data.power; - - sensor_wake_flag = (dependecy_data.flags & SENSOR_FLAG_WAKE_UP); - if (dependencies_num == 0) - sensor_t_data.flags |= sensor_wake_flag; - else { - if (!sensor_wake_flag) - sensor_t_data.flags &= ~sensor_wake_flag; - } - - handle_remapping[p->GetHandle()] = dependencies_num; - dependencies[dependencies_num] = p; - dependencies_num++; - - return 0; -} - -void SensorBase::RemoveSensorDependency(SensorBase *p) -{ - int i; - - for (i = 0; i < (int)dependencies_num; i++) { - if (p == dependencies[i]) - break; - } - if (i == (int)dependencies_num) - return; - - DeAllocateBufferForDependencyData(i); - p->RemoveSensorToDataPush(this); - - for (; i < (int)dependencies_num - 1; i++) - dependencies[i] = dependencies[i + 1]; - - dependencies_num--; -} - -int SensorBase::AllocateBufferForDependencyData(int dependency_id, unsigned int max_fifo_len) -{ - circular_buffer_data[dependency_id] = new CircularBuffer(max_fifo_len < 2 ? 2 : max_fifo_len); - if (!circular_buffer_data[dependency_id]) - return -ENOMEM; - - return 0; -} - -void SensorBase::DeAllocateBufferForDependencyData(int dependency_id) -{ - delete circular_buffer_data[dependency_id]; -} - -int SensorBase::AddSensorToDataPush(SensorBase *t) -{ - int err; - - if (sensors_to_push_data_num >= SENSOR_BASE_MAX_DEPENDENCY) { - ALOGE("%s: Failed to add dependency data, too many sensors to push data.", android_name); - return -ENOMEM; - } - - sensors_to_push_data_type[sensors_to_push_data_num] = t->GetType(); - sensors_to_push_data[sensors_to_push_data_num] = t; - sensors_to_push_data_num++; - - return 0; -} - -void SensorBase::RemoveSensorToDataPush(SensorBase *t) -{ - int i; - - for (i = 0; i < (int)sensors_to_push_data_num; i++) { - if (t == sensors_to_push_data[i]) - break; - } - if (i == (int)sensors_to_push_data_num) - return; - - for (; i < (int)sensors_to_push_data_num - 1; i++) - sensors_to_push_data[i] = sensors_to_push_data[i + 1]; - - sensors_to_push_data_num--; -} - -int SensorBase::AddSensorToTrigger(SensorBase *t) -{ - int err; - - if (sensors_to_trigger_num >= SENSOR_BASE_MAX_DEPENDENCY) { - ALOGE("%s: Failed to add dependency, too many sensors to trigger.", android_name); - return -ENOMEM; - } - - sensors_to_trigger[sensors_to_trigger_num] = t; - sensors_to_trigger_num++; - - return 0; -} - -bool SensorBase::FillSensor_tData(struct sensor_t *data) -{ - memcpy(data, &sensor_t_data, sizeof(struct sensor_t)); - - if (sensor_t_data.type >= SENSOR_TYPE_ST_CUSTOM_NO_SENSOR) - return false; - - return true; -} - -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; - 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 = 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 flush event data to pipe, err=%d.", android_name, err); - else - ALOGD("SensorBase::FlushData completed."); - - return err; -} - -void SensorBase::WriteDataToPipe() -{ - int err; - - if (!GetStatusOfHandle(sensor_t_data.handle)) - return; - - if (sensor_event.timestamp > last_data_timestamp) { - 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; - } - - last_data_timestamp = sensor_event.timestamp; - } -} - -void SensorBase::ProcessData(SensorBaseData *data) -{ - int i; - trigger_mutex *dep_mutex; - - for (i = 0; i < (int)sensors_to_push_data_num; i++) { - if (sensors_to_push_data[i]->GetStatus()) - sensors_to_push_data[i]->ReceiveDataFromDependency(sensor_t_data.handle, data); - } - - for (i = 0; i < (int)sensors_to_trigger_num; i++) { - if (sensors_to_trigger[i]->GetStatus()) { - dep_mutex = sensors_to_trigger[i]->GetMutexForTrigger(); - pthread_mutex_lock(&dep_mutex->trigger_mutex); - pthread_cond_signal(&dep_mutex->trigger_data_cond); - pthread_mutex_unlock(&dep_mutex->trigger_mutex); - } - } -} - -void SensorBase::ProcessEvent(struct iio_event_data __attribute__((unused))*event_data) -{ - return; -} - -void SensorBase::TriggerEventReceived() -{ - return; -} - -void SensorBase::ReceiveDataFromDependency(int handle, SensorBaseData *data) -{ - if (data->timestamp >= last_timestap_pushed[handle]) { - circular_buffer_data[handle_remapping[handle]]->writeElement(data); - last_timestap_pushed[handle] = data->timestamp; - } - - return; -} - -int SensorBase::GetLatestValidDataFromDependency(int dependency_id, SensorBaseData *data) -{ - return circular_buffer_data[dependency_id]->readElement(data); -} - - -int64_t SensorBase::GetMinTimeout() -{ - int i; - int64_t min = INT64_MAX; - - for (i = 0; i < ST_HAL_IIO_MAX_DEVICES; i++) { - if (sensors_timeout[i] < min) - min = sensors_timeout[i]; - } - - return min; -} - -int64_t SensorBase::GetMinPeriod() -{ - int i; - int64_t min = INT64_MAX; - - for (i = 0; i < ST_HAL_IIO_MAX_DEVICES; i++) { - if ((sensors_pollrates[i] < min) && (sensors_pollrates[i] > 0)) - min = sensors_pollrates[i]; - } - - return min; -} - -void *SensorBase::ThreadWork(void *context) -{ - SensorBase *mypointer = (SensorBase *)context; - - mypointer->ThreadTask(); - - return mypointer; -} - -void SensorBase::ThreadTask() -{ - pthread_exit(NULL); -} |