diff options
Diffstat (limited to 'libsensors_iio/src/HWSensorBase.cpp')
| -rw-r--r-- | libsensors_iio/src/HWSensorBase.cpp | 453 |
1 files changed, 453 insertions, 0 deletions
diff --git a/libsensors_iio/src/HWSensorBase.cpp b/libsensors_iio/src/HWSensorBase.cpp new file mode 100644 index 0000000..ea7078b --- /dev/null +++ b/libsensors_iio/src/HWSensorBase.cpp @@ -0,0 +1,453 @@ +/* + * 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 "HWSensorBase.h" + +#define DEFAULT_HRTIMER_PERIOD_NS (200000000) + + +/** + * 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 hw_buf_fifo_len; + int err, current_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; + + current_len = read_sysfs_posint((char *)FILENAME_BUFFER_LENGTH, + common_data.iio_sysfs_path); + if (current_len < 0) + return current_len; + + if (current_len == (int)hw_buf_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; + + 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() +{ + int err; + + if (GetStatus() && (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; + } + } else + return -EINVAL; + + return SensorBase::FlushData(); +} + +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) +{ + int err, i; + int64_t min_pollrate_ns; + unsigned int sampling_frequency, buf_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; + } + + real_pollrate = FREQUENCY_TO_NS(sampling_frequency_available.hz[i]); + + if (sensor_t_data.fifoMaxEventCount > 0) { + buf_len = GetMinTimeout() / FREQUENCY_TO_NS(sampling_frequency_available.hz[i]); + if (buf_len > sensor_t_data.fifoMaxEventCount) + buf_len = sensor_t_data.fifoMaxEventCount; + + err = WriteBufferLenght(buf_len); + if (err < 0) + return err; + } + + return 0; +} + +void HWSensorBaseWithPollrate::WriteDataToPipe() +{ + int err; + + if (!GetStatusOfHandle(sensor_t_data.handle)) + return; + + if (sensor_event.timestamp > (last_data_timestamp + GetDelay())) { + err = write(android_pipe_fd, &sensor_event, sizeof(sensor_event)); + if (err < 0) { + ALOGE("%s: Failed to write sensor data to pipe.", android_name); + return; + } + + last_data_timestamp = sensor_event.timestamp; + } +} |