libsensors_iio/src/HWSensorBase.cpp

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