1059 lines
27 KiB
C++
1059 lines
27 KiB
C++
/*
|
|
* STMicroelectronics HW SensorBase Class
|
|
*
|
|
* Copyright 2015-2016 STMicroelectronics Inc.
|
|
* Author: Denis Ciocca - <denis.ciocca@st.com>
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License").
|
|
*/
|
|
|
|
#define __STDC_LIMIT_MACROS
|
|
#define __STDINT_LIMITS
|
|
#define _BSD_SOURCE
|
|
|
|
#include <endian.h>
|
|
#include <fcntl.h>
|
|
#include <assert.h>
|
|
#include <string.h>
|
|
#include <signal.h>
|
|
#include <unistd.h>
|
|
|
|
#include "HWSensorBase.h"
|
|
|
|
#define HW_SENSOR_BASE_DEELAY_TRANSFER_DATA (500000000LL)
|
|
|
|
/**
|
|
* size_from_channelarray() - Calculate the storage size of a scan
|
|
* @channels: the channel info array.
|
|
* @num_channels: number of channels.
|
|
**/
|
|
static int size_from_channelarray(struct device_iio_info_channel *channels,
|
|
int num_channels)
|
|
{
|
|
int bytes = 0, i;
|
|
|
|
for (i = 0; i < num_channels; i++) {
|
|
channels[i].location = 0;
|
|
|
|
if (channels[i].bytes == 0)
|
|
continue;
|
|
|
|
if (bytes % channels[i].bytes == 0)
|
|
channels[i].location = bytes;
|
|
else
|
|
channels[i].location = bytes -
|
|
(bytes % channels[i].bytes) + channels[i].bytes;
|
|
|
|
bytes = channels[i].location + channels[i].bytes;
|
|
}
|
|
|
|
return bytes;
|
|
}
|
|
|
|
/**
|
|
* process_2byte_received() - Return channel data from 2 byte
|
|
* @input: 2 byte of data received from buffer channel.
|
|
* @info: information about channel structure.
|
|
**/
|
|
static float process_2byte_received(int input,
|
|
struct device_iio_info_channel *info)
|
|
{
|
|
float res;
|
|
int16_t val;
|
|
|
|
if (info->be)
|
|
input = be16toh((uint16_t)input);
|
|
else
|
|
input = le16toh((uint16_t)input);
|
|
|
|
val = input >> info->shift;
|
|
|
|
if (info->sign) {
|
|
val &= (1 << info->bits_used) - 1;
|
|
val = (int16_t)(val << (16 - info->bits_used)) >> (16 - info->bits_used);
|
|
res = (float)val;
|
|
} else {
|
|
val &= (1 << info->bits_used) - 1;
|
|
res = (float)((uint16_t)val);
|
|
}
|
|
|
|
return ((res + info->offset) * info->scale);
|
|
}
|
|
|
|
/**
|
|
* process_3byte_received() - Return channel data from 3 byte
|
|
* @input: 3 byte of data received from buffer channel.
|
|
* @info: information about channel structure.
|
|
**/
|
|
static float process_3byte_received(int input,
|
|
struct device_iio_info_channel *info)
|
|
{
|
|
float res;
|
|
int32_t val;
|
|
|
|
if (info->be)
|
|
input = be32toh((uint32_t)input);
|
|
else
|
|
input = le32toh((uint32_t)input);
|
|
|
|
val = input >> info->shift;
|
|
if (info->sign) {
|
|
val &= (1 << info->bits_used) - 1;
|
|
val = (int32_t)(val << (24 - info->bits_used)) >> (24 - info->bits_used);
|
|
res = (float)val;
|
|
} else {
|
|
val &= (1 << info->bits_used) - 1;
|
|
res = (float)((uint32_t)val);
|
|
}
|
|
|
|
return ((res + info->offset) * info->scale);
|
|
}
|
|
|
|
/**
|
|
* process_scan() - This functions use channels device information to build data
|
|
* @hw_sensor: pointer to current hardware sensor.
|
|
* @data: sensor data of all channels read from buffer.
|
|
* @channels: information about channel structure.
|
|
* @num_channels: number of channels of the sensor.
|
|
**/
|
|
static int ProcessScanData(uint8_t *data,
|
|
struct device_iio_info_channel *channels,
|
|
int num_channels,
|
|
SensorBaseData *sensor_out_data)
|
|
{
|
|
int k;
|
|
|
|
for (k = 0; k < num_channels; k++) {
|
|
sensor_out_data->offset[k] = 0;
|
|
|
|
switch (channels[k].bytes) {
|
|
case 1:
|
|
sensor_out_data->raw[k] = *(uint8_t *)(data + channels[k].location);
|
|
break;
|
|
case 2:
|
|
sensor_out_data->raw[k] = process_2byte_received(*(uint16_t *)
|
|
(data + channels[k].location), &channels[k]);
|
|
break;
|
|
case 3:
|
|
sensor_out_data->raw[k] = process_3byte_received(*(uint32_t *)
|
|
(data + channels[k].location), &channels[k]);
|
|
break;
|
|
case 4:
|
|
uint32_t val;
|
|
|
|
if (channels[k].be)
|
|
val = be32toh(*(uint32_t *)
|
|
(data + channels[k].location));
|
|
else
|
|
val = le32toh(*(uint32_t *)
|
|
(data + channels[k].location));
|
|
val >>= channels[k].shift;
|
|
val &= channels[k].mask;
|
|
if (channels[k].sign) {
|
|
sensor_out_data->raw[k] = ((float)(int32_t)val +
|
|
channels[k].offset) * channels[k].scale;
|
|
} else {
|
|
sensor_out_data->raw[k] = ((float)val +
|
|
channels[k].offset) * channels[k].scale;
|
|
}
|
|
|
|
break;
|
|
case 8:
|
|
if (channels[k].sign) {
|
|
int64_t val = *(int64_t *)(data + channels[k].location);
|
|
if ((val >> channels[k].bits_used) & 1)
|
|
val = (val & channels[k].mask) | ~channels[k].mask;
|
|
|
|
if ((channels[k].scale == 1.0f) && (channels[k].offset == 0.0f)) {
|
|
sensor_out_data->timestamp = val;
|
|
} else {
|
|
sensor_out_data->raw[k] = (((float)val +
|
|
channels[k].offset) * channels[k].scale);
|
|
}
|
|
} else {
|
|
uint64_t val = *(uint64_t *)(data + channels[k].location);
|
|
sensor_out_data->raw[k] = val;
|
|
}
|
|
|
|
break;
|
|
default:
|
|
return -EINVAL;
|
|
}
|
|
}
|
|
|
|
return num_channels;
|
|
}
|
|
|
|
#if (CONFIG_ST_HAL_ANDROID_VERSION >= ST_HAL_MARSHMALLOW_VERSION)
|
|
static int ProcessInjectionData(float *data,
|
|
struct device_iio_info_channel *channels,
|
|
int num_channels,
|
|
uint8_t *out_data,
|
|
int64_t timestamp)
|
|
{
|
|
int k;
|
|
|
|
for (k = 0; k < num_channels; k++) {
|
|
switch (channels[k].bytes) {
|
|
case 1:
|
|
*(uint8_t *)(out_data + channels[k].location) = data[k];
|
|
break;
|
|
|
|
case 2:
|
|
*(uint16_t *)(out_data + channels[k].location) =
|
|
(int16_t)(data[k] / channels[k].scale);
|
|
break;
|
|
|
|
case 8:
|
|
*(int64_t *)(out_data + channels[k].location) = timestamp;
|
|
break;
|
|
|
|
default:
|
|
return -EINVAL;
|
|
}
|
|
}
|
|
|
|
return num_channels;
|
|
}
|
|
#endif /* CONFIG_ST_HAL_ANDROID_VERSION */
|
|
|
|
|
|
HWSensorBase::HWSensorBase(HWSensorBaseCommonData *data, const char *name,
|
|
int handle, int sensor_type, unsigned int hw_fifo_len,
|
|
float power_consumption) : SensorBase(name, handle, sensor_type)
|
|
{
|
|
int err;
|
|
char *buffer_path;
|
|
|
|
memcpy(&common_data, data, sizeof(common_data));
|
|
|
|
sensor_t_data.power = power_consumption;
|
|
sensor_t_data.fifoMaxEventCount = hw_fifo_len;
|
|
|
|
#ifdef CONFIG_ST_HAL_FACTORY_CALIBRATION
|
|
memset(factory_offset, 0, 3 * sizeof(float));
|
|
|
|
factory_scale[0] = 1.0f;
|
|
factory_scale[1] = 1.0f;
|
|
factory_scale[2] = 1.0f;
|
|
factory_calibration_updated = false;
|
|
#endif /* CONFIG_ST_HAL_FACTORY_CALIBRATION */
|
|
|
|
#ifdef CONFIG_ST_HAL_HAS_SELFTEST_FUNCTIONS
|
|
selftest.available = 0;
|
|
#endif /* CONFIG_ST_HAL_HAS_SELFTEST_FUNCTIONS */
|
|
|
|
scan_size = size_from_channelarray(common_data.channels,
|
|
common_data.num_channels);
|
|
|
|
err = asprintf(&buffer_path, "/dev/iio:device%d", data->device_iio_dev_num);
|
|
if (err <= 0) {
|
|
ALOGE("%s: Failed to allocate iio device path string.",
|
|
GetName());
|
|
goto invalid_this_class;
|
|
}
|
|
|
|
pollfd_iio[0].fd = open(buffer_path, O_RDONLY | O_NONBLOCK);
|
|
if (pollfd_iio[0].fd < 0) {
|
|
ALOGE("%s: Failed to open iio char device (%s)." ,
|
|
GetName(), buffer_path);
|
|
goto free_buffer_path;
|
|
}
|
|
|
|
pollfd_iio[0].events = POLLIN;
|
|
|
|
if (!ioctl(pollfd_iio[0].fd, _IOR('i', 0x90, int), &pollfd_iio[1].fd)) {
|
|
pollfd_iio[1].events = POLLIN;
|
|
has_event_channels = true;
|
|
} else {
|
|
has_event_channels= false;
|
|
}
|
|
|
|
#if (CONFIG_ST_HAL_ANDROID_VERSION >= ST_HAL_MARSHMALLOW_VERSION)
|
|
err = device_iio_utils::support_injection_mode(common_data.device_iio_sysfs_path);
|
|
switch (err) {
|
|
case 0:
|
|
#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_INFO)
|
|
ALOGD("\"%s\": injection mode available, sensor is an injector.", GetName());
|
|
#endif /* CONFIG_ST_HAL_DEBUG_INFO */
|
|
sensor_t_data.flags |= DATA_INJECTION_MASK;
|
|
injection_mode = SENSOR_INJECTOR;
|
|
break;
|
|
|
|
case 1:
|
|
#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_INFO)
|
|
ALOGD("\"%s\": injection mode available, sensor is injected.", GetName());
|
|
#endif /* CONFIG_ST_HAL_DEBUG_INFO */
|
|
sensor_t_data.flags |= DATA_INJECTION_MASK;
|
|
injection_mode = SENSOR_INJECTED;
|
|
break;
|
|
|
|
default:
|
|
#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_INFO)
|
|
ALOGD("\"%s\": injection mode not available.", GetName());
|
|
#endif /* CONFIG_ST_HAL_DEBUG_INFO */
|
|
sensor_t_data.flags &= ~DATA_INJECTION_MASK;
|
|
break;
|
|
}
|
|
#endif /* CONFIG_ST_HAL_ANDROID_VERSION */
|
|
|
|
#if (CONFIG_ST_HAL_ANDROID_VERSION >= ST_HAL_PIE_VERSION)
|
|
#if (CONFIG_ST_HAL_ADDITIONAL_INFO_ENABLED)
|
|
sensor_t_data.flags |= SENSOR_FLAG_ADDITIONAL_INFO;
|
|
supportsSensorAdditionalInfo = false;
|
|
#endif /* CONFIG_ST_HAL_ADDITIONAL_INFO_ENABLED */
|
|
#endif /* CONFIG_ST_HAL_ANDROID_VERSION */
|
|
free(buffer_path);
|
|
|
|
return;
|
|
|
|
free_buffer_path:
|
|
free(buffer_path);
|
|
invalid_this_class:
|
|
InvalidThisClass();
|
|
}
|
|
|
|
HWSensorBase::~HWSensorBase()
|
|
{
|
|
if (!IsValidClass())
|
|
return;
|
|
|
|
close(pollfd_iio[0].fd);
|
|
close(pollfd_iio[1].fd);
|
|
}
|
|
|
|
#ifdef CONFIG_ST_HAL_HAS_SELFTEST_FUNCTIONS
|
|
void HWSensorBase::GetSelfTestAvailable()
|
|
{
|
|
int err;
|
|
|
|
err = device_iio_utils::get_selftest_available(common_data.device_iio_sysfs_path,
|
|
selftest.mode);
|
|
if (err < 0)
|
|
return;
|
|
|
|
selftest.available = err;
|
|
}
|
|
|
|
selftest_status HWSensorBase::ExecuteSelfTest()
|
|
{
|
|
int status;
|
|
|
|
if (selftest.available == 0)
|
|
return NOT_AVAILABLE;
|
|
|
|
status = device_iio_utils::execute_selftest(common_data.device_iio_sysfs_path,
|
|
&selftest.mode[0][0]);
|
|
if (status < 0) {
|
|
ALOGE("\"%s\": failed to execute selftest procedure. (errno: %d)",
|
|
GetName(), status);
|
|
return GENERIC_ERROR;
|
|
}
|
|
|
|
if (status == 0)
|
|
return FAILURE;
|
|
|
|
return PASS;
|
|
}
|
|
#endif /* CONFIG_ST_HAL_HAS_SELFTEST_FUNCTIONS */
|
|
|
|
int HWSensorBase::WriteBufferLenght(unsigned int buf_len)
|
|
{
|
|
int err;
|
|
unsigned int hw_buf_fifo_len;
|
|
|
|
if (buf_len == 0)
|
|
hw_buf_fifo_len = 1;
|
|
else
|
|
hw_buf_fifo_len = buf_len;
|
|
|
|
err = device_iio_utils::set_hw_fifo_watermark(common_data.device_iio_sysfs_path,
|
|
hw_buf_fifo_len);
|
|
if (err < 0) {
|
|
ALOGE("%s: Failed to write hw fifo watermark.", GetName());
|
|
return err;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int HWSensorBase::Enable(int handle, bool enable, bool lock_en_mutex)
|
|
{
|
|
int err = 0;
|
|
bool old_status, old_status_no_handle;
|
|
|
|
|
|
if (lock_en_mutex)
|
|
pthread_mutex_lock(&enable_mutex);
|
|
|
|
old_status = GetStatus(false);
|
|
old_status_no_handle = GetStatusExcludeHandle(handle);
|
|
|
|
err = SensorBase::Enable(handle, enable, false);
|
|
if (err < 0)
|
|
goto unlock_mutex;
|
|
|
|
if ((enable && !old_status) || (!enable && !old_status_no_handle)) {
|
|
err = device_iio_utils::enable_sensor(common_data.device_iio_sysfs_path,
|
|
GetStatus(false));
|
|
if (err < 0) {
|
|
ALOGE("%s: Failed to enable iio sensor device.", GetName());
|
|
goto restore_status_enable;
|
|
}
|
|
|
|
if (enable)
|
|
sensor_global_enable = android::elapsedRealtimeNano();
|
|
else
|
|
sensor_global_disable = android::elapsedRealtimeNano();
|
|
}
|
|
|
|
if (sensor_t_data.handle == handle) {
|
|
if (enable) {
|
|
sensor_my_enable = android::elapsedRealtimeNano();
|
|
#if (CONFIG_ST_HAL_ANDROID_VERSION >= ST_HAL_PIE_VERSION)
|
|
#if (CONFIG_ST_HAL_ADDITIONAL_INFO_ENABLED)
|
|
ALOGD("%s:SAINFO Report: ENABLE.", GetName());
|
|
WriteSAIReportToPipe();
|
|
ALOGD("%s : SAI ENABLE Report.", GetName());
|
|
#endif /* CONFIG_ST_HAL_ADDITIONAL_INFO_ENABLED */
|
|
#endif /* CONFIG_ST_HAL_ANDROID_VERSION */
|
|
} else
|
|
sensor_my_disable = android::elapsedRealtimeNano();
|
|
}
|
|
|
|
if (lock_en_mutex)
|
|
pthread_mutex_unlock(&enable_mutex);
|
|
|
|
return 0;
|
|
|
|
restore_status_enable:
|
|
SensorBase::Enable(handle, !enable, false);
|
|
unlock_mutex:
|
|
if (lock_en_mutex)
|
|
pthread_mutex_unlock(&enable_mutex);
|
|
|
|
return err;
|
|
}
|
|
|
|
int HWSensorBase::SetDelay(int __attribute__((unused))handle,
|
|
int64_t __attribute__((unused))period_ns,
|
|
int64_t timeout, bool lock_en_mutex)
|
|
{
|
|
int err;
|
|
unsigned int buf_len;
|
|
|
|
if (timeout < INT64_MAX) {
|
|
if ((sensor_t_data.fifoMaxEventCount == 0) && (timeout > 0))
|
|
return -EINVAL;
|
|
} else
|
|
return 0;
|
|
|
|
if (lock_en_mutex)
|
|
pthread_mutex_lock(&enable_mutex);
|
|
|
|
if (sensor_t_data.fifoMaxEventCount > 0) {
|
|
buf_len = timeout / FREQUENCY_TO_NS(1);
|
|
if (buf_len > sensor_t_data.fifoMaxEventCount)
|
|
buf_len = sensor_t_data.fifoMaxEventCount;
|
|
|
|
err = WriteBufferLenght(buf_len);
|
|
if (err < 0)
|
|
goto mutex_unlock;
|
|
}
|
|
|
|
#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_INFO)
|
|
ALOGD("\"%s\": changed pollrate to timeout=%" PRIu64 "ms (sensor type: %d).",
|
|
sensor_t_data.name,
|
|
(uint64_t)NS_TO_MS((uint64_t)timeout),
|
|
sensor_t_data.type);
|
|
#endif /* CONFIG_ST_HAL_DEBUG_INFO */
|
|
|
|
if (lock_en_mutex)
|
|
pthread_mutex_unlock(&enable_mutex);
|
|
|
|
return 0;
|
|
|
|
mutex_unlock:
|
|
if (lock_en_mutex)
|
|
pthread_mutex_unlock(&enable_mutex);
|
|
|
|
return err;
|
|
}
|
|
|
|
int HWSensorBase::AddSensorDependency(SensorBase *p)
|
|
{
|
|
int dependency_id, err;
|
|
|
|
dependency_id = SensorBase::AddSensorDependency(p);
|
|
if (dependency_id < 0)
|
|
return dependency_id;
|
|
|
|
err = AllocateBufferForDependencyData((DependencyID)dependency_id,
|
|
p->GetMaxFifoLenght());
|
|
if (err < 0)
|
|
return err;
|
|
|
|
return 0;
|
|
}
|
|
|
|
void HWSensorBase::RemoveSensorDependency(SensorBase *p)
|
|
{
|
|
DeAllocateBufferForDependencyData(GetDependencyIDFromHandle(p->GetHandle()));
|
|
SensorBase::RemoveSensorDependency(p);
|
|
}
|
|
|
|
int HWSensorBase::ApplyFactoryCalibrationData(char *filename,
|
|
time_t *last_modification)
|
|
{
|
|
#ifdef CONFIG_ST_HAL_FACTORY_CALIBRATION
|
|
int err;
|
|
struct stat file_stat;
|
|
FILE *calibration_file;
|
|
double calibration_timediff;
|
|
|
|
err = stat(filename, &file_stat);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
calibration_timediff = difftime(file_stat.st_mtime, *last_modification);
|
|
if (calibration_timediff > 0) {
|
|
factory_calibration_updated = true;
|
|
*last_modification = file_stat.st_mtime;
|
|
}
|
|
|
|
calibration_file = fopen(filename, "r");
|
|
if (!calibration_file)
|
|
return -errno;
|
|
|
|
err = fscanf(calibration_file, "%f,%f,%f\n",
|
|
&factory_offset[0],
|
|
&factory_offset[1],
|
|
&factory_offset[2]);
|
|
if (err < 0) {
|
|
fclose(calibration_file);
|
|
return err;
|
|
}
|
|
|
|
err = fscanf(calibration_file, "%f,%f,%f\n",
|
|
&factory_scale[0],
|
|
&factory_scale[1],
|
|
&factory_scale[2]);
|
|
if (err < 0)
|
|
ALOGW("\"%s\": Failed to read factory scale values, it will be used default values.",
|
|
GetName());
|
|
|
|
fclose(calibration_file);
|
|
|
|
return 0;
|
|
#else /* CONFIG_ST_HAL_FACTORY_CALIBRATION */
|
|
(void)filename;
|
|
(void)last_modification;
|
|
|
|
return 0;
|
|
#endif /* CONFIG_ST_HAL_FACTORY_CALIBRATION */
|
|
}
|
|
|
|
void HWSensorBase::ProcessEvent(struct device_iio_events *event_data)
|
|
{
|
|
uint8_t event_type, event_dir;
|
|
|
|
event_type = ((event_data->event_id >> 56) & 0xFF);
|
|
event_dir = ((event_data->event_id >> 48) & 0x7F);
|
|
|
|
if ((event_type == DEVICE_IIO_EV_TYPE_FIFO_FLUSH) ||
|
|
(event_dir == DEVICE_IIO_EV_DIR_FIFO_DATA))
|
|
ProcessFlushData(sensor_t_data.handle,
|
|
event_data->event_timestamp);
|
|
}
|
|
|
|
|
|
int HWSensorBase::FlushData(int handle, bool lock_en_mutex)
|
|
{
|
|
int err;
|
|
unsigned int i;
|
|
|
|
if (lock_en_mutex)
|
|
pthread_mutex_lock(&enable_mutex);
|
|
|
|
if (GetStatus(false)) {
|
|
err = flush_requested.writeElement(handle);
|
|
if (err < 0)
|
|
goto unlock_mutex;
|
|
|
|
if ((GetMinTimeout(false) > 0) &&
|
|
(GetMinTimeout(false) < INT64_MAX)) {
|
|
for (i = 0; i < dependencies.num; i++)
|
|
dependencies.sb[i]->FlushData(sensor_t_data.handle, true);
|
|
|
|
err = device_iio_utils::hw_fifo_flush(common_data.device_iio_sysfs_path);
|
|
if (err < 0) {
|
|
ALOGE("%s: Failed to flush hw fifo.",
|
|
GetName());
|
|
goto unlock_mutex;
|
|
}
|
|
} else
|
|
ProcessFlushData(sensor_t_data.handle, 0);
|
|
} else
|
|
goto unlock_mutex;
|
|
|
|
if (lock_en_mutex)
|
|
pthread_mutex_unlock(&enable_mutex);
|
|
|
|
return 0;
|
|
|
|
unlock_mutex:
|
|
if (lock_en_mutex)
|
|
pthread_mutex_unlock(&enable_mutex);
|
|
|
|
return -EINVAL;
|
|
}
|
|
|
|
void HWSensorBase::ProcessFlushData(int __attribute__((unused))handle,
|
|
int64_t timestamp)
|
|
{
|
|
unsigned int i;
|
|
int err, flush_handle;
|
|
|
|
flush_handle = flush_requested.readElement();
|
|
if (flush_handle < 0)
|
|
return;
|
|
|
|
pthread_mutex_lock(&sample_in_processing_mutex);
|
|
|
|
if (timestamp > sample_in_processing_timestamp) {
|
|
err = flush_stack.writeElement(flush_handle, timestamp);
|
|
if (err < 0)
|
|
ALOGE("%s: Failed to write Flush event into stack.",
|
|
GetName());
|
|
} else {
|
|
if (flush_handle == sensor_t_data.handle) {
|
|
WriteFlushEventToPipe();
|
|
#if (CONFIG_ST_HAL_ANDROID_VERSION >= ST_HAL_PIE_VERSION)
|
|
#if (CONFIG_ST_HAL_ADDITIONAL_INFO_ENABLED)
|
|
ALOGD("%s:SAINFO Report: FLUSH.", GetName());
|
|
WriteSAIReportToPipe();
|
|
ALOGD("%s : SAI FLUSH Report.", GetName());
|
|
#endif /* CONFIG_ST_HAL_ADDITIONAL_INFO_ENABLED */
|
|
#endif /* CONFIG_ST_HAL_ANDROID_VERSION */
|
|
} else {
|
|
for (i = 0; i < push_data.num; i++)
|
|
push_data.sb[i]->ProcessFlushData(flush_handle,
|
|
timestamp);
|
|
}
|
|
}
|
|
|
|
pthread_mutex_unlock(&sample_in_processing_mutex);
|
|
}
|
|
|
|
|
|
void HWSensorBase::ThreadDataTask()
|
|
{
|
|
uint8_t *data;
|
|
unsigned int hw_fifo_len;
|
|
SensorBaseData sensor_data;
|
|
int err, i, read_size, flush_handle;
|
|
int64_t timestamp_flush, timestamp_odr_switch, new_pollrate = 0;
|
|
int64_t old_pollrate = 0;
|
|
|
|
if (sensor_t_data.fifoMaxEventCount > 0)
|
|
hw_fifo_len = sensor_t_data.fifoMaxEventCount;
|
|
else
|
|
hw_fifo_len = 1;
|
|
|
|
data = (uint8_t *)malloc(hw_fifo_len * scan_size * HW_SENSOR_BASE_DEFAULT_IIO_BUFFER_LEN * sizeof(uint8_t));
|
|
if (!data) {
|
|
ALOGE("%s: Failed to allocate sensor data buffer (%u %d).",
|
|
GetName(), hw_fifo_len, (int)scan_size);
|
|
return;
|
|
}
|
|
|
|
while (true) {
|
|
err = poll(&pollfd_iio[0], 1, -1);
|
|
if (err <= 0)
|
|
continue;
|
|
|
|
if (pollfd_iio[0].revents & POLLIN) {
|
|
read_size = read(pollfd_iio[0].fd,
|
|
data,
|
|
hw_fifo_len * scan_size * HW_SENSOR_BASE_DEFAULT_IIO_BUFFER_LEN);
|
|
if (read_size <= 0) {
|
|
ALOGE("%s: Failed to read data from iio char device.",
|
|
GetName());
|
|
continue;
|
|
}
|
|
|
|
for (i = 0; i < (read_size / scan_size); i++) {
|
|
err = ProcessScanData(data + (i * scan_size), common_data.channels, common_data.num_channels, &sensor_data);
|
|
if (err < 0)
|
|
continue;
|
|
|
|
pthread_mutex_lock(&sample_in_processing_mutex);
|
|
sample_in_processing_timestamp = sensor_data.timestamp;
|
|
pthread_mutex_unlock(&sample_in_processing_mutex);
|
|
|
|
timestamp_odr_switch = odr_switch.readLastElement(&new_pollrate);
|
|
if (sensor_data.timestamp > timestamp_odr_switch) {
|
|
sensor_data.pollrate_ns = new_pollrate;
|
|
old_pollrate = new_pollrate;
|
|
odr_switch.removeLastElement();
|
|
} else {
|
|
sensor_data.pollrate_ns = old_pollrate;
|
|
}
|
|
|
|
flush_handle = flush_stack.readLastElement(×tamp_flush);
|
|
if ((flush_handle >= 0) && (timestamp_flush <= sensor_data.timestamp)) {
|
|
sensor_data.flush_event_handle = flush_handle;
|
|
flush_stack.removeLastElement();
|
|
} else {
|
|
sensor_data.flush_event_handle = -1;
|
|
}
|
|
|
|
ProcessData(&sensor_data);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void HWSensorBase::ThreadEventsTask()
|
|
{
|
|
int err, i, read_size;
|
|
struct device_iio_events event_data[10];
|
|
|
|
while (true) {
|
|
err = poll(&pollfd_iio[1], 1, -1);
|
|
if (err <= 0)
|
|
continue;
|
|
|
|
if (pollfd_iio[1].revents & POLLIN) {
|
|
read_size = read(pollfd_iio[1].fd, event_data,
|
|
10 * sizeof(struct device_iio_events));
|
|
if (read_size <= 0) {
|
|
ALOGE("%s: Failed to read event data from iio char device.",
|
|
GetName());
|
|
continue;
|
|
}
|
|
|
|
for (i = 0; i < (int)(read_size / sizeof(struct device_iio_events)); i++)
|
|
ProcessEvent(&event_data[i]);
|
|
}
|
|
}
|
|
}
|
|
|
|
#if (CONFIG_ST_HAL_ANDROID_VERSION >= ST_HAL_MARSHMALLOW_VERSION)
|
|
int HWSensorBase::InjectionMode(bool enable)
|
|
{
|
|
int err;
|
|
|
|
switch (injection_mode) {
|
|
case SENSOR_INJECTION_NONE:
|
|
break;
|
|
|
|
case SENSOR_INJECTOR:
|
|
if (enable) {
|
|
injection_data = (uint8_t *)malloc(sizeof(uint8_t) * scan_size);
|
|
if (!injection_data)
|
|
return -ENOMEM;
|
|
} else
|
|
free(injection_data);
|
|
|
|
err = device_iio_utils::set_injection_mode(common_data.device_iio_sysfs_path,
|
|
enable);
|
|
if (err < 0) {
|
|
ALOGE("%s: Failed to switch injection mode.",
|
|
GetName());
|
|
free(injection_data);
|
|
return err;
|
|
}
|
|
|
|
break;
|
|
|
|
case SENSOR_INJECTED:
|
|
break;
|
|
|
|
default:
|
|
return -EINVAL;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int HWSensorBase::InjectSensorData(const sensors_event_t *data)
|
|
{
|
|
int err;
|
|
device_iio_chan_type_t device_iio_sensor_type;
|
|
|
|
err = ProcessInjectionData((float *)data->data, common_data.channels,
|
|
common_data.num_channels, injection_data,
|
|
data->timestamp);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
switch (sensor_t_data.type) {
|
|
case SENSOR_TYPE_ACCELEROMETER:
|
|
device_iio_sensor_type = DEVICE_IIO_ACC;
|
|
break;
|
|
case SENSOR_TYPE_GEOMAGNETIC_FIELD:
|
|
device_iio_sensor_type = DEVICE_IIO_MAGN;
|
|
break;
|
|
case SENSOR_TYPE_GYROSCOPE:
|
|
device_iio_sensor_type = DEVICE_IIO_GYRO;
|
|
break;
|
|
default:
|
|
return -EINVAL;
|
|
}
|
|
|
|
return device_iio_utils::inject_data(common_data.device_iio_sysfs_path,
|
|
injection_data,
|
|
scan_size,
|
|
device_iio_sensor_type);
|
|
}
|
|
#endif /* CONFIG_ST_HAL_ANDROID_VERSION */
|
|
|
|
HWSensorBaseWithPollrate::HWSensorBaseWithPollrate(HWSensorBaseCommonData *data,
|
|
const char *name, struct device_iio_sampling_freqs *sfa,
|
|
int handle, int sensor_type, unsigned int hw_fifo_len,
|
|
float power_consumption) : HWSensorBase(data, name, handle,
|
|
sensor_type,
|
|
hw_fifo_len,
|
|
power_consumption)
|
|
{
|
|
unsigned int i;
|
|
float max_sampling_frequency = 0.0f;
|
|
#if (CONFIG_ST_HAL_ANDROID_VERSION > ST_HAL_KITKAT_VERSION)
|
|
float min_sampling_frequency = FLT_MAX;
|
|
#endif /* CONFIG_ST_HAL_ANDROID_VERSION */
|
|
|
|
memcpy(&sampling_frequency_available, sfa,
|
|
sizeof(sampling_frequency_available));
|
|
|
|
for (i = 0; i < sfa->length; i++) {
|
|
if ((max_sampling_frequency < sfa->freq[i]) &&
|
|
(sfa->freq[i] <= CONFIG_ST_HAL_MAX_SAMPLING_FREQUENCY))
|
|
max_sampling_frequency = sfa->freq[i];
|
|
|
|
#if (CONFIG_ST_HAL_ANDROID_VERSION > ST_HAL_KITKAT_VERSION)
|
|
if (min_sampling_frequency > sfa->freq[i])
|
|
min_sampling_frequency = sfa->freq[i];
|
|
#endif /* CONFIG_ST_HAL_ANDROID_VERSION */
|
|
}
|
|
|
|
sensor_t_data.minDelay = FREQUENCY_TO_US(max_sampling_frequency);
|
|
#if (CONFIG_ST_HAL_ANDROID_VERSION > ST_HAL_KITKAT_VERSION)
|
|
sensor_t_data.maxDelay = FREQUENCY_TO_US(min_sampling_frequency);
|
|
#endif /* CONFIG_ST_HAL_ANDROID_VERSION */
|
|
}
|
|
|
|
HWSensorBaseWithPollrate::~HWSensorBaseWithPollrate()
|
|
{
|
|
|
|
}
|
|
|
|
int HWSensorBaseWithPollrate::SetDelay(int handle, int64_t period_ns,
|
|
int64_t timeout, bool lock_en_mutex)
|
|
{
|
|
int err, i;
|
|
#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_INFO)
|
|
bool message = false;
|
|
#endif /* CONFIG_ST_HAL_DEBUG_INFO */
|
|
unsigned int sampling_frequency, buf_len;
|
|
int64_t min_pollrate_ns, min_timeout_ns = 0, timestamp;
|
|
|
|
if (lock_en_mutex)
|
|
pthread_mutex_lock(&enable_mutex);
|
|
|
|
if ((sensors_pollrates[handle] == period_ns) &&
|
|
(sensors_timeout[handle] == timeout)) {
|
|
err = 0;
|
|
goto mutex_unlock;
|
|
}
|
|
|
|
if ((period_ns > 0) && (timeout < INT64_MAX)) {
|
|
#if (CONFIG_ST_HAL_ANDROID_VERSION > ST_HAL_KITKAT_VERSION)
|
|
if (period_ns > (((int64_t)sensor_t_data.maxDelay) * 1000))
|
|
period_ns = sensor_t_data.maxDelay * 1000;
|
|
#endif /* CONFIG_ST_HAL_ANDROID_VERSION */
|
|
|
|
if ((period_ns < (((int64_t)sensor_t_data.minDelay) * 1000)) &&
|
|
(period_ns > 0))
|
|
period_ns = sensor_t_data.minDelay * 1000;
|
|
}
|
|
|
|
err = SensorBase::SetDelay(handle, period_ns, timeout, false);
|
|
if (err < 0)
|
|
goto mutex_unlock;
|
|
|
|
min_pollrate_ns = GetMinPeriod(false);
|
|
if (min_pollrate_ns == 0) {
|
|
err = 0;
|
|
current_min_pollrate = 0;
|
|
current_min_timeout = INT64_MAX;
|
|
goto mutex_unlock;
|
|
}
|
|
|
|
sampling_frequency = NS_TO_FREQUENCY(min_pollrate_ns);
|
|
for (i = 0; i < (int)sampling_frequency_available.length; i++) {
|
|
if (sampling_frequency_available.freq[i] >= sampling_frequency)
|
|
break;
|
|
}
|
|
if (i == (int)sampling_frequency_available.length)
|
|
i--;
|
|
|
|
if (current_min_pollrate != min_pollrate_ns) {
|
|
err = device_iio_utils::set_sampling_frequency(common_data.device_iio_sysfs_path,
|
|
sampling_frequency_available.freq[i]);
|
|
if (err < 0) {
|
|
ALOGE("%s: Failed to write sampling frequency to iio device.", GetName());
|
|
goto mutex_unlock;
|
|
}
|
|
|
|
timestamp = android::elapsedRealtimeNano();
|
|
|
|
err = odr_switch.writeElement(timestamp,
|
|
FREQUENCY_TO_NS(sampling_frequency_available.freq[i]));
|
|
if (err < 0)
|
|
ALOGE("%s: Failed to write new odr on stack.",
|
|
GetName());
|
|
|
|
if (handle == sensor_t_data.handle)
|
|
AddNewPollrate(timestamp, period_ns);
|
|
|
|
current_min_pollrate = min_pollrate_ns;
|
|
#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_INFO)
|
|
message = true;
|
|
#endif /* CONFIG_ST_HAL_DEBUG_INFO */
|
|
|
|
}
|
|
|
|
if (sensor_t_data.fifoMaxEventCount > 0) {
|
|
min_timeout_ns = GetMinTimeout(false);
|
|
if (min_timeout_ns < HW_SENSOR_BASE_DEELAY_TRANSFER_DATA)
|
|
min_timeout_ns = 0;
|
|
else
|
|
min_timeout_ns -= HW_SENSOR_BASE_DEELAY_TRANSFER_DATA;
|
|
|
|
if (current_min_timeout != min_timeout_ns) {
|
|
buf_len = min_timeout_ns / FREQUENCY_TO_NS(sampling_frequency_available.freq[i]);
|
|
if (buf_len > sensor_t_data.fifoMaxEventCount)
|
|
buf_len = sensor_t_data.fifoMaxEventCount;
|
|
|
|
err = WriteBufferLenght(buf_len);
|
|
if (err < 0)
|
|
goto mutex_unlock;
|
|
|
|
current_min_timeout = min_timeout_ns;
|
|
#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_INFO)
|
|
message = true;
|
|
#endif /* CONFIG_ST_HAL_DEBUG_INFO */
|
|
}
|
|
}
|
|
|
|
#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_INFO)
|
|
if (message)
|
|
ALOGD("\"%s\": changed pollrate to %.2fHz, timeout=%" PRIu64 "ms (sensor type: %d).",
|
|
sensor_t_data.name, NS_TO_FREQUENCY((float)(uint64_t)min_pollrate_ns),
|
|
(uint64_t)NS_TO_MS((uint64_t)min_timeout_ns), sensor_t_data.type);
|
|
#endif /* CONFIG_ST_HAL_DEBUG_INFO */
|
|
|
|
if (lock_en_mutex)
|
|
pthread_mutex_unlock(&enable_mutex);
|
|
|
|
return 0;
|
|
|
|
mutex_unlock:
|
|
if (lock_en_mutex)
|
|
pthread_mutex_unlock(&enable_mutex);
|
|
|
|
return err;
|
|
}
|
|
|
|
int HWSensorBaseWithPollrate::FlushData(int handle, bool lock_en_mutex)
|
|
{
|
|
int err;
|
|
unsigned int i;
|
|
|
|
if (lock_en_mutex)
|
|
pthread_mutex_lock(&enable_mutex);
|
|
|
|
if (GetStatus(false)) {
|
|
err = flush_requested.writeElement(handle);
|
|
if (err < 0)
|
|
goto unlock_mutex;
|
|
|
|
if ((GetMinTimeout(false) > 0) && (GetMinTimeout(false) < INT64_MAX)) {
|
|
for (i = 0; i < dependencies.num; i++)
|
|
dependencies.sb[i]->FlushData(sensor_t_data.handle, true);
|
|
|
|
err = device_iio_utils::hw_fifo_flush(common_data.device_iio_sysfs_path);
|
|
if (err < 0) {
|
|
ALOGE("%s: Failed to flush hw fifo.", GetName());
|
|
goto unlock_mutex;
|
|
}
|
|
} else
|
|
ProcessFlushData(sensor_t_data.handle, android::elapsedRealtimeNano());
|
|
} else
|
|
goto unlock_mutex;
|
|
|
|
if (lock_en_mutex)
|
|
pthread_mutex_unlock(&enable_mutex);
|
|
|
|
return 0;
|
|
|
|
unlock_mutex:
|
|
if (lock_en_mutex)
|
|
pthread_mutex_unlock(&enable_mutex);
|
|
|
|
return -EINVAL;
|
|
}
|
|
|
|
void HWSensorBaseWithPollrate::WriteDataToPipe(int64_t hw_pollrate)
|
|
{
|
|
int err;
|
|
float temp;
|
|
bool odr_changed = false;
|
|
int64_t timestamp_change = 0, new_pollrate = 0;
|
|
|
|
err = CheckLatestNewPollrate(×tamp_change, &new_pollrate);
|
|
if ((err >= 0) && (sensor_event.timestamp > timestamp_change)) {
|
|
current_real_pollrate = new_pollrate;
|
|
DeleteLatestNewPollrate();
|
|
odr_changed = true;
|
|
}
|
|
|
|
if (ValidDataToPush(sensor_event.timestamp)) {
|
|
temp = (float)current_real_pollrate / hw_pollrate;
|
|
decimator = (int)(temp + (temp / 20));
|
|
samples_counter++;
|
|
|
|
if (decimator == 0)
|
|
decimator = 1;
|
|
|
|
#ifdef CONFIG_ST_HAL_DIRECT_REPORT_SENSOR
|
|
if (mDirectChannel != nullptr) {
|
|
if (mDirectChannelLock.tryLock() == android::NO_ERROR) {
|
|
mDirectChannel->write(&sensor_event);
|
|
mDirectChannelLock.unlock();
|
|
}
|
|
}
|
|
#endif /* CONFIG_ST_HAL_DIRECT_REPORT_SENSOR */
|
|
|
|
if (((samples_counter % decimator) == 0) || odr_changed) {
|
|
err = write(write_pipe_fd, &sensor_event, sizeof(sensors_event_t));
|
|
if (err <= 0) {
|
|
ALOGE("%s: Failed to write sensor data to pipe. (errno: %d)",
|
|
android_name, -errno);
|
|
samples_counter--;
|
|
return;
|
|
}
|
|
|
|
samples_counter = 0;
|
|
last_data_timestamp = sensor_event.timestamp;
|
|
|
|
#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_EXTRA_VERBOSE)
|
|
ALOGD("\"%s\": pushed data to android: timestamp=%" PRIu64 "ns real_pollrate=%" PRIu64 " (sensor type: %d).",
|
|
sensor_t_data.name, sensor_event.timestamp,
|
|
current_real_pollrate, sensor_t_data.type);
|
|
#endif /* CONFIG_ST_HAL_DEBUG_LEVEL */
|
|
}
|
|
}
|
|
}
|