| /* |
| * 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); |
| } |