diff options
Diffstat (limited to 'libsensors_iio/src/SensorHAL.cpp')
| -rw-r--r-- | libsensors_iio/src/SensorHAL.cpp | 986 |
1 files changed, 0 insertions, 986 deletions
diff --git a/libsensors_iio/src/SensorHAL.cpp b/libsensors_iio/src/SensorHAL.cpp deleted file mode 100644 index c0c8443..0000000 --- a/libsensors_iio/src/SensorHAL.cpp +++ /dev/null @@ -1,986 +0,0 @@ -/* - * STMicroelectronics SensorHAL core - * - * Version 3.1.0 - * Copyright 2013-2015 STMicroelectronics Inc. - * Author: Denis Ciocca - <denis.ciocca@st.com> - * - * Licensed under the Apache License, Version 2.0 (the "License"). - */ - -#include <stdlib.h> -#include <stdio.h> -#include <stdint.h> -#include <fcntl.h> -#include <errno.h> -#include <math.h> -#include <pthread.h> -#include <endian.h> - -#include "SensorHAL.h" -#include "Accelerometer.h" -#include "Magnetometer.h" -#include "Gyroscope.h" - -#ifdef CONFIG_ST_HAL_STEP_DETECTOR_ENABLED -#include "StepDetector.h" -#endif /* CONFIG_ST_HAL_STEP_DETECTOR_ENABLED */ - -#ifdef CONFIG_ST_HAL_STEP_COUNTER_ENABLED -#include "StepCounter.h" -#endif /* CONFIG_ST_HAL_STEP_COUNTER_ENABLED */ - -#ifdef CONFIG_ST_HAL_SIGN_MOTION_ENABLED -#include "SignificantMotion.h" -#endif /* CONFIG_ST_HAL_SIGN_MOTION_ENABLED */ - -#ifdef CONFIG_ST_HAL_TILT_ENABLED -#include "TiltSensor.h" -#endif /* CONFIG_ST_HAL_TILT_ENABLED */ - -#ifdef CONFIG_ST_HAL_MAGN_UNCALIB_AP_ENABLED -#include "SWMagnetometerUncalibrated.h" -#endif /* CONFIG_ST_HAL_MAGN_UNCALIB_AP_ENABLED */ - -#ifdef CONFIG_ST_HAL_GYRO_UNCALIB_AP_ENABLED -#include "SWGyroscopeUncalibrated.h" -#endif /* CONFIG_ST_HAL_GYRO_UNCALIB_AP_ENABLED */ - -#ifdef CONFIG_ST_HAL_PRESSURE_ENABLED -#include "Pressure.h" -#endif /* CONFIG_ST_HAL_PRESSURE_ENABLED */ - -#ifdef ST_HAL_NEEDS_GEOMAG_FUSION -#include "SWAccelMagnFusion6X.h" -#endif /* ST_HAL_NEEDS_GEOMAG_FUSION */ - -#ifdef CONFIG_ST_HAL_GEOMAG_ROT_VECTOR_AP_ENABLED -#include "SWGeoMagRotationVector.h" -#endif /* CONFIG_ST_HAL_GEOMAG_ROT_VECTOR_AP_ENABLED */ - -#ifdef ST_HAL_NEEDS_6AX_FUSION -#include "SWAccelGyroFusion6X.h" -#endif /* ST_HAL_NEEDS_6AX_FUSION */ - -#ifdef CONFIG_ST_HAL_GAME_ROT_VECTOR_AP_ENABLED -#include "SWGameRotationVector.h" -#endif /* CONFIG_ST_HAL_GAME_ROT_VECTOR_AP_ENABLED */ - -#ifdef ST_HAL_NEEDS_9AX_FUSION -#include "SWAccelMagnGyroFusion9X.h" -#endif /* ST_HAL_NEEDS_9AX_FUSION */ - -#ifdef CONFIG_ST_HAL_ROT_VECTOR_AP_ENABLED -#include "SWRotationVector.h" -#endif /* CONFIG_ST_HAL_ROT_VECTOR_AP_ENABLED */ - -#ifdef CONFIG_ST_HAL_ORIENTATION_AP_ENABLED -#include "SWOrientation.h" -#endif /* CONFIG_ST_HAL_ORIENTATION_AP_ENABLED */ - -#ifdef CONFIG_ST_HAL_GRAVITY_AP_ENABLED -#include "SWGravity.h" -#endif /* CONFIG_ST_HAL_GRAVITY_AP_ENABLED */ - -#ifdef CONFIG_ST_HAL_LINEAR_AP_ENABLED -#include "SWLinearAccel.h" -#endif /* CONFIG_ST_HAL_LINEAR_AP_ENABLED */ - -struct STSensorHAL_iio_devices_data { - char *iio_sysfs_path; - char *device_name; - char *android_name; - unsigned int dev_id; - int sensor_type; - - bool wake_up_sensor; - - int num_channels; - struct iio_channel_info *channels; - struct iio_scale_available sa; - - unsigned int hw_fifo_len; - float power_consumption; - - struct iio_sampling_frequency_available sfa; -} typedef STSensorHAL_iio_devices_data; - -/* - * ST_sensors_supported: ST sensors data used for discovery procedure - * @driver_name: IIO device name. - * @android_name: name showed in Android OS. - * @sensor_type: Android sensor type. - * @power_consumption: sensor power consumption in mA. - */ -static const struct ST_sensors_supported { - const char *driver_name; - const char *android_name; - int sensor_type; - float power_consumption; -} ST_sensors_supported[] = { -#ifdef CONFIG_ST_HAL_ACCEL_ENABLED -#ifdef CONFIG_ST_HAL_LSM6DS3_ENABLED - { - .driver_name = CONCATENATE_STRING(ST_SENSORS_LIST_1, ACCEL_NAME_SUFFIX_IIO), - .android_name = "LSM6DS3 Accelerometer Sensor", - .sensor_type = SENSOR_TYPE_ACCELEROMETER, - .power_consumption = 240E-3f, - }, -#endif /* CONFIG_ST_HAL_LSM6DS3_ENABLED */ -#endif /* CONFIG_ST_HAL_ACCEL_ENABLED */ -#ifdef CONFIG_ST_HAL_MAGN_ENABLED -#ifdef CONFIG_ST_HAL_LSM6DS3_ENABLED - { - .driver_name = CONCATENATE_STRING(ST_SENSORS_LIST_1, MAGN_NAME_SUFFIX_IIO), - .android_name = "LSM6DS3 Magnetometer Sensor", - .sensor_type = SENSOR_TYPE_GEOMAGNETIC_FIELD, - .power_consumption = 2.0f, - }, -#endif /* CONFIG_ST_HAL_LSM6DS3_ENABLED */ -#endif /* CONFIG_ST_HAL_MAGN_ENABLED */ -#ifdef CONFIG_ST_HAL_GYRO_ENABLED -#ifdef CONFIG_ST_HAL_LSM6DS3_ENABLED - { - .driver_name = CONCATENATE_STRING(ST_SENSORS_LIST_1, GYRO_NAME_SUFFIX_IIO), - .android_name = "LSM6DS3 Gyroscope Sensor", - .sensor_type = SENSOR_TYPE_GYROSCOPE, - .power_consumption = 1.25f, - }, -#endif /* CONFIG_ST_HAL_LSM6DS3_ENABLED */ -#endif /* CONFIG_ST_HAL_GYRO_ENABLED */ -#ifdef CONFIG_ST_HAL_STEP_DETECTOR_ENABLED -#ifdef CONFIG_ST_HAL_LSM6DS3_ENABLED - { - .driver_name = CONCATENATE_STRING(ST_SENSORS_LIST_1, STEP_DETECTOR_NAME_SUFFIX_IIO), - .android_name = "LSM6DS3 Step Detector Sensor", - .sensor_type = SENSOR_TYPE_STEP_DETECTOR, - .power_consumption = 240E-3f, - }, -#endif /* CONFIG_ST_HAL_LSM6DS3_ENABLED */ -#endif /* CONFIG_ST_HAL_STEP_DETECTOR_ENABLED */ -#ifdef CONFIG_ST_HAL_STEP_COUNTER_ENABLED -#ifdef CONFIG_ST_HAL_LSM6DS3_ENABLED - { - .driver_name = CONCATENATE_STRING(ST_SENSORS_LIST_1, STEP_COUNTER_NAME_SUFFIX_IIO), - .android_name = "LSM6DS3 Step Counter Sensor", - .sensor_type = SENSOR_TYPE_STEP_COUNTER, - .power_consumption = 240E-3f, - }, -#endif /* CONFIG_ST_HAL_LSM6DS3_ENABLED */ -#endif /* CONFIG_ST_HAL_STEP_COUNTER_ENABLED */ -#ifdef CONFIG_ST_HAL_SIGN_MOTION_ENABLED -#ifdef CONFIG_ST_HAL_LSM6DS3_ENABLED - { - .driver_name = CONCATENATE_STRING(ST_SENSORS_LIST_1, SIGN_MOTION_NAME_SUFFIX_IIO), - .android_name = "LSM6DS3 Significant Motion Sensor", - .sensor_type = SENSOR_TYPE_SIGNIFICANT_MOTION, - .power_consumption = 240E-3f, - }, -#endif /* CONFIG_ST_HAL_LSM6DS3_ENABLED */ -#endif /* CONFIG_ST_HAL_SIGN_MOTION_ENABLED */ -#ifdef CONFIG_ST_HAL_TILT_ENABLED -#ifdef CONFIG_ST_HAL_LSM6DS3_ENABLED - { - .driver_name = CONCATENATE_STRING(ST_SENSORS_LIST_1, TILT_NAME_SUFFIX_IIO), - .android_name = "LSM6DS3 Tilt Sensor", - .sensor_type = SENSOR_TYPE_TILT_DETECTOR, - .power_consumption = 240E-3f, - }, -#endif /* CONFIG_ST_HAL_LSM6DS3_ENABLED */ -#endif /* CONFIG_ST_HAL_TILT_ENABLED */ -#ifdef CONFIG_ST_HAL_PRESSURE_ENABLED -#ifdef CONFIG_ST_HAL_LSM6DS3_ENABLED - { - .driver_name = CONCATENATE_STRING(ST_SENSORS_LIST_1, PRESSURE_NAME_SUFFIX_IIO), - .android_name = "LSM6DS3 Pressure Sensor", - .sensor_type = SENSOR_TYPE_PRESSURE, - .power_consumption = 40E-3f, - }, -#endif /* CONFIG_ST_HAL_LSM6DS3_ENABLED */ -#endif /* CONFIG_ST_HAL_PRESSURE_ENABLED */ -}; - -static const struct ST_virtual_sensors_list { - int sensor_type; -} ST_virtual_sensors_list[] = { -#ifdef ST_HAL_NEEDS_GEOMAG_FUSION - { .sensor_type = SENSOR_TYPE_ST_ACCEL_MAGN_FUSION6X }, -#endif /* ST_HAL_NEEDS_GEOMAG_FUSION */ -#ifdef CONFIG_ST_HAL_MAGN_UNCALIB_AP_ENABLED - { .sensor_type = SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED }, -#endif /* CONFIG_ST_HAL_MAGN_UNCALIB_AP_ENABLED */ -#ifdef CONFIG_ST_HAL_GYRO_UNCALIB_AP_ENABLED - { .sensor_type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED }, -#endif /* CONFIG_ST_HAL_GYRO_UNCALIB_AP_ENABLED */ -#ifdef CONFIG_ST_HAL_GEOMAG_ROT_VECTOR_AP_ENABLED - { .sensor_type = SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR }, -#endif /* CONFIG_ST_HAL_GEOMAG_ROT_VECTOR_AP_ENABLED */ -#ifdef ST_HAL_NEEDS_6AX_FUSION - { .sensor_type = SENSOR_TYPE_ST_ACCEL_GYRO_FUSION6X }, -#endif /* ST_HAL_NEEDS_6AX_FUSION */ -#ifdef CONFIG_ST_HAL_GAME_ROT_VECTOR_AP_ENABLED - { .sensor_type = SENSOR_TYPE_GAME_ROTATION_VECTOR }, -#endif /* CONFIG_ST_HAL_GAME_ROT_VECTOR_AP_ENABLED */ -#ifdef ST_HAL_NEEDS_9AX_FUSION - { .sensor_type = SENSOR_TYPE_ST_ACCEL_MAGN_GYRO_FUSION9X }, -#endif /* ST_HAL_NEEDS_9AX_FUSION */ -#ifdef CONFIG_ST_HAL_ROT_VECTOR_AP_ENABLED - { .sensor_type = SENSOR_TYPE_ROTATION_VECTOR }, -#endif /* CONFIG_ST_HAL_ROT_VECTOR_AP_ENABLED */ -#ifdef CONFIG_ST_HAL_ORIENTATION_AP_ENABLED - { .sensor_type = SENSOR_TYPE_ORIENTATION }, -#endif /* CONFIG_ST_HAL_ORIENTATION_AP_ENABLED */ -#ifdef CONFIG_ST_HAL_GRAVITY_AP_ENABLED - { .sensor_type = SENSOR_TYPE_GRAVITY }, -#endif /* CONFIG_ST_HAL_GRAVITY_AP_ENABLED */ -#ifdef CONFIG_ST_HAL_LINEAR_AP_ENABLED - { .sensor_type = SENSOR_TYPE_LINEAR_ACCELERATION }, -#endif /* CONFIG_ST_HAL_LINEAR_AP_ENABLED */ -}; - -/* - * st_hal_create_virtual_class_sensor: instantiate virtual sensor class. - * @sensor_type: android sensor type. - * @handle: android handle number. - * @android_pipe_fd: file descriptor used to push new data. - * - * Return value: sensor class pointer on success, NULL pointer if fail. - */ -static SensorBase* st_hal_create_virtual_class_sensor(int sensor_type, int handle, int android_pipe_fd) -{ - SensorBase *sb = NULL; - - switch (sensor_type) { -#ifdef CONFIG_ST_HAL_MAGN_UNCALIB_AP_ENABLED - case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED: - sb = new SWMagnetometerUncalibrated("Magnetometer Uncalibrated Sensor", handle, android_pipe_fd); - break; -#endif /* CONFIG_ST_HAL_MAGN_UNCALIB_AP_ENABLED */ -#ifdef CONFIG_ST_HAL_GYRO_UNCALIB_AP_ENABLED - case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: - sb = new SWGyroscopeUncalibrated("Gyroscope Uncalibrated Sensor", handle, android_pipe_fd); - break; -#endif /* CONFIG_ST_HAL_GYRO_UNCALIB_AP_ENABLED */ -#ifdef ST_HAL_NEEDS_GEOMAG_FUSION - case SENSOR_TYPE_ST_ACCEL_MAGN_FUSION6X: - sb = new SWAccelMagnFusion6X("Accel-Magn Fusion 6X", handle, android_pipe_fd); - break; -#endif /* ST_HAL_NEEDS_GEOMAG_FUSION */ -#ifdef CONFIG_ST_HAL_GEOMAG_ROT_VECTOR_AP_ENABLED - case SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR: - sb = new SWGeoMagRotationVector("iNemoEngine GeoMagnetic Rotation Vector Sensor", handle, android_pipe_fd); - break; -#endif /* CONFIG_ST_HAL_GEOMAG_ROT_VECTOR_AP_ENABLED */ -#ifdef ST_HAL_NEEDS_6AX_FUSION - case SENSOR_TYPE_ST_ACCEL_GYRO_FUSION6X: - sb = new SWAccelGyroFusion6X("Accel-Gyro Fusion 6X", handle, android_pipe_fd); - break; -#endif /* ST_HAL_NEEDS_6AX_FUSION */ -#ifdef CONFIG_ST_HAL_GAME_ROT_VECTOR_AP_ENABLED - case SENSOR_TYPE_GAME_ROTATION_VECTOR: - sb = new SWGameRotationVector("iNemoEngine Game Rotation Vector Sensor", handle, android_pipe_fd); - break; -#endif /* SENSOR_TYPE_GAME_ROTATION_VECTOR */ -#ifdef ST_HAL_NEEDS_9AX_FUSION - case SENSOR_TYPE_ST_ACCEL_MAGN_GYRO_FUSION9X: - sb = new SWAccelMagnGyroFusion9X("Accel-Magn-Gyro Fusion 9X", handle, android_pipe_fd); - break; -#endif /* ST_HAL_NEEDS_9AX_FUSION */ -#ifdef CONFIG_ST_HAL_ROT_VECTOR_AP_ENABLED - case SENSOR_TYPE_ROTATION_VECTOR: - sb = new SWRotationVector("iNemoEngine Rotation Vector Sensor", handle, android_pipe_fd); - break; -#endif /* CONFIG_ST_HAL_ROT_VECTOR_AP_ENABLED */ -#ifdef CONFIG_ST_HAL_ORIENTATION_AP_ENABLED - case SENSOR_TYPE_ORIENTATION: - sb = new SWOrientation("iNemoEngine Orientation Sensor", handle, android_pipe_fd); - break; -#endif /* CONFIG_ST_HAL_ORIENTATION_AP_ENABLED */ -#ifdef CONFIG_ST_HAL_GRAVITY_AP_ENABLED - case SENSOR_TYPE_GRAVITY: - sb = new SWGravity("iNemoEngine Gravity Sensor", handle, android_pipe_fd); - break; -#endif /* CONFIG_ST_HAL_GRAVITY_AP_ENABLED */ -#ifdef CONFIG_ST_HAL_LINEAR_AP_ENABLED - case SENSOR_TYPE_LINEAR_ACCELERATION: - sb = new SWLinearAccel("iNemoEngine Linear Acceleration Sensor", handle, android_pipe_fd); - break; -#endif /* CONFIG_ST_HAL_LINEAR_AP_ENABLED */ - default: - (int)handle; - (int)android_pipe_fd; - return NULL; - } - - return sb->IsValidClass() ? sb : NULL; -} - -/* - * st_hal_create_class_sensor: instantiate sensor class. - * @data: device data. - * @handle: android handle number. - * @android_pipe_fd: file descriptor used to push new data. - * - * Return value: sensor class pointer on success, NULL pointer if fail. - */ -static SensorBase* st_hal_create_class_sensor(STSensorHAL_iio_devices_data *data, int handle, int android_pipe_fd) -{ - SensorBase *sb = NULL; - struct HWSensorBaseCommonData class_data; - - if ((strlen(data->iio_sysfs_path) + 1 > HW_SENSOR_BASE_IIO_SYSFS_PATH_MAX) || - (strlen(data->device_name) + 1 > HW_SENSOR_BASE_IIO_DEVICE_NAME_MAX) || - (data->num_channels > HW_SENSOR_BASE_MAX_CHANNELS)) - return NULL; - - memcpy(class_data.device_name, data->device_name, strlen(data->device_name) + 1); - memcpy(class_data.iio_sysfs_path, data->iio_sysfs_path, strlen(data->iio_sysfs_path) + 1); - memcpy(&class_data.sa, &data->sa, sizeof(class_data.sa)); - memcpy(class_data.channels, data->channels, data->num_channels * sizeof(class_data.channels[0])); - - class_data.iio_dev_num = data->dev_id; - class_data.num_channels = data->num_channels; - - switch (data->sensor_type) { -#ifdef CONFIG_ST_HAL_ACCEL_ENABLED - case SENSOR_TYPE_ACCELEROMETER: - sb = new Accelerometer(&class_data, data->android_name, &data->sfa, - handle, data->hw_fifo_len, android_pipe_fd, - data->power_consumption, data->wake_up_sensor); - break; -#endif /* CONFIG_ST_HAL_ACCEL_ENABLED */ -#ifdef CONFIG_ST_HAL_MAGN_ENABLED - case SENSOR_TYPE_MAGNETIC_FIELD: - sb = new Magnetometer(&class_data, data->android_name, &data->sfa, - handle, data->hw_fifo_len, android_pipe_fd, - data->power_consumption, data->wake_up_sensor); - break; -#endif /* CONFIG_ST_HAL_MAGN_ENABLED */ -#ifdef CONFIG_ST_HAL_GYRO_ENABLED - case SENSOR_TYPE_GYROSCOPE: - sb = new Gyroscope(&class_data, data->android_name, &data->sfa, - handle, data->hw_fifo_len, android_pipe_fd, - data->power_consumption, data->wake_up_sensor); - break; -#endif /* CONFIG_ST_HAL_GYRO_ENABLED */ -#ifdef CONFIG_ST_HAL_STEP_DETECTOR_ENABLED - case SENSOR_TYPE_STEP_DETECTOR: - sb = new StepDetector(&class_data, data->android_name, - handle, data->hw_fifo_len, android_pipe_fd, - data->power_consumption, data->wake_up_sensor); - break; -#endif /* CONFIG_ST_HAL_STEP_DETECTOR_ENABLED */ -#ifdef CONFIG_ST_HAL_STEP_COUNTER_ENABLED - case SENSOR_TYPE_STEP_COUNTER: - sb = new StepCounter(&class_data, data->android_name, - handle, data->hw_fifo_len, android_pipe_fd, - data->power_consumption, data->wake_up_sensor); - break; -#endif /* CONFIG_ST_HAL_STEP_COUNTER_ENABLED */ -#ifdef CONFIG_ST_HAL_SIGN_MOTION_ENABLED - case SENSOR_TYPE_SIGNIFICANT_MOTION: - sb = new SignMotion(&class_data, data->android_name, - handle, android_pipe_fd, data->power_consumption); - break; -#endif /* CONFIG_ST_HAL_SIGN_MOTION_ENABLED */ -#ifdef CONFIG_ST_HAL_TILT_ENABLED - case SENSOR_TYPE_TILT_DETECTOR: - sb = new TiltSensor(&class_data, data->android_name, - handle, android_pipe_fd, data->power_consumption); - break; -#endif /* CONFIG_ST_HAL_TILT_ENABLED */ -#ifdef CONFIG_ST_HAL_PRESSURE_ENABLED - case SENSOR_TYPE_PRESSURE: - sb = new Pressure(&class_data, data->android_name, &data->sfa, - handle, data->hw_fifo_len, android_pipe_fd, - data->power_consumption, data->wake_up_sensor); - break; -#endif /* CONFIG_ST_HAL_PRESSURE_ENABLED */ - default: - return NULL; - } - - return sb->IsValidClass() ? sb : NULL; -} - -/* - * st_hal_set_fullscale: change fullscale of iio device sensor. - * @iio_sysfs_path: iio device driver sysfs path. - * @sensor_type: android sensor type. - * @sa: scale available structure. - * @channels: iio channels informations. - * @num_channels: number of iio channels. - * - * Return value: 0 on success, negative number if fail. - */ -static int st_hal_set_fullscale(char *iio_sysfs_path, int sensor_type, - struct iio_scale_available *sa, struct iio_channel_info *channels, int num_channels) -{ - int err, i, c, max_value; - - switch (sensor_type) { -#ifdef CONFIG_ST_HAL_ACCEL_ENABLED - case SENSOR_TYPE_ACCELEROMETER: - max_value = CONFIG_ST_HAL_ACCEL_RANGE; - break; -#endif /* CONFIG_ST_HAL_ACCEL_ENABLED */ -#ifdef CONFIG_ST_HAL_MAGN_ENABLED - case SENSOR_TYPE_MAGNETIC_FIELD: - max_value = CONFIG_ST_HAL_MAGN_RANGE; - break; -#endif /* CONFIG_ST_HAL_MAGN_ENABLED */ -#ifdef CONFIG_ST_HAL_GYRO_ENABLED - case SENSOR_TYPE_GYROSCOPE: - max_value = CONFIG_ST_HAL_GYRO_RANGE; - break; -#endif /* CONFIG_ST_HAL_GYRO_ENABLED */ - default: - return -EINVAL; - } - - for (i = 0; i < (int)sa->num_available; i++) { - if ((sa->values[i] * ((int)pow(2.0, channels[0].bits_used - 1.0) - 1)) >= max_value) - break; - } - if (i == (int)sa->num_available) - i = sa->num_available - 1; - - err = iio_utils_set_scale(iio_sysfs_path, sa->values[i], sensor_type); - if (err < 0) - return err; - - for (c = 0; c < num_channels - 1; c++) - channels[c].scale = sa->values[i]; - - return 0; -} - -/* - * st_hal_load_iio_devices_data: read iio devices data. - * @data: iio device data. - * - * Return value: number of sensors found on success, negative number if fail. - */ -static int st_hal_load_iio_devices_data(STSensorHAL_iio_devices_data *data) -{ - unsigned int index = 0; - int err, iio_devices_num, i, n; - struct iio_device iio_devices[ST_HAL_IIO_MAX_DEVICES]; - - iio_devices_num = iio_utils_get_devices_name(iio_devices, ST_HAL_IIO_MAX_DEVICES); - if (iio_devices_num <= 0) { - ALOGE("Failed to read iio devices available into /sys/bus/iio/devices/ folder (errno: %d).", iio_devices_num); - return iio_devices_num; - } - -#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_VERBOSE) - ALOGD("%d IIO devices available into /sys/bus/iio/devices/ folder.", iio_devices_num); -#endif /* CONFIG_ST_HAL_DEBUG_LEVEL */ - - for (i = 0; i < iio_devices_num; i++) { - for (n = 0; n < ARRAY_SIZE(ST_sensors_supported); n++) { - err = strncmp(iio_devices[i].name, ST_sensors_supported[n].driver_name, - strlen(ST_sensors_supported[n].driver_name)); - if (err == 0) - break; - } - if (n == ARRAY_SIZE(ST_sensors_supported)) { -#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_EXTRA_VERBOSE) - ALOGD("\"%s\": IIO device not supported by sensor HAL.", iio_devices[i].name); -#endif /* CONFIG_ST_HAL_DEBUG_LEVEL */ - continue; - } - - if (strcmp(&iio_devices[i].name[strlen(iio_devices[i].name) - - strlen(ST_HAL_WAKEUP_SUFFIX_IIO)], ST_HAL_WAKEUP_SUFFIX_IIO) == 0) - data[index].wake_up_sensor = true; - else - data[index].wake_up_sensor = false; - -#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_VERBOSE) - ALOGD("\"%s\": IIO device found and supported. Wake-up sensor: %s", iio_devices[i].name, data[index].wake_up_sensor ? "yes" : "no" ); -#endif /* CONFIG_ST_HAL_DEBUG_LEVEL */ - - err = asprintf(&data[index].iio_sysfs_path, "%siio:device%d", - "/sys/bus/iio/devices/", iio_devices[i].dev_num); - if (err < 0) - continue; - - data[index].power_consumption = ST_sensors_supported[n].power_consumption; - - err = iio_utils_build_channel_array(data[index].iio_sysfs_path, &data[index].channels, - &data[index].num_channels, true); - if (err < 0) { - ALOGE("\"%s\": failed to read IIO channels informations. (errno: %d)", iio_devices[i].name, err); - goto st_hal_load_free_iio_sysfs_path; - } - - err = iio_utils_enable_sensor(data[index].iio_sysfs_path, false); - if (err < 0) { - ALOGE("\"%s\": failed to disable sensor. (errno: %d)", iio_devices[i].name, err); - goto st_hal_load_free_iio_channels; - } - - if ((ST_sensors_supported[n].sensor_type != SENSOR_TYPE_STEP_DETECTOR) && - (ST_sensors_supported[n].sensor_type != SENSOR_TYPE_STEP_COUNTER) && - (ST_sensors_supported[n].sensor_type != SENSOR_TYPE_SIGNIFICANT_MOTION) && - (ST_sensors_supported[n].sensor_type != SENSOR_TYPE_TILT_DETECTOR)) { - err = iio_utils_get_sampling_frequency_available(data[index].iio_sysfs_path, &data[index].sfa); - if (err < 0) - goto st_hal_load_free_iio_channels; - - err = iio_utils_get_scale_available(data[index].iio_sysfs_path, &data[index].sa, - ST_sensors_supported[n].sensor_type); - if (err < 0) - goto st_hal_load_free_iio_channels; - - if (data[index].sa.num_available > 0) { - err = st_hal_set_fullscale(data[index].iio_sysfs_path, ST_sensors_supported[n].sensor_type, - &data[index].sa, data[index].channels, data[index].num_channels); - if (err < 0) { - ALOGE("\"%s\": failed to set device full-scale. (errno: %d)", iio_devices[i].name, err); - goto st_hal_load_free_iio_channels; - } - } - } - - err = asprintf(&data[index].device_name, "%s", iio_devices[i].name); - if (err < 0) - goto st_hal_load_free_iio_channels; - - err = asprintf(&data[index].android_name, "%s", ST_sensors_supported[n].android_name); - if (err < 0) - goto st_hal_load_free_device_name; - - data[index].hw_fifo_len = iio_utils_get_hw_fifo_lenght(data[index].iio_sysfs_path); - data[index].sensor_type = ST_sensors_supported[n].sensor_type; - data[index].dev_id = iio_devices[i].dev_num; - - index++; - - continue; - -st_hal_load_free_device_name: - free(data[index].device_name); -st_hal_load_free_iio_channels: - free(data[index].channels); -st_hal_load_free_iio_sysfs_path: - free(data[index].iio_sysfs_path); - } - - if (index == 0) - ALOGE("No IIO sensors found into /sys/bus/iio/devices/ folder."); - - return index; -} - -/** - * st_hal_dev_flush() - Android call this function to flush sensor batch data. - * @dev: sensors device. - * @handle: android sensor handle. - * - * Return value: 0 on success, negative number if fail. - **/ -static int st_hal_dev_flush(struct sensors_poll_device_1 *dev, int handle) -{ - STSensorHAL_data *hal_data = (STSensorHAL_data *)dev; - ALOGD("st_hal_dev_flush handle=%d", handle); - - return hal_data->sensor_classes[handle]->FlushData(true); -} - -/** - * st_hal_dev_batch() - Android O.S. calls this function to check and set batch mode - * @dev: sensors device structure. - * @handle: android sensor handle. - * @flags: used for test the availability of batch mode. - * @period_ns: time to batch (like setDelay(...)). - * @timeout: 0 to disable batch mode. - * - * Return value: 0 on success, negative number if fail. - */ -static int st_hal_dev_batch(struct sensors_poll_device_1 *dev, int handle, - int __attribute__((unused))flags, int64_t period_ns, int64_t timeout) -{ - STSensorHAL_data *hal_data = (STSensorHAL_data *)dev; - - return hal_data->sensor_classes[handle]->SetDelay(handle, period_ns, timeout); -} - -/** - * st_hal_dev_poll() - Android O.S. calls this function and waits until when new data are available - * @dev: sensors device structure. - * @data: data structure used to push data to the upper layer. - * @count: maximum number of events in the same time. - * - * Return value: 0 on success, negative number if fail. - */ -static int st_hal_dev_poll(struct sensors_poll_device_t *dev, - sensors_event_t *data, int count) -{ - int i, err, read_size; - STSensorHAL_data *hal_data = (STSensorHAL_data *)dev; - - err = poll(&hal_data->android_pollfd, 1, -1); - if (err <= 0) - return 0; - - if (hal_data->android_pollfd.revents > 0) { - read_size = read(hal_data->android_pollfd.fd, data, count * sizeof(struct sensors_event_t)); - if (read_size <= 0) - return 0; - } else - return 0; - - return (read_size / sizeof(struct sensors_event_t)); -} - -/** - * st_hal_dev_setDelay() - Set sensor polling rate - * @dev: sensors device structure. - * @handle: android sensor handle. - * @ns: polling rate value expressed in nanoseconds. - * - * Return value: 0 on success, negative number if fail. - */ -static int st_hal_dev_setDelay(struct sensors_poll_device_t *dev, int handle, int64_t ns) -{ - STSensorHAL_data *hal_data = (STSensorHAL_data *)dev; - - return hal_data->sensor_classes[handle]->SetDelay(handle, ns, 0); -} - -/** - * st_hal_dev_activate() - Enable or Disable sensors - * @dev: sensors device structure. - * @handle: android sensor handle. - * @enable: enable/ disable flag. - * - * Return value: 0 on success, negative number if fail. - */ -static int st_hal_dev_activate(struct sensors_poll_device_t *dev, int handle, int enabled) -{ - STSensorHAL_data *hal_data = (STSensorHAL_data *)dev; - - return hal_data->sensor_classes[handle]->Enable(handle, (bool)enabled); -} - -/** - * st_hal_dev_close() - Close device sensors module - * @dev: sensors device structure. - * - * Return value: 0 on success, negative number if fail. - */ -static int st_hal_dev_close(struct hw_device_t *dev) -{ - int i; - STSensorHAL_data *hal_data = (STSensorHAL_data *)dev; - - for (i = 0; i < (int)hal_data->sensor_available; i++) - delete hal_data->sensor_classes[i]; - - free(hal_data->threads); - close(hal_data->android_pollfd.fd); - free(hal_data->sensor_t_list); - free(hal_data); - - return 0; -} - - -#define PIPE_SIZE (64 * 1024) - -/** - * st_hal_create_android_pipe() - Create dev_poll pipe - * @hal_data: hal common data. - * - * Return value: 0 on success, negative number if fail. - */ -static int st_hal_create_android_pipe(STSensorHAL_data *hal_data) -{ - int err, pipe_fd[2]; - - err = pipe(pipe_fd); - if (err < 0) - return err; - - fcntl(pipe_fd[0], F_SETFL, O_NONBLOCK); - fcntl(pipe_fd[1], F_SETFL, O_NONBLOCK); - fcntl(pipe_fd[1], F_SETPIPE_SZ, PIPE_SIZE); - - hal_data->android_pollfd.events = POLLIN; - hal_data->android_pollfd.fd = pipe_fd[0]; - - return pipe_fd[1]; -} - -/** - * open_sensors() - Open sensor device - * see Android documentation. - * - * Return value: 0 on success, negative number if fail. - */ -static int st_hal_open_sensors(const struct hw_module_t *module, - const char __attribute__((unused))*id, - struct hw_device_t **device) -{ - bool real_sensor_class; - STSensorHAL_data *hal_data; - int sensor_class_valid_num =0 ; - bool sensor_class_valid[ST_HAL_IIO_MAX_DEVICES]; - int type_dependencies[SENSOR_BASE_MAX_DEPENDENCY], type_index, type_sensor_trigger; - SensorBase *sensor_class, *temp_sensor_class[ST_HAL_IIO_MAX_DEVICES]; - STSensorHAL_iio_devices_data iio_devices_data[ST_HAL_IIO_MAX_DEVICES]; - int err, i, c, android_write_pipe_fd, device_found_num, classes_available = 0, n = 0; - bool temp_sensor_class_virtual[ST_HAL_IIO_MAX_DEVICES]; - - hal_data = (STSensorHAL_data *)malloc(sizeof(STSensorHAL_data)); - if (!hal_data) - return -ENOMEM; - - hal_data->sensor_available = 0; - hal_data->poll_device.common.tag = HARDWARE_DEVICE_TAG; - hal_data->poll_device.common.version = ST_HAL_IIO_DEVICE_API_VERSION; - hal_data->poll_device.common.module = const_cast<hw_module_t*>(module); - hal_data->poll_device.common.close = st_hal_dev_close; - hal_data->poll_device.common.module->dso = hal_data; - hal_data->poll_device.activate = st_hal_dev_activate; - hal_data->poll_device.setDelay = st_hal_dev_setDelay; - hal_data->poll_device.poll = st_hal_dev_poll; - hal_data->poll_device.batch = st_hal_dev_batch; - hal_data->poll_device.flush = st_hal_dev_flush; - - do_cal_data_loading(ACCEL_SINDEX, NON_WAKEUP); - do_cal_data_loading(ACCEL_SINDEX, WAKEUP); - do_cal_data_loading(GYRO_SINDEX, NON_WAKEUP); - do_cal_data_loading(GYRO_SINDEX, WAKEUP); - - *device = &hal_data->poll_device.common; - - device_found_num = st_hal_load_iio_devices_data(iio_devices_data); - if (device_found_num <= 0) { - err = device_found_num; - goto free_hal_data; - } - - android_write_pipe_fd = st_hal_create_android_pipe(hal_data); - if (android_write_pipe_fd < 0) { - ALOGE("Failed to create Android pipe file."); - err = device_found_num; - goto free_hal_data; - } - - for (i = 0; i < device_found_num; i++) { - sensor_class = st_hal_create_class_sensor(&iio_devices_data[i], classes_available + 1, android_write_pipe_fd); - - free(iio_devices_data[i].iio_sysfs_path); - free(iio_devices_data[i].android_name); - free(iio_devices_data[i].channels); - - if (!sensor_class) { - ALOGE("\"%s\": failed to create HW sensor class.", iio_devices_data[i].device_name); - free(iio_devices_data[i].device_name); - continue; - } - - free(iio_devices_data[i].device_name); - -#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_VERBOSE) - ALOGD("\"%s\": created HW class instance (sensor type: %d).", sensor_class->GetName(), sensor_class->GetType()); -#endif /* CONFIG_ST_HAL_DEBUG_LEVEL */ - - temp_sensor_class[classes_available] = sensor_class; - temp_sensor_class_virtual[classes_available] = false; - sensor_class_valid[classes_available] = true; - sensor_class_valid_num++; - classes_available++; - } - if (classes_available == 0) { - ALOGE("Failed to create HW sensors classes."); - err = -ENODEV; - goto close_android_pipe_fd; - } - - for (i = 0; i < ARRAY_SIZE(ST_virtual_sensors_list); i++) { - sensor_class = st_hal_create_virtual_class_sensor(ST_virtual_sensors_list[i].sensor_type, classes_available + 1, android_write_pipe_fd); - if (!sensor_class) { - ALOGE("Failed to create SW sensor class (sensor type: %d).", ST_virtual_sensors_list[i].sensor_type); - continue; - } - -#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_VERBOSE) - if (sensor_class->GetType() < SENSOR_TYPE_ST_CUSTOM_NO_SENSOR) - ALOGD("\"%s\": created SW class instance (sensor type: %d).", sensor_class->GetName(), sensor_class->GetType()); -#endif /* CONFIG_ST_HAL_DEBUG_LEVEL */ - - temp_sensor_class[classes_available] = sensor_class; - temp_sensor_class_virtual[classes_available] = true; - sensor_class_valid[classes_available] = true; - sensor_class_valid_num++; - classes_available++; - } - - for (i = 0; i < classes_available; i ++) { - temp_sensor_class[i]->GetDepenciesTypeList(type_dependencies); - type_index = 0; - - while((type_dependencies[type_index] > 0) && (type_index < SENSOR_BASE_MAX_DEPENDENCY)) { - err = 0; - - for (c = 0; c < classes_available; c++) { - if ((type_dependencies[type_index] == temp_sensor_class[c]->GetType()) && (sensor_class_valid[c])) { - if (temp_sensor_class_virtual[i]) - err = ((SWSensorBase *)temp_sensor_class[i])->AddSensorDependency(temp_sensor_class[c]); - else - err = ((HWSensorBase *)temp_sensor_class[i])->AddSensorDependency(temp_sensor_class[c]); - - break; - } - } - if ((c == classes_available) || (err < 0)) { - ALOGE("\"%s\": failed to add dependency (sensor type dependency: %d).", temp_sensor_class[i]->GetName(), type_dependencies[type_index]); - - while (type_index > 0) { - type_index--; - - for (c = 0; c < classes_available; c++) { - if ((type_dependencies[type_index] == temp_sensor_class[c]->GetType()) && (sensor_class_valid[c])) { - if (temp_sensor_class_virtual[i]) - ((SWSensorBase *)temp_sensor_class[i])->RemoveSensorDependency(temp_sensor_class[c]); - else - ((HWSensorBase *)temp_sensor_class[i])->RemoveSensorDependency(temp_sensor_class[c]); - - break; - } - } - } - - sensor_class_valid_num--; - sensor_class_valid[i] = false; - goto failed_to_check_dependency; - } - - type_index++; - } - - type_sensor_trigger = temp_sensor_class[i]->GetSensorNeedTriggerType(); - if (type_sensor_trigger < 0) - continue; - - err = 0; - - for (c = 0; c < classes_available; c++) { - if (type_sensor_trigger == temp_sensor_class[c]->GetType()) { - err = temp_sensor_class[c]->AddSensorToTrigger(temp_sensor_class[i]); - break; - } - } - if ((c == classes_available) || (err < 0)) { - ALOGE("\"%s\": failed to add trigger (sensor trigger type: %d).", temp_sensor_class[i]->GetName(), type_sensor_trigger); - sensor_class_valid_num--; - sensor_class_valid[i] = false; - break; - } - -failed_to_check_dependency: - continue; - } - - for (i = 0; i < classes_available; i++) { - if (sensor_class_valid[i]) - hal_data->sensor_classes[temp_sensor_class[i]->GetHandle()] = temp_sensor_class[i]; - } - - hal_data->sensor_t_list = (struct sensor_t *)malloc(sensor_class_valid_num * sizeof(struct sensor_t)); - if (!hal_data->sensor_t_list) { - err = -ENOMEM; - goto destroy_classes; - } - - hal_data->threads = (pthread_t *)malloc(sensor_class_valid_num * sizeof(pthread_t *)); - if (!hal_data->threads) { - err = -ENOMEM; - goto free_sensor_t_list; - } - - for (i = 0; i < classes_available; i++) { - if (sensor_class_valid[i]) { - err = pthread_create(&hal_data->threads[i], NULL, &SensorBase::ThreadWork, (void *)temp_sensor_class[i]); - if (err < 0) - continue; - - real_sensor_class = hal_data->sensor_classes[temp_sensor_class[i]->GetHandle()]->FillSensor_tData(&hal_data->sensor_t_list[n]); - if (!real_sensor_class) - continue; - - n++; - } else - delete temp_sensor_class[i]; - } - - hal_data->sensor_available = n; - -#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_INFO) - ALOGD("%d sensors available and ready.", n); -#endif /* CONFIG_ST_HAL_DEBUG_LEVEL */ - - return 0; - -free_sensor_t_list: - free(hal_data->sensor_t_list); -destroy_classes: - for (i = 0; i < classes_available; i ++) - delete temp_sensor_class[i]; - -close_android_pipe_fd: - close(android_write_pipe_fd); - close(hal_data->android_pollfd.fd); -free_hal_data: - free(hal_data); - - return err; -} - -/** - * get_sensors_list() - Get sensors list - * @module: hardware specific informations. - * @list: sensors list. - * - * Return value: number of sensors available. - */ -static int st_hal_get_sensors_list(struct sensors_module_t *module, - struct sensor_t const **list) -{ - STSensorHAL_data *hal_data = (STSensorHAL_data *)module->common.dso; - - *list = (struct sensor_t const *)hal_data->sensor_t_list; - - return hal_data->sensor_available; -}; - -/* - * struct hw_module_methods_t - Hardware module functions - * see Android documentation. - */ -static struct hw_module_methods_t st_hal_sensors_module_methods = { - open: st_hal_open_sensors -}; - -/* - * struct sensors_module_t - Hardware module info - * see Android documentation. - */ -struct sensors_module_t HAL_MODULE_INFO_SYM = { - .common = { - .tag = HARDWARE_MODULE_TAG, - .module_api_version = SENSORS_MODULE_API_VERSION_0_1, - .hal_api_version = 0, - .id = SENSORS_HARDWARE_MODULE_ID, - .name = "STMicroelectronics Sensors Module", - .author = "STMicroelectronics", - .methods = &st_hal_sensors_module_methods, - .dso = NULL, - .reserved = { }, - }, - .get_sensors_list = st_hal_get_sensors_list, -}; |