summaryrefslogtreecommitdiff
path: root/libsensors_iio/src/HWSensorBase.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libsensors_iio/src/HWSensorBase.cpp')
-rw-r--r--libsensors_iio/src/HWSensorBase.cpp569
1 files changed, 0 insertions, 569 deletions
diff --git a/libsensors_iio/src/HWSensorBase.cpp b/libsensors_iio/src/HWSensorBase.cpp
deleted file mode 100644
index a910bd0..0000000
--- a/libsensors_iio/src/HWSensorBase.cpp
+++ /dev/null
@@ -1,569 +0,0 @@
-/*
- * STMicroelectronics HW Sensor Base With Pollrate Class
- *
- * Copyright 2013-2015 STMicroelectronics Inc.
- * Author: Denis Ciocca - <denis.ciocca@st.com>
- *
- * Licensed under the Apache License, Version 2.0 (the "License").
- */
-
-#define __STDC_LIMIT_MACROS
-
-#include <fcntl.h>
-#include <assert.h>
-#include <string.h>
-#include <signal.h>
-#include <stdint.h>
-#include <endian.h>
-
-#include "HWSensorBase.h"
-
-#define DEFAULT_HRTIMER_PERIOD_NS (200000000)
-#define LOCAL_REPORTING_MODE_MASK 6
-
-
-/**
- * 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 iio_channel_info *channels, int num_channels)
-{
- int bytes = 0, i;
-
- for (i = 0; i < num_channels; i++) {
- 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.
- * @multi_data: 2byte is part of multiple data.
- **/
-static float process_2byte_received(int input,
- struct iio_channel_info *info, bool multi_data)
-{
- int16_t val;
- float offset = 0;
-
- if (info->be)
- input = be16toh((uint16_t)input);
- else
- input = le16toh((uint16_t)input);
-
- if (!multi_data) {
- offset = info->offset;
- val = input >> info->shift;
- if (info->is_signed) {
- val &= (1 << info->bits_used) - 1;
- val = (int16_t)(val << (16 - info->bits_used)) >>
- (16 - info->bits_used);
- } else
- val &= (1 << info->bits_used) - 1;
- } else
- val = input;
-
- return (((float)val + offset) * info->scale);
-}
-
-static float process_3byte_received(int input, struct iio_channel_info *info)
-{
- int32_t val;
-
- if (info->be)
- input = be32toh((uint32_t)input);
- else
- input = le32toh((uint32_t)input);
-
- val = input >> info->shift;
- if (info->is_signed) {
- val &= (1 << info->bits_used) - 1;
- val = (int32_t)(val << (24 - info->bits_used)) >>
- (24 - info->bits_used);
- } else
- val &= (1 << info->bits_used) - 1;
-
- return (((float)val + 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 iio_channel_info *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], false);
- break;
- case 3:
- sensor_out_data->raw[k] = process_3byte_received(*(uint32_t *)
- (data + channels[k].location), &channels[k]);
- break;
- case 4:
- if (channels->multi_data) {
- sensor_out_data->raw[k] = process_2byte_received(*(uint16_t *)
- (data + channels[k].location), &channels[k], true);
- sensor_out_data->offset[k] = process_2byte_received(*(uint16_t *)
- (data + channels[k].location + sizeof(uint16_t)),
- &channels[k], true);
- } else {
- uint32_t val;
-
- if (channels[k].be)
- val = be32toh(*(uint32_t *)
- (data + channels[k].location));
- else
- val = le32toh(*(uint32_t *)
- (data + channels[k].location));
-
- if (channels->isfloat)
- sensor_out_data->raw[k] = (*((float *)((void *)&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].is_signed) {
- 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);
- }
- }
- break;
- default:
- return -EINVAL;
- }
- }
-
- return num_channels;
-}
-
-HWSensorBase::HWSensorBase(HWSensorBaseCommonData *data, const char *name,
- int handle, int sensor_type, unsigned int hw_fifo_len, int pipe_data_fd,
- float power_consumption) : SensorBase(name, handle, sensor_type, pipe_data_fd)
-{
- 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;
- current_fifo_len = HW_SENSOR_BASE_DEFAULT_IIO_BUFFER_LEN;
-
- scan_size = size_from_channelarray(common_data.channels, common_data.num_channels);
-
- sensor_data = (uint8_t *)malloc(scan_size * HW_SENSOR_BASE_DEFAULT_IIO_BUFFER_LEN * hw_fifo_len * sizeof(uint8_t));
- if (!sensor_data)
- goto failed_creation;
-
- err = asprintf(&buffer_path, "/dev/iio:device%d", data->iio_dev_num);
- if (err <= 0)
- goto free_sensor_data;
-
- pollfd_iio[0].fd = open(buffer_path, O_RDONLY | O_NONBLOCK);
- if (pollfd_iio[0].fd < 0)
- goto free_buffer_path;
-
- err = ioctl(pollfd_iio[0].fd, IIO_GET_EVENT_FD_IOCTL, &pollfd_iio[1].fd);
- if (err < 0)
- goto close_iio_buffer;
-
- pollfd_iio[0].events = POLLIN;
- pollfd_iio[1].events = POLLIN;
-
- free(buffer_path);
-
- return;
-
-close_iio_buffer:
- close(pollfd_iio[0].fd);
-free_buffer_path:
- free(buffer_path);
-free_sensor_data:
- free(sensor_data);
-failed_creation:
- valid_class = false;
-}
-
-HWSensorBase::~HWSensorBase()
-{
- if (!valid_class)
- return;
-
- free(sensor_data);
- close(pollfd_iio[0].fd);
- close(pollfd_iio[1].fd);
-}
-
-int HWSensorBase::WriteBufferLenght(unsigned int buf_len, unsigned int store_len)
-{
- unsigned int hw_buf_fifo_len, hw_store_fifo_len;
- int err, current_len, cur_store_len, buff_enable;
-
- hw_buf_fifo_len = buf_len * HW_SENSOR_BASE_DEFAULT_IIO_BUFFER_LEN;
- if (hw_buf_fifo_len == 0)
- hw_buf_fifo_len = HW_SENSOR_BASE_DEFAULT_IIO_BUFFER_LEN;
-
- hw_store_fifo_len = store_len * HW_SENSOR_BASE_DEFAULT_IIO_BUFFER_LEN;
- if (hw_store_fifo_len == 0)
- hw_store_fifo_len = HW_SENSOR_BASE_DEFAULT_IIO_BUFFER_LEN;
-
- current_len = read_sysfs_posint((char *)FILENAME_BUFFER_LENGTH,
- common_data.iio_sysfs_path);
- if (current_len < 0)
- return current_len;
-
- cur_store_len = read_sysfs_posint((char *)FILENAME_BUFFER_STORE_LENGTH,
- common_data.iio_sysfs_path);
- if (cur_store_len < 0)
- return cur_store_len;
-
- if ((current_len == (int)hw_buf_fifo_len) && (cur_store_len == (int)hw_store_fifo_len))
- return 0;
-
- buff_enable = read_sysfs_posint((char *)FILENAME_BUFFER_ENABLE,
- common_data.iio_sysfs_path);
- if (buff_enable < 0)
- return buff_enable;
-
- if (buff_enable == 1) {
- err = write_sysfs_int_and_verify((char *)FILENAME_BUFFER_ENABLE,
- common_data.iio_sysfs_path, 0);
- if (err < 0)
- return err;
- }
-
- err = write_sysfs_int_and_verify((char *)FILENAME_BUFFER_LENGTH,
- common_data.iio_sysfs_path, hw_buf_fifo_len);
- if (err < 0)
- return err;
-
- err = write_sysfs_int_and_verify((char *)FILENAME_BUFFER_STORE_LENGTH,
- common_data.iio_sysfs_path, hw_store_fifo_len);
- if (err < 0)
- return err;
-
- current_fifo_len = hw_buf_fifo_len;
-
- if (buff_enable > 0) {
- err = write_sysfs_int_and_verify((char *)FILENAME_BUFFER_ENABLE,
- common_data.iio_sysfs_path, 1);
- if (err < 0)
- return err;
- }
-
- return 0;
-}
-
-int HWSensorBase::Enable(int handle, bool enable)
-{
- int err;
-
- err = SensorBase::Enable(handle, enable);
- if (err < 0)
- return err;
-
- err = write_sysfs_int_and_verify((char *)FILENAME_BUFFER_ENABLE,
- common_data.iio_sysfs_path, GetStatus());
- if (err < 0) {
- ALOGE("%s: Failed to write buffer file \"%s/%s\".",
- common_data.device_name, common_data.iio_sysfs_path, FILENAME_BUFFER_ENABLE);
- goto restore_status_enable;
- }
-
- return 0;
-
-restore_status_enable:
- SensorBase::Enable(handle, !enable);
- return err;
-}
-
-int HWSensorBase::FlushData(bool need_report_event)
-{
- 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) {
- ALOGE("%s: Failed to write flush file \"%s/%s\".",
- common_data.device_name, common_data.iio_sysfs_path, FILENAME_FLUSH);
- 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;
-
-}
-
-void HWSensorBase::ThreadTask()
-{
- uint8_t *data;
- int err, i, read_size;
- unsigned int hw_fifo_len;
- SensorBaseData sensor_data;
- struct iio_event_data event_data;
-
- 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)
- return;
-
- while (true) {
- err = poll(pollfd_iio, 2, -1);
- if (err <= 0)
- continue;
-
- if (pollfd_iio[0].revents > 0) {
- read_size = read(pollfd_iio[0].fd, data, current_fifo_len * scan_size);
- if (read_size <= 0)
- 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;
-
- ProcessData(&sensor_data);
- }
- }
-
- if (pollfd_iio[1].revents > 0) {
- read_size = read(pollfd_iio[1].fd, &event_data, sizeof(event_data));
- if (read_size <= 0)
- continue;
-
- ProcessEvent(&event_data);
- }
- }
-}
-
-
-HWSensorBaseWithPollrate::HWSensorBaseWithPollrate(HWSensorBaseCommonData *data, const char *name,
- struct iio_sampling_frequency_available *sfa, int handle,
- int sensor_type, unsigned int hw_fifo_len, int pipe_data_fd, float power_consumption) :
- HWSensorBase(data, name, handle, sensor_type, hw_fifo_len, pipe_data_fd, power_consumption)
-{
- int i;
- unsigned int max_sampling_frequency = 0, min_sampling_frequency = UINT_MAX;
-
- memcpy(&sampling_frequency_available, sfa, sizeof(sampling_frequency_available));
-
- for (i = 0; i < (int)sfa->num_available; i++) {
- if ((max_sampling_frequency < sfa->hz[i]) &&
- (sfa->hz[i] <= CONFIG_ST_HAL_MAX_SAMPLING_FREQUENCY))
- max_sampling_frequency = sfa->hz[i];
-
- if (min_sampling_frequency > sfa->hz[i])
- min_sampling_frequency = sfa->hz[i];
- }
-
- sensor_t_data.minDelay = FREQUENCY_TO_US(max_sampling_frequency);
- sensor_t_data.maxDelay = FREQUENCY_TO_US(min_sampling_frequency);
-}
-
-HWSensorBaseWithPollrate::~HWSensorBaseWithPollrate()
-{
-
-}
-
-int HWSensorBaseWithPollrate::SetDelay(int handle, int64_t period_ns, int64_t timeout)
-{
-#define MIN_BUF_TIMEOUT 2500000000ULL
- int err, i;
- int64_t min_pollrate_ns, tmp_real_pollrate;
- unsigned int sampling_frequency, buf_len, store_len;
-
- err = HWSensorBase::SetDelay(handle, period_ns, timeout);
- if (err < 0)
- return err;
-
- min_pollrate_ns = GetMinPeriod();
-
- sampling_frequency = NS_TO_FREQUENCY(min_pollrate_ns);
- for (i = 0; i < (int)sampling_frequency_available.num_available; i++) {
- if (sampling_frequency_available.hz[i] >= sampling_frequency)
- break;
- }
- if (i == (int)sampling_frequency_available.num_available)
- i--;
-
- err = write_sysfs_int_and_verify((char *)FILENAME_SAMPLING_FREQ,
- common_data.iio_sysfs_path, sampling_frequency_available.hz[i]);
- if (err < 0) {
- ALOGE("%s: Failed to write sampling frequency file \"%s/%s\".",
- common_data.device_name, common_data.iio_sysfs_path, FILENAME_SAMPLING_FREQ);
- return err;
- }
-
- tmp_real_pollrate = real_pollrate = FREQUENCY_TO_NS(sampling_frequency_available.hz[i]);
-
- if (sensor_t_data.fifoMaxEventCount > 0) {
- store_len = GetMinTimeout() / tmp_real_pollrate;
- if (store_len > sensor_t_data.fifoMaxEventCount)
- store_len = sensor_t_data.fifoMaxEventCount;
-
- buf_len = MIN_BUF_TIMEOUT / tmp_real_pollrate;
- if (buf_len < store_len)
- buf_len = store_len;
-
- err = WriteBufferLenght(buf_len, store_len);
- if (err < 0)
- return err;
- }
-
- return 0;
-}
-
-void HWSensorBaseWithPollrate::WriteDataToPipe()
-{
- int err, retry = 3;
- std::vector<int64_t>::iterator it;
-
- if (!GetStatusOfHandle(sensor_t_data.handle))
- return;
-
- 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);
-}
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-25 20:18:06 +0000