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