From 049ab1da89d389f05870f7e30d585ecfc14524dd Mon Sep 17 00:00:00 2001 From: Baixing Tan Date: Wed, 25 Mar 2015 11:16:57 +0800 Subject: Original code get from ST for lsm6ds3 sensor base on iio subsystem Add original SensorHAL_IIO_v3.1.0 for lsm6ds3 sensor Change-Id: Iebd049a36696f9b1f09049cc8672ed260c12cdf7 Tracked-On: https://jira01.devtools.intel.com/browse/MARVIN-175 Signed-off-by: Baixing Tan Reviewed-on: https://android.intel.com:443/346267 --- libsensors_iio/Android.mk | 108 +++ libsensors_iio/Kconfig | 80 ++ libsensors_iio/Kconfig_menu/Kconfig_common | 57 ++ libsensors_iio/Kconfig_menu/Kconfig_drivers | 13 + libsensors_iio/Kconfig_menu/Kconfig_enable_sensors | 194 ++++ libsensors_iio/Kconfig_menu/Kconfig_fusion_config | 31 + libsensors_iio/README | 29 + libsensors_iio/lib/Android.mk | 17 + libsensors_iio/src/Accelerometer.cpp | 55 ++ libsensors_iio/src/Accelerometer.h | 37 + libsensors_iio/src/Android.mk | 172 ++++ libsensors_iio/src/CircularBuffer.cpp | 75 ++ libsensors_iio/src/CircularBuffer.h | 55 ++ libsensors_iio/src/Gyroscope.cpp | 137 +++ libsensors_iio/src/Gyroscope.h | 39 + libsensors_iio/src/HWSensorBase.cpp | 453 ++++++++++ libsensors_iio/src/HWSensorBase.h | 101 +++ libsensors_iio/src/Magnetometer.cpp | 115 +++ libsensors_iio/src/Magnetometer.h | 38 + libsensors_iio/src/Pressure.cpp | 46 + libsensors_iio/src/Pressure.h | 37 + libsensors_iio/src/SWAccelGyroFusion6X.cpp | 141 +++ libsensors_iio/src/SWAccelGyroFusion6X.h | 40 + libsensors_iio/src/SWAccelMagnFusion6X.cpp | 166 ++++ libsensors_iio/src/SWAccelMagnFusion6X.h | 43 + libsensors_iio/src/SWAccelMagnGyroFusion9X.cpp | 178 ++++ libsensors_iio/src/SWAccelMagnGyroFusion9X.h | 43 + libsensors_iio/src/SWGameRotationVector.cpp | 50 ++ libsensors_iio/src/SWGameRotationVector.h | 31 + libsensors_iio/src/SWGeoMagRotationVector.cpp | 50 ++ libsensors_iio/src/SWGeoMagRotationVector.h | 31 + libsensors_iio/src/SWGravity.cpp | 50 ++ libsensors_iio/src/SWGravity.h | 31 + libsensors_iio/src/SWGyroscopeUncalibrated.cpp | 49 + libsensors_iio/src/SWGyroscopeUncalibrated.h | 31 + libsensors_iio/src/SWLinearAccel.cpp | 50 ++ libsensors_iio/src/SWLinearAccel.h | 31 + libsensors_iio/src/SWMagnetometerUncalibrated.cpp | 49 + libsensors_iio/src/SWMagnetometerUncalibrated.h | 31 + libsensors_iio/src/SWOrientation.cpp | 50 ++ libsensors_iio/src/SWOrientation.h | 31 + libsensors_iio/src/SWRotationVector.cpp | 50 ++ libsensors_iio/src/SWRotationVector.h | 31 + libsensors_iio/src/SWSensorBase.cpp | 148 +++ libsensors_iio/src/SWSensorBase.h | 64 ++ libsensors_iio/src/SensorBase.cpp | 483 ++++++++++ libsensors_iio/src/SensorBase.h | 157 ++++ libsensors_iio/src/SensorHAL.cpp | 976 ++++++++++++++++++++ libsensors_iio/src/SensorHAL.h | 104 +++ libsensors_iio/src/SignificantMotion.cpp | 50 ++ libsensors_iio/src/SignificantMotion.h | 36 + libsensors_iio/src/StepCounter.cpp | 83 ++ libsensors_iio/src/StepCounter.h | 40 + libsensors_iio/src/StepDetector.cpp | 53 ++ libsensors_iio/src/StepDetector.h | 37 + libsensors_iio/src/TiltSensor.cpp | 48 + libsensors_iio/src/TiltSensor.h | 36 + libsensors_iio/src/android_KK_defconfig | 151 ++++ libsensors_iio/src/android_L_defconfig | 67 ++ libsensors_iio/src/common_data.h | 55 ++ libsensors_iio/src/events.h | 106 +++ libsensors_iio/src/iio_utils.c | 987 +++++++++++++++++++++ libsensors_iio/src/iio_utils.h | 146 +++ libsensors_iio/src/types.h | 65 ++ libsensors_iio/tools/cfgdefine.c | 222 +++++ libsensors_iio/tools/cfgdefine.h | 50 ++ libsensors_iio/tools/kconfig-language.txt | 395 +++++++++ libsensors_iio/tools/kconfig-mconf | Bin 0 -> 482182 bytes libsensors_iio/tools/mkconfig | Bin 0 -> 14294 bytes libsensors_iio/tools/mkconfig.c | 90 ++ 70 files changed, 7795 insertions(+) create mode 100644 libsensors_iio/Android.mk create mode 100644 libsensors_iio/Kconfig create mode 100644 libsensors_iio/Kconfig_menu/Kconfig_common create mode 100644 libsensors_iio/Kconfig_menu/Kconfig_drivers create mode 100644 libsensors_iio/Kconfig_menu/Kconfig_enable_sensors create mode 100644 libsensors_iio/Kconfig_menu/Kconfig_fusion_config create mode 100644 libsensors_iio/README create mode 100644 libsensors_iio/lib/Android.mk create mode 100644 libsensors_iio/src/Accelerometer.cpp create mode 100644 libsensors_iio/src/Accelerometer.h create mode 100644 libsensors_iio/src/Android.mk create mode 100644 libsensors_iio/src/CircularBuffer.cpp create mode 100644 libsensors_iio/src/CircularBuffer.h create mode 100644 libsensors_iio/src/Gyroscope.cpp create mode 100644 libsensors_iio/src/Gyroscope.h create mode 100644 libsensors_iio/src/HWSensorBase.cpp create mode 100644 libsensors_iio/src/HWSensorBase.h create mode 100644 libsensors_iio/src/Magnetometer.cpp create mode 100644 libsensors_iio/src/Magnetometer.h create mode 100644 libsensors_iio/src/Pressure.cpp create mode 100644 libsensors_iio/src/Pressure.h create mode 100644 libsensors_iio/src/SWAccelGyroFusion6X.cpp create mode 100644 libsensors_iio/src/SWAccelGyroFusion6X.h create mode 100644 libsensors_iio/src/SWAccelMagnFusion6X.cpp create mode 100644 libsensors_iio/src/SWAccelMagnFusion6X.h create mode 100644 libsensors_iio/src/SWAccelMagnGyroFusion9X.cpp create mode 100644 libsensors_iio/src/SWAccelMagnGyroFusion9X.h create mode 100644 libsensors_iio/src/SWGameRotationVector.cpp create mode 100644 libsensors_iio/src/SWGameRotationVector.h create mode 100644 libsensors_iio/src/SWGeoMagRotationVector.cpp create mode 100644 libsensors_iio/src/SWGeoMagRotationVector.h create mode 100644 libsensors_iio/src/SWGravity.cpp create mode 100644 libsensors_iio/src/SWGravity.h create mode 100644 libsensors_iio/src/SWGyroscopeUncalibrated.cpp create mode 100644 libsensors_iio/src/SWGyroscopeUncalibrated.h create mode 100644 libsensors_iio/src/SWLinearAccel.cpp create mode 100644 libsensors_iio/src/SWLinearAccel.h create mode 100644 libsensors_iio/src/SWMagnetometerUncalibrated.cpp create mode 100644 libsensors_iio/src/SWMagnetometerUncalibrated.h create mode 100644 libsensors_iio/src/SWOrientation.cpp create mode 100644 libsensors_iio/src/SWOrientation.h create mode 100644 libsensors_iio/src/SWRotationVector.cpp create mode 100644 libsensors_iio/src/SWRotationVector.h create mode 100644 libsensors_iio/src/SWSensorBase.cpp create mode 100644 libsensors_iio/src/SWSensorBase.h create mode 100644 libsensors_iio/src/SensorBase.cpp create mode 100644 libsensors_iio/src/SensorBase.h create mode 100644 libsensors_iio/src/SensorHAL.cpp create mode 100644 libsensors_iio/src/SensorHAL.h create mode 100644 libsensors_iio/src/SignificantMotion.cpp create mode 100644 libsensors_iio/src/SignificantMotion.h create mode 100644 libsensors_iio/src/StepCounter.cpp create mode 100644 libsensors_iio/src/StepCounter.h create mode 100644 libsensors_iio/src/StepDetector.cpp create mode 100644 libsensors_iio/src/StepDetector.h create mode 100644 libsensors_iio/src/TiltSensor.cpp create mode 100644 libsensors_iio/src/TiltSensor.h create mode 100644 libsensors_iio/src/android_KK_defconfig create mode 100644 libsensors_iio/src/android_L_defconfig create mode 100644 libsensors_iio/src/common_data.h create mode 100644 libsensors_iio/src/events.h create mode 100644 libsensors_iio/src/iio_utils.c create mode 100644 libsensors_iio/src/iio_utils.h create mode 100644 libsensors_iio/src/types.h create mode 100644 libsensors_iio/tools/cfgdefine.c create mode 100644 libsensors_iio/tools/cfgdefine.h create mode 100644 libsensors_iio/tools/kconfig-language.txt create mode 100755 libsensors_iio/tools/kconfig-mconf create mode 100755 libsensors_iio/tools/mkconfig create mode 100644 libsensors_iio/tools/mkconfig.c diff --git a/libsensors_iio/Android.mk b/libsensors_iio/Android.mk new file mode 100644 index 0000000..1a206c3 --- /dev/null +++ b/libsensors_iio/Android.mk @@ -0,0 +1,108 @@ +# +# Copyright (C) 2013-2015 STMicroelectronics +# Denis Ciocca - Motion MEMS Product Div. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +#/ + +ifneq ($(TARGET_SIMULATOR),true) + +CURRENT_DIRECTORY := $(call my-dir) + +include $(CLEAR_VARS) + +MAJOR_VERSION := $(shell echo $(PLATFORM_VERSION) | cut -f1 -d.) +MINOR_VERSION := $(shell echo $(PLATFORM_VERSION) | cut -f2 -d.) + +VERSION_KK := $(shell test $(MAJOR_VERSION) -eq 4 -a $(MINOR_VERSION) -eq 4 && echo true) +VERSION_L := $(shell test $(MAJOR_VERSION) -eq 5 && echo true) + +ifeq ($(VERSION_KK),true) +export ST_HAL_ANDROID_VERSION=0 +DEFCONFIG := android_KK_defconfig +endif +ifeq ($(VERSION_L),true) +export ST_HAL_ANDROID_VERSION=1 +DEFCONFIG := android_L_defconfig +endif + +ifneq ("$(wildcard $(CURRENT_DIRECTORY)/lib/FUFD_CustomTilt/FUFD_CustomTilt*)","") +export ST_HAL_HAS_FDFD_LIB=y +else +export ST_HAL_HAS_FDFD_LIB=n +endif + +ifneq ("$(wildcard $(CURRENT_DIRECTORY)/lib/iNemoEngine_gbias_Estimation/iNemoEngine_gbias_Estimation*)","") +export ST_HAL_HAS_GBIAS_LIB=y +else +export ST_HAL_HAS_GBIAS_LIB=n +endif + +ifneq ("$(wildcard $(CURRENT_DIRECTORY)/lib/iNemoEngine_GeoMag_Fusion/iNemoEngine_GeoMag_Fusion*)","") +export ST_HAL_HAS_GEOMAG_LIB=y +else +export ST_HAL_HAS_GEOMAG_LIB=n +endif + +ifneq ("$(wildcard $(CURRENT_DIRECTORY)/lib/iNemoEngine_SensorFusion/iNemoEngine_SensorFusion*)","") +export ST_HAL_HAS_9X_6X_LIB=y +else +export ST_HAL_HAS_9X_6X_LIB=n +endif + +ifneq ("$(wildcard $(CURRENT_DIRECTORY)/lib/STCompass/STCompass*)","") +export ST_HAL_HAS_COMPASS_LIB=y +else +export ST_HAL_HAS_COMPASS_LIB=n +endif + +export KCONFIG_CONFIG=$(CURRENT_DIRECTORY)/.config +export ST_HAL_PATH=$(CURRENT_DIRECTORY) + +all_modules: +ifeq (${menuconfig},y) +ifeq ("$(wildcard $(CURRENT_DIRECTORY)/.config)","") +$(error .config file not found! use `mm defconfig=y` first.) +endif + + gcc -o $(CURRENT_DIRECTORY)/tools/mkconfig $(CURRENT_DIRECTORY)/tools/mkconfig.c $(CURRENT_DIRECTORY)/tools/cfgdefine.c + $(CURRENT_DIRECTORY)/tools/kconfig-mconf $(CURRENT_DIRECTORY)/Kconfig + $(CURRENT_DIRECTORY)/tools/mkconfig $(CURRENT_DIRECTORY)/ > $(CURRENT_DIRECTORY)/configuration.h +else +ifeq (${defconfig},y) + cp $(CURRENT_DIRECTORY)/src/$(DEFCONFIG) $(CURRENT_DIRECTORY)/.config + $(CURRENT_DIRECTORY)/tools/mkconfig $(CURRENT_DIRECTORY)/ > $(CURRENT_DIRECTORY)/configuration.h +else +ifeq (${clean},y) +ifneq ("$(wildcard $(CURRENT_DIRECTORY)/.config)","") + rm $(CURRENT_DIRECTORY)/.config +endif +ifneq ("$(wildcard $(CURRENT_DIRECTORY)/configuration.h)","") + rm $(CURRENT_DIRECTORY)/configuration.h +endif +ifneq ("$(wildcard $(CURRENT_DIRECTORY)/.config.old)","") + rm $(CURRENT_DIRECTORY)/.config.old +endif +else +ifeq ("$(wildcard $(CURRENT_DIRECTORY)/.config)","") +$(error .config file not found! use `mm defconfig=y` first.) +endif + +$(shell $(CURRENT_DIRECTORY)/tools/mkconfig $(CURRENT_DIRECTORY)/ > $(CURRENT_DIRECTORY)/configuration.h) +include $(call all-makefiles-under, $(CURRENT_DIRECTORY)) + +endif +endif +endif + +endif # !TARGET_SIMULATOR diff --git a/libsensors_iio/Kconfig b/libsensors_iio/Kconfig new file mode 100644 index 0000000..8619323 --- /dev/null +++ b/libsensors_iio/Kconfig @@ -0,0 +1,80 @@ +# +# For a description of the syntax of this configuration file, +# see tools/kconfig-language.txt. +# + +mainmenu "SensorHAL IIO Configuration" + +config ST_HAL_PATH + string + option env="ST_HAL_PATH" + +config ENV_ANDROID_VERSION + int + option env="ST_HAL_ANDROID_VERSION" + +if (ENV_ANDROID_VERSION = 0) +config ST_HAL_ANDROID_VERSION + int + default 0 +endif +if (ENV_ANDROID_VERSION = 1) +config ST_HAL_ANDROID_VERSION + int + default 1 +endif + + +config ST_HAL_HAS_6AX_FUSION_CHECK + bool + option env="ST_HAL_HAS_9X_6X_LIB" + +config ST_HAL_HAS_9AX_FUSION_CHECK + bool + option env="ST_HAL_HAS_9X_6X_LIB" + +config ST_HAL_HAS_GEOMAG_FUSION_CHECK + bool + option env="ST_HAL_HAS_GEOMAG_LIB" + +config ST_HAL_HAS_MAGN_CALIB_CHECK + bool + option env="ST_HAL_HAS_COMPASS_LIB" + +config ST_HAL_HAS_GYRO_GBIAS_ESTIMATION_CHECK + bool + option env="ST_HAL_HAS_GBIAS_LIB" + +config ST_HAL_HAS_TILT_FU_FD_CHECK + bool + option env="ST_HAL_HAS_FDFD_LIB" + + +config ST_HAL_HAS_6AX_FUSION + bool + default ST_HAL_HAS_6AX_FUSION_CHECK + +config ST_HAL_HAS_9AX_FUSION + bool + default ST_HAL_HAS_9AX_FUSION_CHECK + +config ST_HAL_HAS_GEOMAG_FUSION + bool + default ST_HAL_HAS_GEOMAG_FUSION_CHECK + +config ST_HAL_HAS_MAGN_CALIB + bool + default ST_HAL_HAS_MAGN_CALIB_CHECK + +config ST_HAL_HAS_GYRO_GBIAS_ESTIMATION + bool + default ST_HAL_HAS_GYRO_GBIAS_ESTIMATION_CHECK + +config ST_HAL_HAS_TILT_FU_FD + bool + default ST_HAL_HAS_TILT_FU_FD_CHECK + +source "$ST_HAL_PATH/Kconfig_menu/Kconfig_enable_sensors" +source "$ST_HAL_PATH/Kconfig_menu/Kconfig_common" +source "$ST_HAL_PATH/Kconfig_menu/Kconfig_fusion_config" +source "$ST_HAL_PATH/Kconfig_menu/Kconfig_drivers" diff --git a/libsensors_iio/Kconfig_menu/Kconfig_common b/libsensors_iio/Kconfig_menu/Kconfig_common new file mode 100644 index 0000000..697aa5f --- /dev/null +++ b/libsensors_iio/Kconfig_menu/Kconfig_common @@ -0,0 +1,57 @@ +# +# Common configuration +# + +menu "Common configuration" +config ST_HAL_MAX_SAMPLING_FREQUENCY + int "Max sampling frequency selectable [Hz]" + range 50 400 + default 200 + help + Maximium value of sampling frequency that can be used in IIO driver. + +config ST_HAL_DEBUG_LEVEL + int "Debug level" + range 0 3 + default 0 + help + Debugging information level. [0 - 3] + 0: error only; + 1: info messages; + 2: verbose; + 3: extra-verbose; + +if ST_HAL_ACCEL_ENABLED +config ST_HAL_ACCEL_ROT_MATRIX + string "Accelerometer Rotation matrix" + default "1,0,0,0,1,0,0,0,1" + +config ST_HAL_ACCEL_RANGE + int "Accelerometer Max Range (m/s^2)" + default 17 + +endif + +if ST_HAL_MAGN_ENABLED || ST_HAL_MAGN_UNCALIB_HW_ENABLED +config ST_HAL_MAGN_ROT_MATRIX + string "Magnetometer Rotation matrix" + default "1,0,0,0,1,0,0,0,1" + +config ST_HAL_MAGN_RANGE + int "Magnetometer Max Range (uTesla)" + default 1000 + +endif + +if ST_HAL_GYRO_ENABLED || ST_HAL_GYRO_UNCALIB_HW_ENABLED +config ST_HAL_GYRO_ROT_MATRIX + string "Gyroscope Rotation matrix" + default "1,0,0,0,1,0,0,0,1" + +config ST_HAL_GYRO_RANGE + int "Gyroscope Max Range (rad/sec)" + default 35 + +endif + +endmenu # Common configuration diff --git a/libsensors_iio/Kconfig_menu/Kconfig_drivers b/libsensors_iio/Kconfig_menu/Kconfig_drivers new file mode 100644 index 0000000..c45c70c --- /dev/null +++ b/libsensors_iio/Kconfig_menu/Kconfig_drivers @@ -0,0 +1,13 @@ +# +# IIO drivers configuration +# + +menu "IIO supported sensors" + +config ST_HAL_LSM6DS3_ENABLED + bool "LSM6DS3 sensor" + default y + help + Enable support for LSM6DS3 sensor. + +endmenu # IIO drivers configuration diff --git a/libsensors_iio/Kconfig_menu/Kconfig_enable_sensors b/libsensors_iio/Kconfig_menu/Kconfig_enable_sensors new file mode 100644 index 0000000..a36cbe3 --- /dev/null +++ b/libsensors_iio/Kconfig_menu/Kconfig_enable_sensors @@ -0,0 +1,194 @@ +# +# Enable/Disable sensors +# + +menu "Enable/disable sensors" +config ST_HAL_ACCEL_ENABLED + bool "Accelerometer sensor" + default y + help + Enable support for Accelerometer sensor. + +config ST_HAL_MAGN_ENABLED + bool "Magnetometer sensor" + default y + help + Enable support for Magnetometer sensor. + +if ST_HAL_MAGN_ENABLED && ST_HAL_HAS_MAGN_CALIB +config ST_HAL_MAGN_CALIB_ENABLED + bool "Enable Magnetometer calibration Library" + depends on ST_HAL_ACCEL_ENABLED + default n + help + Enable Magnetometer calibration library on Software by Application Processor Library. + +endif + +config ST_HAL_GYRO_ENABLED + bool "Gyroscope sensor" + default y + help + Enable support for Gyroscope sensor. + +if ST_HAL_GYRO_ENABLED && ST_HAL_HAS_GYRO_GBIAS_ESTIMATION +config ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED + bool "Enable Gyroscope gbias estimation Library" + depends on ST_HAL_ACCEL_ENABLED + default n + help + Enable Gyroscope bias library estimation on Software by Application Processor Library. + +endif + +choice + prompt "Magnetometer Uncalibrated sensor" + default ST_HAL_MAGN_UNCALIB_DISABLED + help + Enable support for Magnetometer Uncalibrated sensor. + +config ST_HAL_MAGN_UNCALIB_DISABLED + bool "Disabled" +config ST_HAL_MAGN_UNCALIB_AP_ENABLED + depends on ST_HAL_MAGN_CALIB_ENABLED + bool "Software by Application Processor Library" +endchoice + +choice + prompt "Gyroscope Uncalibrated sensor" + default ST_HAL_GYRO_UNCALIB_DISABLED + help + Enable support for Gyroscope Uncalibrated sensor. + +config ST_HAL_GYRO_UNCALIB_DISABLED + bool "Disabled" +config ST_HAL_GYRO_UNCALIB_AP_ENABLED + depends on ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED + bool "Software by Application Processor Library" +endchoice + +config ST_HAL_SIGN_MOTION_ENABLED + bool "Significant Motion sensor" + default y + help + Enable support for Significant Motion sensor. + +config ST_HAL_STEP_DETECTOR_ENABLED + bool "Step Detector sensor sensor" + default y + help + Enable support for Step Detector sensor. + +config ST_HAL_STEP_COUNTER_ENABLED + bool "Step Counter sensor" + default y + help + Enable support for Step Counter sensor. + +config ST_HAL_TILT_ENABLED + bool "Tilt sensor" + default y + help + Enable support for Tilt sensor. + +config ST_HAL_PRESSURE_ENABLED + bool "Pressure sensor" + default y + help + Enable support for Pressure sensor. + +choice + prompt "Orientation sensor" + default ST_HAL_ORIENTATION_DISABLED + help + Enable support for Orientation sensor. + +config ST_HAL_ORIENTATION_DISABLED + bool "Disabled" +config ST_HAL_ORIENTATION_AP_ENABLED + depends on ST_HAL_HAS_9AX_FUSION + depends on ST_HAL_ACCEL_ENABLED + depends on ST_HAL_MAGN_ENABLED + depends on ST_HAL_GYRO_ENABLED + bool "Software by Application Processor Library" +endchoice + +choice + prompt "Gravity sensor" + default ST_HAL_GRAVITY_DISABLED + help + Enable support for Gravity sensor. + +config ST_HAL_GRAVITY_DISABLED + bool "Disabled" +config ST_HAL_GRAVITY_AP_ENABLED + depends on ST_HAL_HAS_9AX_FUSION + depends on ST_HAL_ACCEL_ENABLED + depends on ST_HAL_MAGN_ENABLED + depends on ST_HAL_GYRO_ENABLED + bool "Software by Application Processor Library" +endchoice + +choice + prompt "Linear Acceleration sensor" + default ST_HAL_LINEAR_DISABLED + help + Enable support for Linear Acceleration sensor. + +config ST_HAL_LINEAR_DISABLED + bool "Disabled" +config ST_HAL_LINEAR_AP_ENABLED + depends on ST_HAL_HAS_9AX_FUSION + depends on ST_HAL_ACCEL_ENABLED + depends on ST_HAL_MAGN_ENABLED + depends on ST_HAL_GYRO_ENABLED + bool "Software by Application Processor Library" +endchoice + +choice + prompt "Rotation Vector sensor" + default ST_HAL_ROT_VECTOR_DISABLED + help + Enable support for Rotation Vector sensor. + +config ST_HAL_ROT_VECTOR_DISABLED + bool "Disabled" +config ST_HAL_ROT_VECTOR_AP_ENABLED + depends on ST_HAL_HAS_9AX_FUSION + depends on ST_HAL_ACCEL_ENABLED + depends on ST_HAL_MAGN_ENABLED + depends on ST_HAL_GYRO_ENABLED + bool "Software by Application Processor Library" +endchoice + +choice + prompt "Game Rotation Vector sensor" + default ST_HAL_GAME_ROT_VECTOR_DISABLED + help + Enable support for Game Rotation Vector sensor. + +config ST_HAL_GAME_ROT_VECTOR_DISABLED + bool "Disabled" +config ST_HAL_GAME_ROT_VECTOR_AP_ENABLED + depends on ST_HAL_HAS_6AX_FUSION + depends on ST_HAL_ACCEL_ENABLED + depends on ST_HAL_GYRO_ENABLED + bool "Software by Application Processor Library" +endchoice + +choice + prompt "Geomagnetic Rotation Vector sensor" + default ST_HAL_GEOMAG_ROT_VECTOR_DISABLED + help + Enable support for Geomagnetic Rotation Vector sensor. + +config ST_HAL_GEOMAG_ROT_VECTOR_DISABLED + bool "Disabled" +config ST_HAL_GEOMAG_ROT_VECTOR_AP_ENABLED + depends on ST_HAL_HAS_GEOMAG_FUSION + depends on ST_HAL_ACCEL_ENABLED + depends on ST_HAL_MAGN_ENABLED + bool "Software by Application Processor Library" +endchoice + +endmenu # Enable/disable sensors diff --git a/libsensors_iio/Kconfig_menu/Kconfig_fusion_config b/libsensors_iio/Kconfig_menu/Kconfig_fusion_config new file mode 100644 index 0000000..843ed45 --- /dev/null +++ b/libsensors_iio/Kconfig_menu/Kconfig_fusion_config @@ -0,0 +1,31 @@ +# +# SensorFusion configuration +# + +if ST_HAL_HAS_6AX_FUSION || ST_HAL_HAS_9AX_FUSION || ST_HAL_HAS_GEOMAG_FUSION +menu "SensorFusion configuration" + +config ST_HAL_MIN_FUSION_POLLRATE + int "Min SensorFusion run frequency [Hz]" + range 20 ST_HAL_MAX_SAMPLING_FREQUENCY + default 50 + +config ST_HAL_INEMO_GBIAS_THRESOLD_ACCEL + string "Accelerometer Gbias thresold" + default "86999e-9" + +if ST_HAL_HAS_9AX_FUSION || ST_HAL_HAS_GEOMAG_FUSION +config ST_HAL_INEMO_GBIAS_THRESOLD_MAGN + string "Magnetometer Gbias thresold" + default "599999e-9" + +endif + +if ST_HAL_HAS_6AX_FUSION || ST_HAL_HAS_9AX_FUSION +config ST_HAL_INEMO_GBIAS_THRESOLD_GYRO + string "Gyroscope Gbias thresold" + default "114999e-9" + +endif +endmenu # Sensor Fusion configuration +endif diff --git a/libsensors_iio/README b/libsensors_iio/README new file mode 100644 index 0000000..3b981fc --- /dev/null +++ b/libsensors_iio/README @@ -0,0 +1,29 @@ + Android STMicroelectronics SensorHAL release 3.x + +These are the release notes for STMicroelectronics SensorHAL version 3. +Read them carefully, as they tell you what this is all about, explain how +to install the dynamic library, and what to do if something goes wrong. + +SOFTWARE REQUIREMENTS + + Compiling and running the 3.x SensorHAL requires up-to-date versions of + various software packages. + + Installing required packages: + $ sudo apt-get install build-essential libncurses5-dev + +LIBS + In order to run some sensors in the application processor side, libraries + are needed. Run auto-extract script packages in the root folder of the HAL. + +CONFIGURING the HAL: + + "mm defconfig=y" Generated default .config. + + "mm menuconfig=y" Text based color menus, radiolists & dialogs. + + "mm clean=y" Delete .config and configuration.h files. + +COMPILING the HAL + + "mm" Build the code diff --git a/libsensors_iio/lib/Android.mk b/libsensors_iio/lib/Android.mk new file mode 100644 index 0000000..83175be --- /dev/null +++ b/libsensors_iio/lib/Android.mk @@ -0,0 +1,17 @@ +# Copyright (C) 2012 STMicroelectronics +# Matteo Dameno, Denis Ciocca - Motion MEMS Product Div. +# Copyright (C) 2008 The Android Open Source Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +include $(call all-subdir-makefiles) diff --git a/libsensors_iio/src/Accelerometer.cpp b/libsensors_iio/src/Accelerometer.cpp new file mode 100644 index 0000000..98ed7c6 --- /dev/null +++ b/libsensors_iio/src/Accelerometer.cpp @@ -0,0 +1,55 @@ +/* + * STMicroelectronics Accelerometer Sensor Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "Accelerometer.h" + +Accelerometer::Accelerometer(HWSensorBaseCommonData *data, const char *name, + struct iio_sampling_frequency_available *sfa, int handle, + unsigned int hw_fifo_len, int pipe_data_fd, float power_consumption, bool wakeup) : + HWSensorBaseWithPollrate(data, name, sfa, handle, + SENSOR_TYPE_ACCELEROMETER, hw_fifo_len, pipe_data_fd, power_consumption) +{ + sensor_t_data.stringType = SENSOR_STRING_TYPE_ACCELEROMETER; + sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE; + + if (wakeup) + sensor_t_data.flags |= SENSOR_FLAG_WAKE_UP; + + sensor_t_data.resolution = data->channels[0].scale; + sensor_t_data.maxRange = sensor_t_data.resolution * (pow(2, data->channels[0].bits_used - 1) - 1); +} + +Accelerometer::~Accelerometer() +{ + +} + +void Accelerometer::ProcessData(SensorBaseData *data) +{ + float tmp_raw_data[num_data_axis]; + + memcpy(tmp_raw_data, data->raw, num_data_axis * sizeof(float)); + + data->raw[0] = SENSOR_X_DATA(tmp_raw_data[0], tmp_raw_data[1], tmp_raw_data[2], CONFIG_ST_HAL_ACCEL_ROT_MATRIX); + data->raw[1] = SENSOR_Y_DATA(tmp_raw_data[0], tmp_raw_data[1], tmp_raw_data[2], CONFIG_ST_HAL_ACCEL_ROT_MATRIX); + data->raw[2] = SENSOR_Z_DATA(tmp_raw_data[0], tmp_raw_data[1], tmp_raw_data[2], CONFIG_ST_HAL_ACCEL_ROT_MATRIX); + + sensor_event.acceleration.x = data->raw[0]; + sensor_event.acceleration.y = data->raw[1]; + sensor_event.acceleration.z = data->raw[2]; + sensor_event.acceleration.status = SENSOR_STATUS_UNRELIABLE; + sensor_event.timestamp = data->timestamp; + + HWSensorBaseWithPollrate::WriteDataToPipe(); + HWSensorBaseWithPollrate::ProcessData(data); +} diff --git a/libsensors_iio/src/Accelerometer.h b/libsensors_iio/src/Accelerometer.h new file mode 100644 index 0000000..3996adb --- /dev/null +++ b/libsensors_iio/src/Accelerometer.h @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_ACCELEROMETER_SENSOR_H +#define ST_ACCELEROMETER_SENSOR_H + +#include "HWSensorBase.h" + +/* + * class Accelerometer + */ +class Accelerometer : public HWSensorBaseWithPollrate { +public: + Accelerometer(HWSensorBaseCommonData *data, const char *name, + struct iio_sampling_frequency_available *sfa, int handle, + unsigned int hw_fifo_len, int pipe_data_fd, + float power_consumption, bool wakeup); + ~Accelerometer(); + + virtual void ProcessData(SensorBaseData *data); +}; + +#endif /* ST_ACCELEROMETER_SENSOR_H */ diff --git a/libsensors_iio/src/Android.mk b/libsensors_iio/src/Android.mk new file mode 100644 index 0000000..10b40dc --- /dev/null +++ b/libsensors_iio/src/Android.mk @@ -0,0 +1,172 @@ +# +# Copyright (C) 2013-2015 STMicroelectronics +# Denis Ciocca - Motion MEMS Product Div. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +#/ + +ifneq ($(TARGET_SIMULATOR),true) + +LOCAL_PATH := $(call my-dir) +ST_HAL_ROOT_PATH := $(call my-dir) + +include $(CLEAR_VARS) +include $(ST_HAL_ROOT_PATH)/../.config + +LOCAL_PRELINK_MODULE := false + +ifdef TARGET_BOARD_PLATFORM +LOCAL_MODULE := sensors.$(TARGET_BOARD_PLATFORM) +else +LOCAL_MODULE := sensors.default +endif + +LOCAL_MODULE_RELATIVE_PATH := hw + +LOCAL_MODULE_OWNER := STMicroelectronics + +LOCAL_C_INCLUDES := $(LOCAL_PATH)/ + +LOCAL_CFLAGS += -DLOG_TAG=\"SensorHAL\" + + +ifeq ($(DEBUG),y) +LOCAL_CFLAGS += -g -O0 +endif + + +ifdef CONFIG_ST_HAL_HAS_6AX_FUSION +LOCAL_C_INCLUDES += $(LOCAL_PATH)/../lib/iNemoEngine_SensorFusion +LOCAL_STATIC_LIBRARIES += iNemoEngine_SensorFusion +else +ifdef CONFIG_ST_HAL_HAS_9AX_FUSION +LOCAL_C_INCLUDES += $(LOCAL_PATH)/../lib/iNemoEngine_SensorFusion +LOCAL_STATIC_LIBRARIES += iNemoEngine_SensorFusion +endif +endif + +ifdef CONFIG_ST_HAL_HAS_GYRO_GBIAS_ESTIMATION +LOCAL_C_INCLUDES += $(LOCAL_PATH)/../lib/iNemoEngine_gbias_Estimation +LOCAL_STATIC_LIBRARIES += iNemoEngine_gbias_Estimation +endif + +ifdef CONFIG_ST_HAL_HAS_TILT_FU_FD +LOCAL_C_INCLUDES += $(LOCAL_PATH)/../lib/FUFD_CustomTilt +LOCAL_STATIC_LIBRARIES += FUFD_CustomTilt +endif + +ifdef CONFIG_ST_HAL_HAS_GEOMAG_FUSION +LOCAL_C_INCLUDES += $(LOCAL_PATH)/../lib/iNemoEngine_GeoMag_Fusion +LOCAL_STATIC_LIBRARIES += iNemoEngine_GeoMag_Fusion +endif + +ifdef CONFIG_ST_HAL_HAS_MAGN_CALIB +LOCAL_C_INCLUDES += $(LOCAL_PATH)/../lib/STCompass +LOCAL_STATIC_LIBRARIES += STCompass +endif + + +LOCAL_SRC_FILES := \ + iio_utils.c \ + SensorHAL.cpp \ + CircularBuffer.cpp \ + SensorBase.cpp \ + HWSensorBase.cpp \ + SWSensorBase.cpp + +ifdef CONFIG_ST_HAL_ACCEL_ENABLED +LOCAL_SRC_FILES += Accelerometer.cpp +endif + +ifdef CONFIG_ST_HAL_MAGN_ENABLED +LOCAL_SRC_FILES += Magnetometer.cpp +endif + +ifdef CONFIG_ST_HAL_GYRO_ENABLED +LOCAL_SRC_FILES += Gyroscope.cpp +endif + +ifdef CONFIG_ST_HAL_STEP_DETECTOR_ENABLED +LOCAL_SRC_FILES += StepDetector.cpp +endif + +ifdef CONFIG_ST_HAL_STEP_COUNTER_ENABLED +LOCAL_SRC_FILES += StepCounter.cpp +endif + +ifdef CONFIG_ST_HAL_SIGN_MOTION_ENABLED +LOCAL_SRC_FILES += SignificantMotion.cpp +endif + +ifdef CONFIG_ST_HAL_TILT_ENABLED +LOCAL_SRC_FILES += TiltSensor.cpp +endif + +ifdef CONFIG_ST_HAL_MAGN_UNCALIB_AP_ENABLED +LOCAL_SRC_FILES += SWMagnetometerUncalibrated.cpp +endif + +ifdef CONFIG_ST_HAL_GYRO_UNCALIB_AP_ENABLED +LOCAL_SRC_FILES += SWGyroscopeUncalibrated.cpp +endif + +ifdef CONFIG_ST_HAL_PRESSURE_ENABLED +LOCAL_SRC_FILES += Pressure.cpp +endif + +ifdef CONFIG_ST_HAL_HAS_GEOMAG_FUSION +LOCAL_SRC_FILES += SWAccelMagnFusion6X.cpp +endif + +ifdef CONFIG_ST_HAL_GEOMAG_ROT_VECTOR_AP_ENABLED +LOCAL_SRC_FILES += SWGeoMagRotationVector.cpp +endif + +ifdef CONFIG_ST_HAL_HAS_6AX_FUSION +LOCAL_SRC_FILES += SWAccelGyroFusion6X.cpp +endif + +ifdef CONFIG_ST_HAL_GAME_ROT_VECTOR_AP_ENABLED +LOCAL_SRC_FILES += SWGameRotationVector.cpp +endif + +ifdef CONFIG_ST_HAL_HAS_9AX_FUSION +LOCAL_SRC_FILES += SWAccelMagnGyroFusion9X.cpp +endif + +ifdef CONFIG_ST_HAL_ROT_VECTOR_AP_ENABLED +LOCAL_SRC_FILES += SWRotationVector.cpp +endif + +ifdef CONFIG_ST_HAL_ORIENTATION_AP_ENABLED +LOCAL_SRC_FILES += SWOrientation.cpp +endif + +ifdef CONFIG_ST_HAL_GRAVITY_AP_ENABLED +LOCAL_SRC_FILES += SWGravity.cpp +endif + +ifdef CONFIG_ST_HAL_LINEAR_AP_ENABLED +LOCAL_SRC_FILES += SWLinearAccel.cpp +endif + + +LOCAL_SHARED_LIBRARIES := liblog libcutils libutils libdl libc + +LOCAL_MODULE_TAGS := optional + +include $(BUILD_SHARED_LIBRARY) + +include $(call all-makefiles-under, $(LOCAL_PATH)) + +endif # !TARGET_SIMULATOR diff --git a/libsensors_iio/src/CircularBuffer.cpp b/libsensors_iio/src/CircularBuffer.cpp new file mode 100644 index 0000000..d2c280e --- /dev/null +++ b/libsensors_iio/src/CircularBuffer.cpp @@ -0,0 +1,75 @@ +/* + * STMicroelectronics Circular Buffer Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include "CircularBuffer.h" +#include +CircularBuffer::CircularBuffer(unsigned int num_elements) +{ + data_sensor = (SensorBaseData *)malloc(num_elements * sizeof(SensorBaseData)); + + pthread_mutex_init(&data_mutex, NULL); + + lenght = num_elements; + elements_available = 0; + first_free_element = &data_sensor[0]; + first_available_element = &data_sensor[0]; +} + +CircularBuffer::~CircularBuffer() +{ + delete data_sensor; +} + +void CircularBuffer::writeElement(SensorBaseData *data) +{ + pthread_mutex_lock(&data_mutex); + + if (elements_available == lenght) { + first_available_element++; + + if (first_available_element == (&data_sensor[0] + lenght)) + first_available_element = &data_sensor[0]; + } + + memcpy(first_free_element, data, sizeof(SensorBaseData)); + first_free_element++; + + if (first_free_element == (&data_sensor[0] + lenght)) + first_free_element = &data_sensor[0]; + + if (elements_available < lenght) + elements_available++; + + pthread_mutex_unlock(&data_mutex); +} + +int CircularBuffer::readElement(SensorBaseData *data) +{ + int num_remaining_elements; + + pthread_mutex_lock(&data_mutex); + + if (elements_available == 0) { + pthread_mutex_unlock(&data_mutex); + return -EFAULT; + } + + memcpy(data, first_available_element, sizeof(SensorBaseData)); + first_available_element++; + + if (first_available_element == (&data_sensor[0] + lenght)) + first_available_element = &data_sensor[0]; + + elements_available--; + num_remaining_elements = elements_available; + + pthread_mutex_unlock(&data_mutex); + + return num_remaining_elements; +} diff --git a/libsensors_iio/src/CircularBuffer.h b/libsensors_iio/src/CircularBuffer.h new file mode 100644 index 0000000..c07eb1f --- /dev/null +++ b/libsensors_iio/src/CircularBuffer.h @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_CIRCULAR_BUFFER_H +#define ST_CIRCULAR_BUFFER_H + +#include +#include +#include +#include +#include +#include + +typedef struct SensorBaseData { + float raw[4]; + float offset[4]; + float processed[4]; + int64_t timestamp; +} SensorBaseData; + +/* + * class CircularBuffer + */ +class CircularBuffer { +private: + pthread_mutex_t data_mutex; + unsigned int lenght, elements_available; + + SensorBaseData *data_sensor; + SensorBaseData *first_available_element; + SensorBaseData *first_free_element; + +public: + CircularBuffer(unsigned int num_elements); + ~CircularBuffer(); + + void writeElement(SensorBaseData *data); + int readElement(SensorBaseData *data); +}; + +#endif /* ST_CIRCULAR_BUFFER_H */ diff --git a/libsensors_iio/src/Gyroscope.cpp b/libsensors_iio/src/Gyroscope.cpp new file mode 100644 index 0000000..aef27ff --- /dev/null +++ b/libsensors_iio/src/Gyroscope.cpp @@ -0,0 +1,137 @@ +/* + * STMicroelectronics Gyroscope Sensor Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "Gyroscope.h" + +#ifdef CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED +extern "C" { + #include "iNemoEngineAPI_gbias_estimation.h" +} +#endif /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */ + +Gyroscope::Gyroscope(HWSensorBaseCommonData *data, const char *name, + struct iio_sampling_frequency_available *sfa, int handle, + unsigned int hw_fifo_len, int pipe_data_fd, float power_consumption, bool wakeup) : + HWSensorBaseWithPollrate(data, name, sfa, handle, + SENSOR_TYPE_GYROSCOPE, hw_fifo_len, pipe_data_fd, power_consumption) +{ + sensor_t_data.stringType = SENSOR_STRING_TYPE_GYROSCOPE; + sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE; + + if (wakeup) + sensor_t_data.flags |= SENSOR_FLAG_WAKE_UP; + + sensor_t_data.resolution = data->channels[0].scale; + sensor_t_data.maxRange = sensor_t_data.resolution * (pow(2, data->channels[0].bits_used - 1) - 1); + +#ifdef CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED + iNemoEngine_API_gbias_Initialization(NULL); + + type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_ACCELEROMETER; +#endif /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */ +} + +Gyroscope::~Gyroscope() +{ + +} + +int Gyroscope::Enable(int handle, bool enable) +{ +#ifdef CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED + int err; + bool old_status; + + old_status = GetStatus(); + + err = HWSensorBaseWithPollrate::Enable(handle, enable); + if (err < 0) + return err; + + if (GetStatus() != old_status) + iNemoEngine_API_gbias_enable(enable); + + return 0; +#else /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */ + return HWSensorBaseWithPollrate::Enable(handle, enable); +#endif /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */ +} + +int Gyroscope::SetDelay(int handle, int64_t period_ns, int64_t timeout) +{ +#ifdef CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED + int err; + + err = HWSensorBaseWithPollrate::SetDelay(handle, period_ns, timeout); + if (err < 0) + return err; + + + iNemoEngine_API_gbias_set_frequency(NS_TO_FREQUENCY(GetRealPollrate())); + + return 0; +#else /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */ + return HWSensorBaseWithPollrate::SetDelay(handle, period_ns, timeout); +#endif /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */ +} + +void Gyroscope::ProcessData(SensorBaseData *data) +{ + float tmp_raw_data[num_data_axis]; +#ifdef CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED + int64_t time_diff = 0; + int err, nomaxdata = 10; + SensorBaseData accel_data; +#endif /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */ + + memcpy(tmp_raw_data, data->raw, num_data_axis * sizeof(float)); + + data->raw[0] = SENSOR_X_DATA(tmp_raw_data[0], tmp_raw_data[1], tmp_raw_data[2], CONFIG_ST_HAL_GYRO_ROT_MATRIX); + data->raw[1] = SENSOR_Y_DATA(tmp_raw_data[0], tmp_raw_data[1], tmp_raw_data[2], CONFIG_ST_HAL_GYRO_ROT_MATRIX); + data->raw[2] = SENSOR_Z_DATA(tmp_raw_data[0], tmp_raw_data[1], tmp_raw_data[2], CONFIG_ST_HAL_GYRO_ROT_MATRIX); + +#ifdef CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED + do { + err = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &accel_data); + if (err < 0) { + nomaxdata--; + usleep(200); + continue; + } + + time_diff = data->timestamp - accel_data.timestamp; + + } while ((time_diff >= GetRealPollrate()) && (nomaxdata > 0)); + + if (err >= 0) + iNemoEngine_API_gbias_Run(accel_data.raw, data->raw); + + iNemoEngine_API_Get_gbias(data->offset); + + sensor_event.gyro.status = SENSOR_STATUS_ACCURACY_HIGH; +#else /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */ + sensor_event.gyro.status = SENSOR_STATUS_UNRELIABLE; +#endif /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */ + + data->processed[0] = data->raw[0] - data->offset[0]; + data->processed[1] = data->raw[1] - data->offset[1]; + data->processed[2] = data->raw[2] - data->offset[2]; + + sensor_event.gyro.x = data->processed[0]; + sensor_event.gyro.y = data->processed[1]; + sensor_event.gyro.z = data->processed[2]; + sensor_event.timestamp = data->timestamp; + + HWSensorBaseWithPollrate::WriteDataToPipe(); + HWSensorBaseWithPollrate::ProcessData(data); +} diff --git a/libsensors_iio/src/Gyroscope.h b/libsensors_iio/src/Gyroscope.h new file mode 100644 index 0000000..8465366 --- /dev/null +++ b/libsensors_iio/src/Gyroscope.h @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_GYROSCOPE_SENSOR_H +#define ANDROID_GYROSCOPE_SENSOR_H + +#include "HWSensorBase.h" + +/* + * class Gyroscope + */ +class Gyroscope : public HWSensorBaseWithPollrate { +public: + Gyroscope(HWSensorBaseCommonData *data, const char *name, + struct iio_sampling_frequency_available *sfa, int handle, + unsigned int hw_fifo_len, int pipe_data_fd, + float power_consumption, bool wakeup); + ~Gyroscope(); + + virtual int Enable(int handle, bool enable); + virtual int SetDelay(int handle, int64_t period_ns, int64_t timeout); + virtual void ProcessData(SensorBaseData *data); +}; + +#endif /* ANDROID_GYROSCOPE_SENSOR_H */ 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 - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#define __STDC_LIMIT_MACROS + +#include +#include +#include +#include +#include + +#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; + } +} diff --git a/libsensors_iio/src/HWSensorBase.h b/libsensors_iio/src/HWSensorBase.h new file mode 100644 index 0000000..c2b0440 --- /dev/null +++ b/libsensors_iio/src/HWSensorBase.h @@ -0,0 +1,101 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_HWSENSOR_BASE_H +#define ST_HWSENSOR_BASE_H + +#include +#include + +#include "SensorBase.h" + +extern "C" { + #include "iio_utils.h" + #include "events.h" +}; + +#define HW_SENSOR_BASE_DEFAULT_IIO_BUFFER_LEN (2) +#define HW_SENSOR_BASE_IIO_SYSFS_PATH_MAX (50) +#define HW_SENSOR_BASE_IIO_DEVICE_NAME_MAX (30) +#define HW_SENSOR_BASE_MAX_CHANNELS (8) + +#define FILENAME_BUFFER_ENABLE "buffer/enable" +#define FILENAME_BUFFER_LENGTH "buffer/length" +#define FILENAME_SAMPLING_FREQ "sampling_frequency" +#define FILENAME_MAX_RATE_DELIVERY "max_delivery_rate" +#define FILENAME_HRTIMER_TRIGGER_FREQ "frequency" +#define FILENAME_FLUSH "flush" + +struct HWSensorBaseCommonData { + char iio_sysfs_path[HW_SENSOR_BASE_IIO_SYSFS_PATH_MAX]; + char device_name[HW_SENSOR_BASE_IIO_DEVICE_NAME_MAX]; + unsigned int iio_dev_num; + + int num_channels; + struct iio_channel_info channels[HW_SENSOR_BASE_MAX_CHANNELS]; + + struct iio_scale_available sa; +} typedef HWSensorBaseCommonData; + + +class HWSensorBase; +class HWSensorBaseWithPollrate; + +/* + * class HWSensorBase + */ +class HWSensorBase : public SensorBase { +protected: + struct pollfd pollfd_iio[2]; + ssize_t scan_size; + uint8_t *sensor_data; + unsigned int current_fifo_len; + HWSensorBaseCommonData common_data; + + int WriteBufferLenght(unsigned int buf_len); + +public: + HWSensorBase(HWSensorBaseCommonData *data, const char *name, + int handle, int sensor_type, unsigned int hw_fifo_len, + int pipe_data_fd, float power_consumption); + virtual ~HWSensorBase(); + + virtual int Enable(int handle, bool enable); + virtual int FlushData(); + virtual void ThreadTask(); +}; + + +/* + * class HWSensorBaseWithPollrate + */ +class HWSensorBaseWithPollrate : public HWSensorBase { +private: + struct iio_sampling_frequency_available sampling_frequency_available; + +public: + 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); + virtual ~HWSensorBaseWithPollrate(); + + virtual int SetDelay(int handle, int64_t period_ns, int64_t timeout); + virtual void WriteDataToPipe(); +}; + +#endif /* ST_HWSENSOR_BASE_H */ diff --git a/libsensors_iio/src/Magnetometer.cpp b/libsensors_iio/src/Magnetometer.cpp new file mode 100644 index 0000000..4ba8d64 --- /dev/null +++ b/libsensors_iio/src/Magnetometer.cpp @@ -0,0 +1,115 @@ +/* + * STMicroelectronics Magnetometer Sensor Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "Magnetometer.h" + +#ifdef CONFIG_ST_HAL_MAGN_CALIB_ENABLED +extern "C" { + #include "STCompass_API.h" +} +#endif /* CONFIG_ST_HAL_MAGN_CALIB_ENABLED */ + +Magnetometer::Magnetometer(HWSensorBaseCommonData *data, const char *name, + struct iio_sampling_frequency_available *sfa, int handle, + unsigned int hw_fifo_len, int pipe_data_fd, float power_consumption, bool wakeup) : + HWSensorBaseWithPollrate(data, name, sfa, handle, + SENSOR_TYPE_MAGNETIC_FIELD, hw_fifo_len, pipe_data_fd, power_consumption) +{ + sensor_t_data.stringType = SENSOR_STRING_TYPE_MAGNETIC_FIELD; + sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE; + + if (wakeup) + sensor_t_data.flags |= SENSOR_FLAG_WAKE_UP; + + sensor_t_data.resolution = GAUSS_TO_UTESLA(data->channels[0].scale); + sensor_t_data.maxRange = sensor_t_data.resolution * (pow(2, data->channels[0].bits_used - 1) - 1); + +#ifdef CONFIG_ST_HAL_MAGN_CALIB_ENABLED + type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_ACCELEROMETER; +#endif /* CONFIG_ST_HAL_MAGN_CALIB_ENABLED */ +} + +Magnetometer::~Magnetometer() +{ + +} + +int Magnetometer::Enable(int handle, bool enable) +{ +#ifdef CONFIG_ST_HAL_MAGN_CALIB_ENABLED + int err; + bool old_status; + + old_status = GetStatus(); + + err = HWSensorBaseWithPollrate::Enable(handle, enable); + if (err < 0) + return err; + + if (GetStatus() && !old_status) + STCompass_API_Init(NULL); + + return 0; +#else /* CONFIG_ST_HAL_MAGN_CALIB_ENABLED */ + return HWSensorBaseWithPollrate::Enable(handle, enable); +#endif /* CONFIG_ST_HAL_MAGN_CALIB_ENABLED */ +} + +void Magnetometer::ProcessData(SensorBaseData *data) +{ + float tmp_raw_data[num_data_axis]; +#ifdef CONFIG_ST_HAL_MAGN_CALIB_ENABLED + int64_t time_diff = 0; + int err, nomaxdata = 10; + SensorBaseData accel_data; +#endif /* CONFIG_ST_HAL_MAGN_CALIB_ENABLED */ + + memcpy(tmp_raw_data, data->raw, num_data_axis * sizeof(float)); + + data->raw[0] = GAUSS_TO_UTESLA(SENSOR_X_DATA(tmp_raw_data[0], tmp_raw_data[1], tmp_raw_data[2], CONFIG_ST_HAL_MAGN_ROT_MATRIX)); + data->raw[1] = GAUSS_TO_UTESLA(SENSOR_Y_DATA(tmp_raw_data[0], tmp_raw_data[1], tmp_raw_data[2], CONFIG_ST_HAL_MAGN_ROT_MATRIX)); + data->raw[2] = GAUSS_TO_UTESLA(SENSOR_Z_DATA(tmp_raw_data[0], tmp_raw_data[1], tmp_raw_data[2], CONFIG_ST_HAL_MAGN_ROT_MATRIX)); + +#ifdef CONFIG_ST_HAL_MAGN_CALIB_ENABLED + do { + err = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &accel_data); + if (err < 0) { + nomaxdata--; + usleep(200); + continue; + } + + time_diff = data->timestamp - accel_data.timestamp; + + } while ((time_diff >= GetRealPollrate()) && (nomaxdata > 0)); + + if (err >= 0) + STCompass_API_Run(accel_data.raw, data->raw); + + sensor_event.magnetic.status = STCompass_API_Get_Calibration_Data(data->offset); +#else /* CONFIG_ST_HAL_MAGN_CALIB_ENABLED */ + sensor_event.magnetic.status = SENSOR_STATUS_UNRELIABLE; +#endif /* CONFIG_ST_HAL_MAGN_CALIB_ENABLED */ + + data->processed[0] = data->raw[0] - data->offset[0]; + data->processed[1] = data->raw[1] - data->offset[1]; + data->processed[2] = data->raw[2] - data->offset[2]; + + sensor_event.magnetic.azimuth = data->processed[0]; + sensor_event.magnetic.pitch = data->processed[1]; + sensor_event.magnetic.roll = data->processed[2]; + sensor_event.timestamp = data->timestamp; + + HWSensorBaseWithPollrate::WriteDataToPipe(); + HWSensorBaseWithPollrate::ProcessData(data); +} diff --git a/libsensors_iio/src/Magnetometer.h b/libsensors_iio/src/Magnetometer.h new file mode 100644 index 0000000..0fa4b57 --- /dev/null +++ b/libsensors_iio/src/Magnetometer.h @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_MAGNETOMETER_SENSOR_H +#define ANDROID_MAGNETOMETER_SENSOR_H + +#include "HWSensorBase.h" + +/* + * class Magnetometer + */ +class Magnetometer : public HWSensorBaseWithPollrate { +public: + Magnetometer(HWSensorBaseCommonData *data, const char *name, + struct iio_sampling_frequency_available *sfa, int handle, + unsigned int hw_fifo_len, int pipe_data_fd, + float power_consumption, bool wakeup); + ~Magnetometer(); + + virtual int Enable(int handle, bool enable); + virtual void ProcessData(SensorBaseData *data); +}; + +#endif /* ANDROID_MAGNETOMETER_SENSOR_H */ diff --git a/libsensors_iio/src/Pressure.cpp b/libsensors_iio/src/Pressure.cpp new file mode 100644 index 0000000..3fe8820 --- /dev/null +++ b/libsensors_iio/src/Pressure.cpp @@ -0,0 +1,46 @@ +/* + * STMicroelectronics Pressure Sensor Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "Pressure.h" + +Pressure::Pressure(HWSensorBaseCommonData *data, const char *name, + struct iio_sampling_frequency_available *sfa, int handle, + unsigned int hw_fifo_len, int pipe_data_fd, float power_consumption, bool wakeup) : + HWSensorBaseWithPollrate(data, name, sfa, handle, + SENSOR_TYPE_PRESSURE, hw_fifo_len, pipe_data_fd, power_consumption) +{ + sensor_t_data.stringType = SENSOR_STRING_TYPE_PRESSURE; + sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE; + + if (wakeup) + sensor_t_data.flags |= SENSOR_FLAG_WAKE_UP; + + sensor_t_data.resolution = data->channels[0].scale; + sensor_t_data.maxRange = sensor_t_data.resolution * (pow(2, data->channels[0].bits_used) - 1); + + num_data_axis = SENSOR_BASE_1AXIS; +} + +Pressure::~Pressure() +{ + +} + +void Pressure::ProcessData(SensorBaseData *data) +{ + sensor_event.pressure = data->raw[0]; + sensor_event.timestamp = data->timestamp; + + HWSensorBaseWithPollrate::WriteDataToPipe(); + HWSensorBaseWithPollrate::ProcessData(data); +} diff --git a/libsensors_iio/src/Pressure.h b/libsensors_iio/src/Pressure.h new file mode 100644 index 0000000..7c230bd --- /dev/null +++ b/libsensors_iio/src/Pressure.h @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_PRESSURE_SENSOR_H +#define ST_PRESSURE_SENSOR_H + +#include "HWSensorBase.h" + +/* + * class Pressure + */ +class Pressure : public HWSensorBaseWithPollrate { +public: + Pressure(HWSensorBaseCommonData *data, const char *name, + struct iio_sampling_frequency_available *sfa, int handle, + unsigned int hw_fifo_len, int pipe_data_fd, + float power_consumption, bool wakeup); + ~Pressure(); + + virtual void ProcessData(SensorBaseData *data); +}; + +#endif /* ST_PRESSURE_SENSOR_H */ diff --git a/libsensors_iio/src/SWAccelGyroFusion6X.cpp b/libsensors_iio/src/SWAccelGyroFusion6X.cpp new file mode 100644 index 0000000..f03c950 --- /dev/null +++ b/libsensors_iio/src/SWAccelGyroFusion6X.cpp @@ -0,0 +1,141 @@ +/* + * STMicroelectronics Accel-Gyro Fusion 6X Sensor Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "SWAccelGyroFusion6X.h" + +extern "C" { + #include "iNemoEngineAPI.h" +} + +SWAccelGyroFusion6X::SWAccelGyroFusion6X(const char *name, int handle, int pipe_data_fd) : + SWSensorBaseWithPollrate(name, handle, SENSOR_TYPE_ST_ACCEL_GYRO_FUSION6X, + pipe_data_fd, false, false, true, false) +{ + sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE; + + sensor_t_data.resolution = ST_SENSOR_FUSION_RESOLUTION(1.0f); + sensor_t_data.maxRange = 1.0f; + + type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_ACCELEROMETER; + type_dependencies[SENSOR_BASE_DEPENDENCY_1] = SENSOR_TYPE_GYROSCOPE; + type_sensor_need_trigger = SENSOR_TYPE_GYROSCOPE; + + iNemoEngine_API_Initialization_6X(NULL); +} + +SWAccelGyroFusion6X::~SWAccelGyroFusion6X() +{ + +} + +int SWAccelGyroFusion6X::Enable(int handle, bool enable) +{ + int err; + bool old_status; + + old_status = GetStatus(); + + err = SWSensorBaseWithPollrate::Enable(handle, enable); + if (err < 0) + return err; + + if ((GetStatus() && !old_status) || (!GetStatus() && old_status)) { + sensor_event.timestamp = 0; + iNemoEngine_API_enable_6X(enable); + } + + return 0; +} + +int SWAccelGyroFusion6X::SetDelay(int handle, int64_t period_ns, int64_t timeout) +{ + int err; + + if ((period_ns > FREQUENCY_TO_NS(CONFIG_ST_HAL_MIN_FUSION_POLLRATE) && period_ns != INT64_MAX)) + period_ns = FREQUENCY_TO_NS(CONFIG_ST_HAL_MIN_FUSION_POLLRATE); + + err = SWSensorBaseWithPollrate::SetDelay(handle, period_ns, timeout); + if (err < 0) + return err; + + real_pollrate = dependencies[SENSOR_BASE_DEPENDENCY_1]->GetRealPollrate(); + + return 0; +} + +void SWAccelGyroFusion6X::SplitAndProcessData(SensorBaseData data[ST_ACCEL_GYRO_MAX_OUT_ID]) +{ + int i, id, sensor_type; + trigger_mutex *dep_mutex; + + for (i = 0; i < (int)sensors_to_push_data_num; i++) { + if (sensors_to_push_data[i]->GetStatus()) { + switch (sensors_to_push_data_type[i]) { + case SENSOR_TYPE_GAME_ROTATION_VECTOR: + id = ST_ACCEL_GYRO_ROTATION_VECTOR_OUT_ID; + break; + default: + continue; + } + + sensors_to_push_data[i]->ReceiveDataFromDependency(sensor_t_data.handle, &data[id]); + } + } + + for (i = 0; i < (int)sensors_to_trigger_num; i++) { + if (sensors_to_trigger[i]->GetStatus()) { + dep_mutex = sensors_to_trigger[i]->GetMutexForTrigger(); + pthread_mutex_lock(&dep_mutex->trigger_mutex); + pthread_cond_signal(&dep_mutex->trigger_data_cond); + pthread_mutex_unlock(&dep_mutex->trigger_mutex); + } + } +} + +void SWAccelGyroFusion6X::TriggerEventReceived() +{ + int64_t time_diff = 0; + SensorBaseData accel_data, gyro_data; + int err, data_remaining_gyro, nomaxdata = 10; + + do { + data_remaining_gyro = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_1, &gyro_data); + if (data_remaining_gyro < 0) + return; + + do { + err = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &accel_data); + if (err < 0) { + nomaxdata--; + usleep(200); + continue; + } + + time_diff = gyro_data.timestamp - accel_data.timestamp; + + } while ((time_diff >= GetRealPollrate()) && (nomaxdata > 0)); + + if (err >= 0) + iNemoEngine_API_Run_6X(accel_data.raw, gyro_data.processed, gyro_data.timestamp); + + sensor_event.timestamp = gyro_data.timestamp; + + err = iNemoEngine_API_Get_Quaternion_6X(outdata[ST_ACCEL_GYRO_ROTATION_VECTOR_OUT_ID].processed); + if (err < 0) + return; + + outdata[ST_ACCEL_GYRO_ROTATION_VECTOR_OUT_ID].timestamp = sensor_event.timestamp; + + SplitAndProcessData(outdata); + } while (data_remaining_gyro > 0); +} diff --git a/libsensors_iio/src/SWAccelGyroFusion6X.h b/libsensors_iio/src/SWAccelGyroFusion6X.h new file mode 100644 index 0000000..1e2fdd8 --- /dev/null +++ b/libsensors_iio/src/SWAccelGyroFusion6X.h @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_SW_ACCEL_GYRO_6X_FUSION_H +#define ST_SW_ACCEL_GYRO_6X_FUSION_H + +#include "SWSensorBase.h" + +#define ST_ACCEL_GYRO_ROTATION_VECTOR_OUT_ID (0) +#define ST_ACCEL_GYRO_MAX_OUT_ID (1) + +class SWAccelGyroFusion6X : public SWSensorBaseWithPollrate { +protected: + SensorBaseData outdata[ST_ACCEL_GYRO_MAX_OUT_ID]; + +public: + SWAccelGyroFusion6X(const char *name, int handle, int pipe_data_fd); + ~SWAccelGyroFusion6X(); + + virtual int Enable(int handle, bool enable); + virtual int SetDelay(int handle, int64_t period_ns, int64_t timeout); + virtual void SplitAndProcessData(SensorBaseData data[ST_ACCEL_GYRO_MAX_OUT_ID]); + virtual void TriggerEventReceived(); +}; + +#endif /* ST_SW_ACCEL_GYRO_6X_FUSION_H */ diff --git a/libsensors_iio/src/SWAccelMagnFusion6X.cpp b/libsensors_iio/src/SWAccelMagnFusion6X.cpp new file mode 100644 index 0000000..4822256 --- /dev/null +++ b/libsensors_iio/src/SWAccelMagnFusion6X.cpp @@ -0,0 +1,166 @@ +/* + * STMicroelectronics Accel-Magn Fusion 6X Sensor Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "SWAccelMagnFusion6X.h" + +extern "C" { + #include "iNemoEngineGeoMagAPI.h" +} + +SWAccelMagnFusion6X::SWAccelMagnFusion6X(const char *name, int handle, int pipe_data_fd) : + SWSensorBaseWithPollrate(name, handle, SENSOR_TYPE_ST_ACCEL_MAGN_FUSION6X, + pipe_data_fd, false, false, true, false) +{ + sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE; + + sensor_t_data.resolution = ST_SENSOR_FUSION_RESOLUTION(1.0f); + sensor_t_data.maxRange = 1.0f; + + type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_ACCELEROMETER; + type_dependencies[SENSOR_BASE_DEPENDENCY_1] = SENSOR_TYPE_MAGNETIC_FIELD; + type_sensor_need_trigger = SENSOR_TYPE_MAGNETIC_FIELD; +} + +SWAccelMagnFusion6X::~SWAccelMagnFusion6X() +{ + +} + +int SWAccelMagnFusion6X::Enable(int handle, bool enable) +{ + int err; + bool old_status; + + old_status = GetStatus(); + + err = SWSensorBaseWithPollrate::Enable(handle, enable); + if (err < 0) + return err; + + if (GetStatus() && !old_status) { + sensor_event.timestamp = 0; + iNemoEngine_GeoMag_API_Initialization(); + } + + return 0; +} + +int SWAccelMagnFusion6X::SetDelay(int handle, int64_t period_ns, int64_t timeout) +{ + int err; + + if ((period_ns > FREQUENCY_TO_NS(CONFIG_ST_HAL_MIN_FUSION_POLLRATE) && period_ns != INT64_MAX)) + period_ns = FREQUENCY_TO_NS(CONFIG_ST_HAL_MIN_FUSION_POLLRATE); + + err = SWSensorBaseWithPollrate::SetDelay(handle, period_ns, timeout); + if (err < 0) + return err; + + real_pollrate = dependencies[SENSOR_BASE_DEPENDENCY_1]->GetRealPollrate(); + + return 0; +} + +void SWAccelMagnFusion6X::SplitAndProcessData(SensorBaseData data[ST_GEOMAG_MAX_OUT_ID]) +{ + int i, id, sensor_type; + trigger_mutex *dep_mutex; + + for (i = 0; i < (int)sensors_to_push_data_num; i++) { + if (sensors_to_push_data[i]->GetStatus()) { + switch (sensors_to_push_data_type[i]) { + case SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR: + id = ST_GEOMAG_ROTATION_VECTOR_OUT_ID; + break; + case SENSOR_TYPE_ORIENTATION: + id = ST_GEOMAG_ORIENTATION_OUT_ID; + break; + case SENSOR_TYPE_LINEAR_ACCELERATION: + id = ST_GEOMAG_LINEAR_ACCEL_OUT_ID; + break; + case SENSOR_TYPE_GRAVITY: + id = ST_GEOMAG_GRAVITY_OUT_ID; + break; + default: + continue; + } + + sensors_to_push_data[i]->ReceiveDataFromDependency(sensor_t_data.handle, &data[id]); + } + } + + for (i = 0; i < (int)sensors_to_trigger_num; i++) { + if (sensors_to_trigger[i]->GetStatus()) { + dep_mutex = sensors_to_trigger[i]->GetMutexForTrigger(); + pthread_mutex_lock(&dep_mutex->trigger_mutex); + pthread_cond_signal(&dep_mutex->trigger_data_cond); + pthread_mutex_unlock(&dep_mutex->trigger_mutex); + } + } +} + +void SWAccelMagnFusion6X::TriggerEventReceived() +{ + int deltatime; + int64_t time_diff = 0; + SensorBaseData accel_data, magn_data; + int err, data_remaining_magn, nomaxdata = 10; + + do { + data_remaining_magn = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_1, &magn_data); + if (data_remaining_magn < 0) + return; + + do { + err = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &accel_data); + if (err < 0) { + nomaxdata--; + usleep(200); + continue; + } + + time_diff = magn_data.timestamp - accel_data.timestamp; + + } while ((time_diff >= GetRealPollrate()) && (nomaxdata > 0)); + + if ((err >= 0) && (sensor_event.timestamp > 0)) { + deltatime = (int)NS_TO_MS((uint64_t)(magn_data.timestamp - sensor_event.timestamp)); + iNemoEngine_GeoMag_API_Run(accel_data.raw, magn_data.processed, deltatime); + } + + sensor_event.timestamp = magn_data.timestamp; + + err = iNemoEngine_GeoMag_API_Get_Quaternion(outdata[ST_GEOMAG_ROTATION_VECTOR_OUT_ID].processed); + if (err < 0) + return; + + err = iNemoEngine_GeoMag_API_Get_Hpr(outdata[ST_GEOMAG_ORIENTATION_OUT_ID].processed); + if (err < 0) + return; + + err = iNemoEngine_GeoMag_API_Get_LinAcc(outdata[ST_GEOMAG_LINEAR_ACCEL_OUT_ID].processed); + if (err < 0) + return; + + err = iNemoEngine_GeoMag_API_Get_Gravity(outdata[ST_GEOMAG_GRAVITY_OUT_ID].processed); + if (err < 0) + return; + + outdata[ST_GEOMAG_ROTATION_VECTOR_OUT_ID].timestamp = sensor_event.timestamp; + outdata[ST_GEOMAG_ORIENTATION_OUT_ID].timestamp = sensor_event.timestamp; + outdata[ST_GEOMAG_LINEAR_ACCEL_OUT_ID].timestamp = sensor_event.timestamp; + outdata[ST_GEOMAG_GRAVITY_OUT_ID].timestamp = sensor_event.timestamp; + + SplitAndProcessData(outdata); + } while (data_remaining_magn > 0); +} diff --git a/libsensors_iio/src/SWAccelMagnFusion6X.h b/libsensors_iio/src/SWAccelMagnFusion6X.h new file mode 100644 index 0000000..2cd27ce --- /dev/null +++ b/libsensors_iio/src/SWAccelMagnFusion6X.h @@ -0,0 +1,43 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_SW_ACCEL_MAGN_6X_FUSION_H +#define ST_SW_ACCEL_MAGN_6X_FUSION_H + +#include "SWSensorBase.h" + +#define ST_GEOMAG_ROTATION_VECTOR_OUT_ID (0) +#define ST_GEOMAG_ORIENTATION_OUT_ID (1) +#define ST_GEOMAG_LINEAR_ACCEL_OUT_ID (2) +#define ST_GEOMAG_GRAVITY_OUT_ID (3) +#define ST_GEOMAG_MAX_OUT_ID (4) + +class SWAccelMagnFusion6X : public SWSensorBaseWithPollrate { +protected: + SensorBaseData outdata[ST_GEOMAG_MAX_OUT_ID]; + +public: + SWAccelMagnFusion6X(const char *name, int handle, int pipe_data_fd); + ~SWAccelMagnFusion6X(); + + virtual int Enable(int handle, bool enable); + virtual int SetDelay(int handle, int64_t period_ns, int64_t timeout); + virtual void SplitAndProcessData(SensorBaseData data[ST_GEOMAG_MAX_OUT_ID]); + virtual void TriggerEventReceived(); +}; + +#endif /* ST_SW_ACCEL_MAGN_6X_FUSION_H */ diff --git a/libsensors_iio/src/SWAccelMagnGyroFusion9X.cpp b/libsensors_iio/src/SWAccelMagnGyroFusion9X.cpp new file mode 100644 index 0000000..a772f39 --- /dev/null +++ b/libsensors_iio/src/SWAccelMagnGyroFusion9X.cpp @@ -0,0 +1,178 @@ +/* + * STMicroelectronics Accel-Magn-Gyro Fusion 9X Sensor Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "SWAccelMagnGyroFusion9X.h" + +extern "C" { + #include "iNemoEngineAPI.h" +} + +SWAccelMagnGyroFusion9X::SWAccelMagnGyroFusion9X(const char *name, int handle, int pipe_data_fd) : + SWSensorBaseWithPollrate(name, handle, SENSOR_TYPE_ST_ACCEL_MAGN_GYRO_FUSION9X, + pipe_data_fd, false, false, true, false) +{ + sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE; + + sensor_t_data.resolution = ST_SENSOR_FUSION_RESOLUTION(1.0f); + sensor_t_data.maxRange = 1.0f; + + type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_ACCELEROMETER; + type_dependencies[SENSOR_BASE_DEPENDENCY_1] = SENSOR_TYPE_GEOMAGNETIC_FIELD; + type_dependencies[SENSOR_BASE_DEPENDENCY_2] = SENSOR_TYPE_GYROSCOPE; + type_sensor_need_trigger = SENSOR_TYPE_GYROSCOPE; + + iNemoEngine_API_Initialization_9X(NULL); +} + +SWAccelMagnGyroFusion9X::~SWAccelMagnGyroFusion9X() +{ + +} + +int SWAccelMagnGyroFusion9X::Enable(int handle, bool enable) +{ + int err; + bool old_status; + + old_status = GetStatus(); + + err = SWSensorBaseWithPollrate::Enable(handle, enable); + if (err < 0) + return err; + + if ((GetStatus() && !old_status) || (!GetStatus() && old_status)) { + sensor_event.timestamp = 0; + iNemoEngine_API_enable_9X(enable); + } + + return 0; +} + +int SWAccelMagnGyroFusion9X::SetDelay(int handle, int64_t period_ns, int64_t timeout) +{ + int err; + + if ((period_ns > FREQUENCY_TO_NS(CONFIG_ST_HAL_MIN_FUSION_POLLRATE) && period_ns != INT64_MAX)) + period_ns = FREQUENCY_TO_NS(CONFIG_ST_HAL_MIN_FUSION_POLLRATE); + + err = SWSensorBaseWithPollrate::SetDelay(handle, period_ns, timeout); + if (err < 0) + return err; + + real_pollrate = dependencies[SENSOR_BASE_DEPENDENCY_2]->GetRealPollrate(); + + return 0; +} + +void SWAccelMagnGyroFusion9X::SplitAndProcessData(SensorBaseData data[ST_ACCEL_MAGN_GYRO_MAX_OUT_ID]) +{ + int i, id, sensor_type; + trigger_mutex *dep_mutex; + + for (i = 0; i < (int)sensors_to_push_data_num; i++) { + if (sensors_to_push_data[i]->GetStatus()) { + switch (sensors_to_push_data_type[i]) { + case SENSOR_TYPE_ROTATION_VECTOR: + id = ST_ACCEL_MAGN_GYRO_ROTATION_VECTOR_OUT_ID; + break; + case SENSOR_TYPE_ORIENTATION: + id = ST_ACCEL_MAGN_GYRO_ORIENTATION_OUT_ID; + break; + case SENSOR_TYPE_GRAVITY: + id = ST_ACCEL_MAGN_GYRO_GRAVITY_OUT_ID; + break; + case SENSOR_TYPE_LINEAR_ACCELERATION: + id = ST_ACCEL_MAGN_GYRO_LINEAR_ACCEL__OUT_ID; + break; + default: + continue; + } + + sensors_to_push_data[i]->ReceiveDataFromDependency(sensor_t_data.handle, &data[id]); + } + } + + for (i = 0; i < (int)sensors_to_trigger_num; i++) { + if (sensors_to_trigger[i]->GetStatus()) { + dep_mutex = sensors_to_trigger[i]->GetMutexForTrigger(); + pthread_mutex_lock(&dep_mutex->trigger_mutex); + pthread_cond_signal(&dep_mutex->trigger_data_cond); + pthread_mutex_unlock(&dep_mutex->trigger_mutex); + } + } +} + +void SWAccelMagnGyroFusion9X::TriggerEventReceived() +{ + int64_t time_diff = 0; + SensorBaseData accel_data, magn_data, gyro_data; + int err, err2, data_remaining_gyro, nomaxdata_accel = 10, nomaxdata_magn = 10; + + do { + data_remaining_gyro = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_2, &gyro_data); + if (data_remaining_gyro < 0) + return; + + do { + err = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &accel_data); + if (err < 0) { + nomaxdata_accel--; + usleep(200); + continue; + } + + time_diff = gyro_data.timestamp - accel_data.timestamp; + + } while ((time_diff >= GetRealPollrate()) && (nomaxdata_accel > 0)); + + do { + err2 = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_1, &magn_data); + if (err2 < 0) { + nomaxdata_magn--; + usleep(200); + continue; + } + + time_diff = gyro_data.timestamp - magn_data.timestamp; + + } while ((time_diff >= GetRealPollrate()) && (nomaxdata_magn > 0)); + + if ((err >= 0) && (err2 >= 0)) + iNemoEngine_API_Run_9X(accel_data.raw, magn_data.processed, gyro_data.processed, gyro_data.timestamp); + + sensor_event.timestamp = gyro_data.timestamp; + + err = iNemoEngine_API_Get_Quaternion_9X(outdata[ST_ACCEL_MAGN_GYRO_ROTATION_VECTOR_OUT_ID].processed); + if (err < 0) + return; + + err = iNemoEngine_API_Get_Euler_Angles_9X(outdata[ST_ACCEL_MAGN_GYRO_ORIENTATION_OUT_ID].processed); + if (err < 0) + return; + + err = iNemoEngine_API_Get_Gravity_9X(outdata[ST_ACCEL_MAGN_GYRO_GRAVITY_OUT_ID].processed); + if (err < 0) + return; + + err = iNemoEngine_API_Get_Linear_Acceleration_9X(outdata[ST_ACCEL_MAGN_GYRO_LINEAR_ACCEL__OUT_ID].processed); + if (err < 0) + return; + + outdata[ST_ACCEL_MAGN_GYRO_ROTATION_VECTOR_OUT_ID].timestamp = sensor_event.timestamp; + outdata[ST_ACCEL_MAGN_GYRO_ORIENTATION_OUT_ID].timestamp = sensor_event.timestamp; + outdata[ST_ACCEL_MAGN_GYRO_GRAVITY_OUT_ID].timestamp = sensor_event.timestamp; + outdata[ST_ACCEL_MAGN_GYRO_LINEAR_ACCEL__OUT_ID].timestamp = sensor_event.timestamp; + + SplitAndProcessData(outdata); + } while (data_remaining_gyro > 0); +} diff --git a/libsensors_iio/src/SWAccelMagnGyroFusion9X.h b/libsensors_iio/src/SWAccelMagnGyroFusion9X.h new file mode 100644 index 0000000..c197190 --- /dev/null +++ b/libsensors_iio/src/SWAccelMagnGyroFusion9X.h @@ -0,0 +1,43 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_SW_ACCEL_MAGN_GYRO_9X_FUSION_H +#define ST_SW_ACCEL_MAGN_GYRO_9X_FUSION_H + +#include "SWSensorBase.h" + +#define ST_ACCEL_MAGN_GYRO_ROTATION_VECTOR_OUT_ID (0) +#define ST_ACCEL_MAGN_GYRO_ORIENTATION_OUT_ID (1) +#define ST_ACCEL_MAGN_GYRO_GRAVITY_OUT_ID (2) +#define ST_ACCEL_MAGN_GYRO_LINEAR_ACCEL__OUT_ID (3) +#define ST_ACCEL_MAGN_GYRO_MAX_OUT_ID (4) + +class SWAccelMagnGyroFusion9X : public SWSensorBaseWithPollrate { +protected: + SensorBaseData outdata[ST_ACCEL_MAGN_GYRO_MAX_OUT_ID]; + +public: + SWAccelMagnGyroFusion9X(const char *name, int handle, int pipe_data_fd); + ~SWAccelMagnGyroFusion9X(); + + virtual int Enable(int handle, bool enable); + virtual int SetDelay(int handle, int64_t period_ns, int64_t timeout); + virtual void SplitAndProcessData(SensorBaseData data[ST_ACCEL_MAGN_GYRO_MAX_OUT_ID]); + virtual void TriggerEventReceived(); +}; + +#endif /* ST_SW_ACCEL_MAGN_GYRO_9X_FUSION_H */ diff --git a/libsensors_iio/src/SWGameRotationVector.cpp b/libsensors_iio/src/SWGameRotationVector.cpp new file mode 100644 index 0000000..7fe993d --- /dev/null +++ b/libsensors_iio/src/SWGameRotationVector.cpp @@ -0,0 +1,50 @@ +/* + * STMicroelectronics SW Game Rotation Vector Sensor Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "SWGameRotationVector.h" + +SWGameRotationVector::SWGameRotationVector(const char *name, int handle, int pipe_data_fd) : + SWSensorBaseWithPollrate(name, handle, SENSOR_TYPE_GAME_ROTATION_VECTOR, + pipe_data_fd, true, true, true, false) +{ + sensor_t_data.stringType = SENSOR_STRING_TYPE_GAME_ROTATION_VECTOR; + sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE; + + type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_ST_ACCEL_GYRO_FUSION6X; + type_sensor_need_trigger = SENSOR_TYPE_ST_ACCEL_GYRO_FUSION6X; + + num_data_axis = SENSOR_BASE_4AXIS; +} + +SWGameRotationVector::~SWGameRotationVector() +{ + +} + +void SWGameRotationVector::TriggerEventReceived() +{ + int data_remaining; + SensorBaseData game_quaternion; + + do { + data_remaining = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &game_quaternion); + if (data_remaining < 0) + return; + + memcpy(sensor_event.data, game_quaternion.processed, num_data_axis * sizeof(float)); + sensor_event.timestamp = game_quaternion.timestamp; + + SWSensorBaseWithPollrate::WriteDataToPipe(); + SWSensorBaseWithPollrate::ProcessData(&game_quaternion); + } while (data_remaining > 0); +} diff --git a/libsensors_iio/src/SWGameRotationVector.h b/libsensors_iio/src/SWGameRotationVector.h new file mode 100644 index 0000000..4b7e30a --- /dev/null +++ b/libsensors_iio/src/SWGameRotationVector.h @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_SWGAME_ROTATION_VECTOR_H +#define ST_SWGAME_ROTATION_VECTOR_H + +#include "SWSensorBase.h" + +class SWGameRotationVector : public SWSensorBaseWithPollrate { +public: + SWGameRotationVector(const char *name, int handle, int pipe_data_fd); + ~SWGameRotationVector(); + + virtual void TriggerEventReceived(); +}; + +#endif /* ST_SWGAME_ROTATION_VECTOR_H */ diff --git a/libsensors_iio/src/SWGeoMagRotationVector.cpp b/libsensors_iio/src/SWGeoMagRotationVector.cpp new file mode 100644 index 0000000..f0f455e --- /dev/null +++ b/libsensors_iio/src/SWGeoMagRotationVector.cpp @@ -0,0 +1,50 @@ +/* + * STMicroelectronics SW Geomagnetic Rotation Vector Sensor Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "SWGeoMagRotationVector.h" + +SWGeoMagRotationVector::SWGeoMagRotationVector(const char *name, int handle, int pipe_data_fd) : + SWSensorBaseWithPollrate(name, handle, SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR, + pipe_data_fd, true, true, true, false) +{ + sensor_t_data.stringType = SENSOR_STRING_TYPE_GEOMAGNETIC_ROTATION_VECTOR; + sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE; + + type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_ST_ACCEL_MAGN_FUSION6X; + type_sensor_need_trigger = SENSOR_TYPE_ST_ACCEL_MAGN_FUSION6X; + + num_data_axis = SENSOR_BASE_4AXIS; +} + +SWGeoMagRotationVector::~SWGeoMagRotationVector() +{ + +} + +void SWGeoMagRotationVector::TriggerEventReceived() +{ + int data_remaining; + SensorBaseData geomag_quaternion; + + do { + data_remaining = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &geomag_quaternion); + if (data_remaining < 0) + return; + + memcpy(sensor_event.data, geomag_quaternion.processed, num_data_axis * sizeof(float)); + sensor_event.timestamp = geomag_quaternion.timestamp; + + SWSensorBaseWithPollrate::WriteDataToPipe(); + SWSensorBaseWithPollrate::ProcessData(&geomag_quaternion); + } while (data_remaining > 0); +} diff --git a/libsensors_iio/src/SWGeoMagRotationVector.h b/libsensors_iio/src/SWGeoMagRotationVector.h new file mode 100644 index 0000000..e3bc0c4 --- /dev/null +++ b/libsensors_iio/src/SWGeoMagRotationVector.h @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_SWGEOMAG_ROTATION_VECTOR_H +#define ST_SWGEOMAG_ROTATION_VECTOR_H + +#include "SWSensorBase.h" + +class SWGeoMagRotationVector : public SWSensorBaseWithPollrate { +public: + SWGeoMagRotationVector(const char *name, int handle, int pipe_data_fd); + ~SWGeoMagRotationVector(); + + virtual void TriggerEventReceived(); +}; + +#endif /* ST_SWGEOMAG_ROTATION_VECTOR_H */ diff --git a/libsensors_iio/src/SWGravity.cpp b/libsensors_iio/src/SWGravity.cpp new file mode 100644 index 0000000..4bb8d35 --- /dev/null +++ b/libsensors_iio/src/SWGravity.cpp @@ -0,0 +1,50 @@ +/* + * STMicroelectronics SW Gravity Sensor Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "SWGravity.h" + +SWGravity::SWGravity(const char *name, int handle, int pipe_data_fd) : + SWSensorBaseWithPollrate(name, handle, SENSOR_TYPE_GRAVITY, + pipe_data_fd, true, false, true, false) +{ + sensor_t_data.stringType = SENSOR_STRING_TYPE_GRAVITY; + sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE; + + sensor_t_data.maxRange = CONFIG_ST_HAL_ACCEL_RANGE; + + type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_ST_ACCEL_MAGN_GYRO_FUSION9X; + type_sensor_need_trigger = SENSOR_TYPE_ST_ACCEL_MAGN_GYRO_FUSION9X; +} + +SWGravity::~SWGravity() +{ + +} + +void SWGravity::TriggerEventReceived() +{ + int data_remaining; + SensorBaseData gravity; + + do { + data_remaining = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &gravity); + if (data_remaining < 0) + return; + + memcpy(sensor_event.data, gravity.processed, num_data_axis * sizeof(float)); + sensor_event.timestamp = gravity.timestamp; + + SWSensorBaseWithPollrate::WriteDataToPipe(); + SWSensorBaseWithPollrate::ProcessData(&gravity); + } while (data_remaining > 0); +} diff --git a/libsensors_iio/src/SWGravity.h b/libsensors_iio/src/SWGravity.h new file mode 100644 index 0000000..7b02f15 --- /dev/null +++ b/libsensors_iio/src/SWGravity.h @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_SWGRAVITY_H +#define ST_SWGRAVITY_H + +#include "SWSensorBase.h" + +class SWGravity : public SWSensorBaseWithPollrate { +public: + SWGravity(const char *name, int handle, int pipe_data_fd); + ~SWGravity(); + + virtual void TriggerEventReceived(); +}; + +#endif /* ST_SWGRAVITY_H */ diff --git a/libsensors_iio/src/SWGyroscopeUncalibrated.cpp b/libsensors_iio/src/SWGyroscopeUncalibrated.cpp new file mode 100644 index 0000000..55e8bfe --- /dev/null +++ b/libsensors_iio/src/SWGyroscopeUncalibrated.cpp @@ -0,0 +1,49 @@ +/* + * STMicroelectronics Gyroscope Uncalibrated Sensor Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "SWGyroscopeUncalibrated.h" + +SWGyroscopeUncalibrated::SWGyroscopeUncalibrated(const char *name, int handle, int pipe_data_fd) : + SWSensorBaseWithPollrate(name, handle, SENSOR_TYPE_GYROSCOPE_UNCALIBRATED, + pipe_data_fd, true, true, true, true) +{ + sensor_t_data.stringType = SENSOR_STRING_TYPE_GYROSCOPE_UNCALIBRATED; + sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE; + + type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_GYROSCOPE; + type_sensor_need_trigger = SENSOR_TYPE_GYROSCOPE; +} + +SWGyroscopeUncalibrated::~SWGyroscopeUncalibrated() +{ + +} + +void SWGyroscopeUncalibrated::TriggerEventReceived() +{ + int data_remaining; + SensorBaseData gyro_data; + + do { + data_remaining = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &gyro_data); + if (data_remaining < 0) + return; + + memcpy(sensor_event.uncalibrated_gyro.uncalib, gyro_data.raw, num_data_axis * sizeof(float)); + memcpy(sensor_event.uncalibrated_gyro.bias, gyro_data.offset, num_data_axis * sizeof(float)); + sensor_event.timestamp = gyro_data.timestamp; + + SWSensorBaseWithPollrate::WriteDataToPipe(); + SWSensorBaseWithPollrate::ProcessData(&gyro_data); + } while (data_remaining > 0); +} diff --git a/libsensors_iio/src/SWGyroscopeUncalibrated.h b/libsensors_iio/src/SWGyroscopeUncalibrated.h new file mode 100644 index 0000000..3f87bdd --- /dev/null +++ b/libsensors_iio/src/SWGyroscopeUncalibrated.h @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_SWGYROSCOPE_UNCALIBRATED_H +#define ST_SWGYROSCOPE_UNCALIBRATED_H + +#include "SWSensorBase.h" + +class SWGyroscopeUncalibrated : public SWSensorBaseWithPollrate { +public: + SWGyroscopeUncalibrated(const char *name, int handle, int pipe_data_fd); + ~SWGyroscopeUncalibrated(); + + virtual void TriggerEventReceived(); +}; + +#endif /* ST_SWGYROSCOPE_UNCALIBRATED_H */ diff --git a/libsensors_iio/src/SWLinearAccel.cpp b/libsensors_iio/src/SWLinearAccel.cpp new file mode 100644 index 0000000..111d822 --- /dev/null +++ b/libsensors_iio/src/SWLinearAccel.cpp @@ -0,0 +1,50 @@ +/* + * STMicroelectronics SW Linear Acceleration Sensor Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "SWLinearAccel.h" + +SWLinearAccel::SWLinearAccel(const char *name, int handle, int pipe_data_fd) : + SWSensorBaseWithPollrate(name, handle, SENSOR_TYPE_LINEAR_ACCELERATION, + pipe_data_fd, true, false, true, false) +{ + sensor_t_data.stringType = SENSOR_STRING_TYPE_LINEAR_ACCELERATION; + sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE; + + sensor_t_data.maxRange = CONFIG_ST_HAL_ACCEL_RANGE; + + type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_ST_ACCEL_MAGN_GYRO_FUSION9X; + type_sensor_need_trigger = SENSOR_TYPE_ST_ACCEL_MAGN_GYRO_FUSION9X; +} + +SWLinearAccel::~SWLinearAccel() +{ + +} + +void SWLinearAccel::TriggerEventReceived() +{ + int data_remaining; + SensorBaseData linear_accel; + + do { + data_remaining = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &linear_accel); + if (data_remaining < 0) + return; + + memcpy(sensor_event.data, linear_accel.processed, num_data_axis * sizeof(float)); + sensor_event.timestamp = linear_accel.timestamp; + + SWSensorBaseWithPollrate::WriteDataToPipe(); + SWSensorBaseWithPollrate::ProcessData(&linear_accel); + } while (data_remaining > 0); +} diff --git a/libsensors_iio/src/SWLinearAccel.h b/libsensors_iio/src/SWLinearAccel.h new file mode 100644 index 0000000..baf7507 --- /dev/null +++ b/libsensors_iio/src/SWLinearAccel.h @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_SWLINEAR_ACCEL_H +#define ST_SWLINEAR_ACCEL_H + +#include "SWSensorBase.h" + +class SWLinearAccel : public SWSensorBaseWithPollrate { +public: + SWLinearAccel(const char *name, int handle, int pipe_data_fd); + ~SWLinearAccel(); + + virtual void TriggerEventReceived(); +}; + +#endif /* ST_SWLINEAR_ACCEL_H */ diff --git a/libsensors_iio/src/SWMagnetometerUncalibrated.cpp b/libsensors_iio/src/SWMagnetometerUncalibrated.cpp new file mode 100644 index 0000000..ab4baaf --- /dev/null +++ b/libsensors_iio/src/SWMagnetometerUncalibrated.cpp @@ -0,0 +1,49 @@ +/* + * STMicroelectronics Magnetometer Uncalibrated Sensor Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "SWMagnetometerUncalibrated.h" + +SWMagnetometerUncalibrated::SWMagnetometerUncalibrated(const char *name, int handle, int pipe_data_fd) : + SWSensorBaseWithPollrate(name, handle, SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED, + pipe_data_fd, true, true, true, true) +{ + sensor_t_data.stringType = SENSOR_STRING_TYPE_MAGNETIC_FIELD_UNCALIBRATED; + sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE; + + type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_MAGNETIC_FIELD; + type_sensor_need_trigger = SENSOR_TYPE_MAGNETIC_FIELD; +} + +SWMagnetometerUncalibrated::~SWMagnetometerUncalibrated() +{ + +} + +void SWMagnetometerUncalibrated::TriggerEventReceived() +{ + int data_remaining; + SensorBaseData magn_data; + + do { + data_remaining = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &magn_data); + if (data_remaining < 0) + return; + + memcpy(sensor_event.uncalibrated_magnetic.uncalib, magn_data.raw, num_data_axis * sizeof(float)); + memcpy(sensor_event.uncalibrated_magnetic.bias, magn_data.offset, num_data_axis * sizeof(float)); + sensor_event.timestamp = magn_data.timestamp; + + SWSensorBaseWithPollrate::WriteDataToPipe(); + SWSensorBaseWithPollrate::ProcessData(&magn_data); + } while (data_remaining > 0); +} diff --git a/libsensors_iio/src/SWMagnetometerUncalibrated.h b/libsensors_iio/src/SWMagnetometerUncalibrated.h new file mode 100644 index 0000000..dbcd7f4 --- /dev/null +++ b/libsensors_iio/src/SWMagnetometerUncalibrated.h @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_SWMAGNETOMETER_UNCALIBRATED_H +#define ST_SWMAGNETOMETER_UNCALIBRATED_H + +#include "SWSensorBase.h" + +class SWMagnetometerUncalibrated : public SWSensorBaseWithPollrate { +public: + SWMagnetometerUncalibrated(const char *name, int handle, int pipe_data_fd); + ~SWMagnetometerUncalibrated(); + + virtual void TriggerEventReceived(); +}; + +#endif /* ST_SWMAGNETOMETER_UNCALIBRATED_H */ diff --git a/libsensors_iio/src/SWOrientation.cpp b/libsensors_iio/src/SWOrientation.cpp new file mode 100644 index 0000000..2b7a807 --- /dev/null +++ b/libsensors_iio/src/SWOrientation.cpp @@ -0,0 +1,50 @@ +/* + * STMicroelectronics SW Orientation Sensor Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "SWOrientation.h" + +SWOrientation::SWOrientation(const char *name, int handle, int pipe_data_fd) : + SWSensorBaseWithPollrate(name, handle, SENSOR_TYPE_ORIENTATION, + pipe_data_fd, true, false, true, false) +{ + sensor_t_data.stringType = SENSOR_STRING_TYPE_ORIENTATION; + sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE; + + sensor_t_data.maxRange = 360.0f; + + type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_ST_ACCEL_MAGN_GYRO_FUSION9X; + type_sensor_need_trigger = SENSOR_TYPE_ST_ACCEL_MAGN_GYRO_FUSION9X; +} + +SWOrientation::~SWOrientation() +{ + +} + +void SWOrientation::TriggerEventReceived() +{ + int data_remaining; + SensorBaseData orientation; + + do { + data_remaining = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &orientation); + if (data_remaining < 0) + return; + + memcpy(sensor_event.data, orientation.processed, num_data_axis * sizeof(float)); + sensor_event.timestamp = orientation.timestamp; + + SWSensorBaseWithPollrate::WriteDataToPipe(); + SWSensorBaseWithPollrate::ProcessData(&orientation); + } while (data_remaining > 0); +} diff --git a/libsensors_iio/src/SWOrientation.h b/libsensors_iio/src/SWOrientation.h new file mode 100644 index 0000000..391c07a --- /dev/null +++ b/libsensors_iio/src/SWOrientation.h @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_SWORIENTATION_H +#define ST_SWORIENTATION_H + +#include "SWSensorBase.h" + +class SWOrientation : public SWSensorBaseWithPollrate { +public: + SWOrientation(const char *name, int handle, int pipe_data_fd); + ~SWOrientation(); + + virtual void TriggerEventReceived(); +}; + +#endif /* ST_SWORIENTATION_H */ diff --git a/libsensors_iio/src/SWRotationVector.cpp b/libsensors_iio/src/SWRotationVector.cpp new file mode 100644 index 0000000..7d57a92 --- /dev/null +++ b/libsensors_iio/src/SWRotationVector.cpp @@ -0,0 +1,50 @@ +/* + * STMicroelectronics SW Rotation Vector Sensor Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "SWRotationVector.h" + +SWRotationVector::SWRotationVector(const char *name, int handle, int pipe_data_fd) : + SWSensorBaseWithPollrate(name, handle, SENSOR_TYPE_ROTATION_VECTOR, + pipe_data_fd, true, true, true, false) +{ + sensor_t_data.stringType = SENSOR_STRING_TYPE_ROTATION_VECTOR; + sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE; + + type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_ST_ACCEL_MAGN_GYRO_FUSION9X; + type_sensor_need_trigger = SENSOR_TYPE_ST_ACCEL_MAGN_GYRO_FUSION9X; + + num_data_axis = SENSOR_BASE_4AXIS; +} + +SWRotationVector::~SWRotationVector() +{ + +} + +void SWRotationVector::TriggerEventReceived() +{ + int data_remaining; + SensorBaseData quaternion; + + do { + data_remaining = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &quaternion); + if (data_remaining < 0) + return; + + memcpy(sensor_event.data, quaternion.processed, num_data_axis * sizeof(float)); + sensor_event.timestamp = quaternion.timestamp; + + SWSensorBaseWithPollrate::WriteDataToPipe(); + SWSensorBaseWithPollrate::ProcessData(&quaternion); + } while (data_remaining > 0); +} diff --git a/libsensors_iio/src/SWRotationVector.h b/libsensors_iio/src/SWRotationVector.h new file mode 100644 index 0000000..4b82907 --- /dev/null +++ b/libsensors_iio/src/SWRotationVector.h @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_SWROTATION_VECTOR_H +#define ST_SWROTATION_VECTOR_H + +#include "SWSensorBase.h" + +class SWRotationVector : public SWSensorBaseWithPollrate { +public: + SWRotationVector(const char *name, int handle, int pipe_data_fd); + ~SWRotationVector(); + + virtual void TriggerEventReceived(); +}; + +#endif /* ST_SWROTATION_VECTOR_H */ diff --git a/libsensors_iio/src/SWSensorBase.cpp b/libsensors_iio/src/SWSensorBase.cpp new file mode 100644 index 0000000..b08b225 --- /dev/null +++ b/libsensors_iio/src/SWSensorBase.cpp @@ -0,0 +1,148 @@ +/* + * STMicroelectronics SW Sensor Base Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "SWSensorBase.h" + +SWSensorBase::SWSensorBase(const char *name, int handle, int sensor_type, int pipe_data_fd, + bool use_dependency_resolution, bool use_dependency_range, bool use_dependency_delay, + bool use_dependency_name) : SensorBase(name, handle, sensor_type, pipe_data_fd) +{ + dependency_resolution = use_dependency_resolution; + dependency_range = use_dependency_range; + dependency_delay = use_dependency_delay; + dependency_name = use_dependency_name; + + return; +} + +SWSensorBase::~SWSensorBase() +{ + return; +} + +int SWSensorBase::AddSensorDependency(SensorBase *p) +{ + struct sensor_t dependecy_data; + + if (sensor_t_data.fifoMaxEventCount == 0) + sensor_t_data.fifoMaxEventCount = p->GetMaxFifoLenght(); + else { + if (p->GetMaxFifoLenght() < (int)sensor_t_data.fifoMaxEventCount) + sensor_t_data.fifoMaxEventCount = p->GetMaxFifoLenght(); + } + + p->FillSensor_tData(&dependecy_data); + + if (dependency_resolution) + sensor_t_data.resolution = dependecy_data.resolution; + + if (dependency_range) + sensor_t_data.maxRange = dependecy_data.maxRange; + + if (dependency_delay) { + if (sensor_t_data.minDelay == 0) + sensor_t_data.minDelay = dependecy_data.minDelay; + else { + if (dependecy_data.minDelay > sensor_t_data.minDelay) + sensor_t_data.minDelay = dependecy_data.minDelay; + } + + if (sensor_t_data.maxDelay == 0) + sensor_t_data.maxDelay = dependecy_data.maxDelay; + else { + if (dependecy_data.maxDelay < sensor_t_data.maxDelay) + sensor_t_data.maxDelay = dependecy_data.maxDelay; + } + } + + if (dependency_name) { + memcpy((char *)sensor_t_data.name, dependecy_data.name, strlen(dependecy_data.name) + 1); + } + + return SensorBase::AddSensorDependency(p); +} + +int SWSensorBase::FlushData() +{ + int err, i; + + if (GetStatus() && (GetMinTimeout() > 0)) { + for (i = 0; i < (int)dependencies_num; i++) { + err = dependencies[i]->FlushData(); + if (err < 0) + return -EINVAL; + } + } else + return -EINVAL; + + return SensorBase::FlushData(); +} + +void SWSensorBase::ThreadTask() +{ + while (true) { + pthread_mutex_lock(&mutext.trigger_mutex); + pthread_cond_wait(&mutext.trigger_data_cond, &mutext.trigger_mutex); + TriggerEventReceived(); + pthread_mutex_unlock(&mutext.trigger_mutex); + } +} + + +SWSensorBaseWithPollrate::SWSensorBaseWithPollrate(const char *name, int handle, int sensor_type, int pipe_data_fd, + bool use_dependency_resolution, bool use_dependency_range, bool use_dependency_delay, + bool use_dependency_name) : SWSensorBase(name, handle, sensor_type, pipe_data_fd, + use_dependency_resolution, use_dependency_range, use_dependency_delay, use_dependency_name) +{ + +} + +SWSensorBaseWithPollrate::~SWSensorBaseWithPollrate() +{ + +} + +int SWSensorBaseWithPollrate::SetDelay(int handle, int64_t period_ns, int64_t timeout) +{ + int i, err; + int64_t temp_real_pollrate = 0; + + err = SWSensorBase::SetDelay(handle, period_ns, timeout); + if (err < 0) + return err; + + for (i = 0; i < (int)dependencies_num; i++) { + if (temp_real_pollrate < GetMinPeriod()) + temp_real_pollrate = GetMinPeriod(); + } + + return 0; +} + +void SWSensorBaseWithPollrate::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; + } +} diff --git a/libsensors_iio/src/SWSensorBase.h b/libsensors_iio/src/SWSensorBase.h new file mode 100644 index 0000000..06330d0 --- /dev/null +++ b/libsensors_iio/src/SWSensorBase.h @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_SWSENSOR_BASE_H +#define ST_SWSENSOR_BASE_H + + +#include "SensorBase.h" + +#define ST_SENSOR_FUSION_RESOLUTION(maxRange) (maxRange / (1 << 24)) + +class SWSensorBase; + +/* + * class SWSensorBase + */ +class SWSensorBase : public SensorBase { +protected: + bool dependency_resolution; + bool dependency_range; + bool dependency_delay; + bool dependency_name; + +public: + SWSensorBase(const char *name, int handle, int sensor_type, int pipe_data_fd, + bool use_dependency_resolution, bool use_dependency_range, + bool use_dependency_delay, bool use_dependency_name); + virtual ~SWSensorBase(); + + int AddSensorDependency(SensorBase *p); + virtual int FlushData(); + virtual void ThreadTask(); +}; + + +/* + * class SWSensorBaseWithPollrate + */ +class SWSensorBaseWithPollrate : public SWSensorBase { +public: + SWSensorBaseWithPollrate(const char *name, int handle, int sensor_type, int pipe_data_fd, + bool use_dependency_resolution, bool use_dependency_range, + bool use_dependency_delay, bool use_dependency_name); + virtual ~SWSensorBaseWithPollrate(); + + virtual int SetDelay(int handle, int64_t period_ns, int64_t timeout); + virtual void WriteDataToPipe(); +}; + +#endif /* ST_SWSENSOR_BASE_H */ diff --git a/libsensors_iio/src/SensorBase.cpp b/libsensors_iio/src/SensorBase.cpp new file mode 100644 index 0000000..827077d --- /dev/null +++ b/libsensors_iio/src/SensorBase.cpp @@ -0,0 +1,483 @@ +/* + * STMicroelectronics Sensor Base Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include +#include + +#include "SensorBase.h" + +#define ST_SENSOR_BASE_WAIT_US_BEFORE_SEND_FLUSH (200000) + +SensorBase::SensorBase(const char *name, int handle, int type, int pipe_data_fd) +{ + int i; + + if (strlen(name) + 1 > SENSOR_BASE_ANDROID_NAME_MAX) { + memcpy(android_name, name, SENSOR_BASE_ANDROID_NAME_MAX - 1); + android_name[SENSOR_BASE_ANDROID_NAME_MAX - 1] = '\0'; + } else + memcpy(android_name, name, strlen(name) + 1); + + memset(&sensor_t_data, 0, sizeof(sensor_t)); + memset(&sensor_event, 0, sizeof(sensors_event_t)); + memset(type_dependencies, 0, SENSOR_BASE_MAX_DEPENDENCY * sizeof(int)); + memset(sensors_pollrates, 0, ST_HAL_IIO_MAX_DEVICES * sizeof(int64_t)); + memset(last_timestap_pushed, 0, ST_HAL_IIO_MAX_DEVICES * sizeof(int64_t)); + + for (i = 0; i < ST_HAL_IIO_MAX_DEVICES; i++) + sensors_timeout[i] = INT64_MAX; + + sensor_event.version = sizeof(sensors_event_t); + sensor_event.sensor = handle; + sensor_event.type = type; + + sensor_t_data.name = android_name; + sensor_t_data.handle = handle; + sensor_t_data.type = type; + sensor_t_data.vendor = "STMicroelectronics"; + sensor_t_data.version = 1; + + real_pollrate = 0; + dependencies_num = 0; + last_data_timestamp = 0; + enabled_sensors_mask = 0; + sensors_to_trigger_num = 0; + type_sensor_need_trigger = -ENODEV; + sensors_to_push_data_num = 0; + num_data_axis = SENSOR_BASE_3AXIS; + + android_pipe_fd = pipe_data_fd; + + pthread_mutex_init(&mutext.trigger_mutex, NULL); + pthread_cond_init(&mutext.trigger_data_cond, NULL); + + valid_class = true; +} + +SensorBase::~SensorBase() +{ + int i; + + for (i = 0; i < (int)dependencies_num; i++) + DeAllocateBufferForDependencyData(i); +} + +bool SensorBase::IsValidClass() +{ + return valid_class; +} + +int SensorBase::GetHandle() +{ + return sensor_t_data.handle; +} + +int SensorBase::GetType() +{ + return sensor_t_data.type; +} + +int SensorBase::GetMaxFifoLenght() +{ + return sensor_t_data.fifoMaxEventCount; +} + +void SensorBase::SetBitEnableMask(int handle) +{ + enabled_sensors_mask |= (1ULL << handle); +} + +void SensorBase::ResetBitEnableMask(int handle) +{ + enabled_sensors_mask &= ~(1ULL << handle); +} + +char* SensorBase::GetName() +{ + return (char *)sensor_t_data.name; +} + +int SensorBase::Enable(int handle, bool enable) +{ + int err, i = 0; +#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_INFO) + bool old_status = GetStatus(); +#endif /* CONFIG_ST_HAL_DEBUG_LEVEL */ + + if ((enable && !GetStatus()) || (!enable && !GetStatusExcludeHandle(handle))) { + for (i = 0; i < (int)dependencies_num; i++) { + err = dependencies[i]->Enable(sensor_event.sensor, enable); + if (err < 0) + goto restore_enable_dependencies; + } + } + + if (enable) + SetBitEnableMask(handle); + else { + err = SetDelay(handle, 0, INT64_MAX); + if (err < 0) + goto restore_enable_dependencies; + + ResetBitEnableMask(handle); + } + +#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_INFO) + if (((old_status && !GetStatus()) || (!old_status && GetStatus())) && (sensor_t_data.type < SENSOR_TYPE_ST_CUSTOM_NO_SENSOR)) { + if (GetStatus()) + ALOGI("\"%s\": power-on (sensor type: %d).", sensor_t_data.name, sensor_t_data.type); + else + ALOGI("\"%s\": power-off (sensor type: %d).", sensor_t_data.name, sensor_t_data.type); + } +#endif /* CONFIG_ST_HAL_DEBUG_LEVEL */ + + return 0; + +restore_enable_dependencies: + for (i--; i >= 0; i--) + dependencies[i]->Enable(sensor_event.sensor, !enable); + + return err; +} + +bool SensorBase::GetStatusExcludeHandle(int handle) +{ + return (enabled_sensors_mask & ~(1ULL << handle)) > 0 ? true : false; +} + +bool SensorBase::GetStatusOfHandle(int handle) +{ + return (enabled_sensors_mask & (1ULL << handle)) > 0 ? true : false; +} + +bool SensorBase::GetStatus() +{ + return enabled_sensors_mask > 0 ? true : false; +} + +int SensorBase::SetDelay(int handle, int64_t period_ns, int64_t timeout) +{ + int err, i; + int64_t restore_min_timeout, restore_min_period_ms; + + restore_min_timeout = sensors_timeout[handle]; + restore_min_period_ms = sensors_pollrates[handle]; + + sensors_pollrates[handle] = period_ns; + sensors_timeout[handle] = timeout; + + for (i = 0; i < (int)dependencies_num; i++) { + err = dependencies[i]->SetDelay(sensor_event.sensor, GetMinPeriod(), GetMinTimeout()); + if (err < 0) + goto restore_delay_dependencies; + } + +#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_INFO) + if ((handle == sensor_t_data.handle) && (period_ns > 0)) + ALOGI("\"%s\": changed pollrate to %.2f Hz, timeout %lld ms (sensor type: %d).", + sensor_t_data.name, NS_TO_FREQUENCY((float)(uint64_t)period_ns), + (uint64_t)NS_TO_MS((uint64_t)timeout), sensor_t_data.type); +#endif /* CONFIG_ST_HAL_DEBUG_LEVEL */ + + return 0; + +restore_delay_dependencies: + sensors_pollrates[handle] = restore_min_period_ms; + sensors_timeout[handle] = restore_min_timeout; + + for (i--; i >= 0; i--) + dependencies[i]->SetDelay(sensor_event.sensor, GetMinPeriod(), GetMinTimeout()); + + return err; +} + +int64_t SensorBase::GetDelay() +{ + return sensors_pollrates[sensor_event.sensor]; +} + +int64_t SensorBase::GetRealPollrate() +{ + return real_pollrate; +} + +void SensorBase::GetDepenciesTypeList(int type[SENSOR_BASE_MAX_DEPENDENCY]) +{ + memcpy(type, type_dependencies, SENSOR_BASE_MAX_DEPENDENCY * sizeof(int)); +} + +trigger_mutex* SensorBase::GetMutexForTrigger() +{ + return &mutext; +} + +int SensorBase::GetSensorNeedTriggerType() +{ + return type_sensor_need_trigger; +} + +int SensorBase::AddSensorDependency(SensorBase *p) +{ + int err; + uint32_t sensor_wake_flag; + struct sensor_t dependecy_data; + + if (dependencies_num >= SENSOR_BASE_MAX_DEPENDENCY) { + ALOGE("%s: Failed to add dependency, too many dependencies.", android_name); + return -ENOMEM; + } + + err = AllocateBufferForDependencyData(dependencies_num, p->GetMaxFifoLenght()); + if (err < 0) + return err; + + err = p->AddSensorToDataPush(this); + if (err < 0) { + DeAllocateBufferForDependencyData(dependencies_num); + return err; + } + + p->FillSensor_tData(&dependecy_data); + sensor_t_data.power += dependecy_data.power; + + sensor_wake_flag = (dependecy_data.flags & SENSOR_FLAG_WAKE_UP); + if (dependencies_num == 0) + sensor_t_data.flags |= sensor_wake_flag; + else { + if (!sensor_wake_flag) + sensor_t_data.flags &= ~sensor_wake_flag; + } + + handle_remapping[p->GetHandle()] = dependencies_num; + dependencies[dependencies_num] = p; + dependencies_num++; + + return 0; +} + +void SensorBase::RemoveSensorDependency(SensorBase *p) +{ + int i; + + for (i = 0; i < (int)dependencies_num; i++) { + if (p == dependencies[i]) + break; + } + if (i == (int)dependencies_num) + return; + + DeAllocateBufferForDependencyData(i); + p->RemoveSensorToDataPush(this); + + for (; i < (int)dependencies_num - 1; i++) + dependencies[i] = dependencies[i + 1]; + + dependencies_num--; +} + +int SensorBase::AllocateBufferForDependencyData(int dependency_id, unsigned int max_fifo_len) +{ + circular_buffer_data[dependency_id] = new CircularBuffer(max_fifo_len < 2 ? 2 : max_fifo_len); + if (!circular_buffer_data[dependency_id]) + return -ENOMEM; + + return 0; +} + +void SensorBase::DeAllocateBufferForDependencyData(int dependency_id) +{ + delete circular_buffer_data[dependency_id]; +} + +int SensorBase::AddSensorToDataPush(SensorBase *t) +{ + int err; + + if (sensors_to_push_data_num >= SENSOR_BASE_MAX_DEPENDENCY) { + ALOGE("%s: Failed to add dependency data, too many sensors to push data.", android_name); + return -ENOMEM; + } + + sensors_to_push_data_type[sensors_to_push_data_num] = t->GetType(); + sensors_to_push_data[sensors_to_push_data_num] = t; + sensors_to_push_data_num++; + + return 0; +} + +void SensorBase::RemoveSensorToDataPush(SensorBase *t) +{ + int i; + + for (i = 0; i < (int)sensors_to_push_data_num; i++) { + if (t == sensors_to_push_data[i]) + break; + } + if (i == (int)sensors_to_push_data_num) + return; + + for (; i < (int)sensors_to_push_data_num - 1; i++) + sensors_to_push_data[i] = sensors_to_push_data[i + 1]; + + sensors_to_push_data_num--; +} + +int SensorBase::AddSensorToTrigger(SensorBase *t) +{ + int err; + + if (sensors_to_trigger_num >= SENSOR_BASE_MAX_DEPENDENCY) { + ALOGE("%s: Failed to add dependency, too many sensors to trigger.", android_name); + return -ENOMEM; + } + + sensors_to_trigger[sensors_to_trigger_num] = t; + sensors_to_trigger_num++; + + return 0; +} + +bool SensorBase::FillSensor_tData(struct sensor_t *data) +{ + memcpy(data, &sensor_t_data, sizeof(struct sensor_t)); + + if (sensor_t_data.type >= SENSOR_TYPE_ST_CUSTOM_NO_SENSOR) + return false; + + return true; +} + +int SensorBase::FlushData() +{ + int err; + 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; + + usleep(ST_SENSOR_BASE_WAIT_US_BEFORE_SEND_FLUSH); + + err = write(android_pipe_fd, &flush_event_data, sizeof(sensor_event)); + if (err < 0) { + ALOGE("%s: Failed to write flush event data to pipe.", android_name); + return err; + } + + return 0; +} + +void SensorBase::WriteDataToPipe() +{ + int err; + + if (!GetStatusOfHandle(sensor_t_data.handle)) + return; + + if (sensor_event.timestamp > last_data_timestamp) { + 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; + } +} + +void SensorBase::ProcessData(SensorBaseData *data) +{ + int i; + trigger_mutex *dep_mutex; + + for (i = 0; i < (int)sensors_to_push_data_num; i++) { + if (sensors_to_push_data[i]->GetStatus()) + sensors_to_push_data[i]->ReceiveDataFromDependency(sensor_t_data.handle, data); + } + + for (i = 0; i < (int)sensors_to_trigger_num; i++) { + if (sensors_to_trigger[i]->GetStatus()) { + dep_mutex = sensors_to_trigger[i]->GetMutexForTrigger(); + pthread_mutex_lock(&dep_mutex->trigger_mutex); + pthread_cond_signal(&dep_mutex->trigger_data_cond); + pthread_mutex_unlock(&dep_mutex->trigger_mutex); + } + } +} + +void SensorBase::ProcessEvent(struct iio_event_data __attribute__((unused))*event_data) +{ + return; +} + +void SensorBase::TriggerEventReceived() +{ + return; +} + +void SensorBase::ReceiveDataFromDependency(int handle, SensorBaseData *data) +{ + if (data->timestamp >= last_timestap_pushed[handle]) { + circular_buffer_data[handle_remapping[handle]]->writeElement(data); + last_timestap_pushed[handle] = data->timestamp; + } + + return; +} + +int SensorBase::GetLatestValidDataFromDependency(int dependency_id, SensorBaseData *data) +{ + return circular_buffer_data[dependency_id]->readElement(data); +} + + +int64_t SensorBase::GetMinTimeout() +{ + int i; + int64_t min = INT64_MAX; + + for (i = 0; i < ST_HAL_IIO_MAX_DEVICES; i++) { + if (sensors_timeout[i] < min) + min = sensors_timeout[i]; + } + + return min; +} + +int64_t SensorBase::GetMinPeriod() +{ + int i; + int64_t min = INT64_MAX; + + for (i = 0; i < ST_HAL_IIO_MAX_DEVICES; i++) { + if ((sensors_pollrates[i] < min) && (sensors_pollrates[i] > 0)) + min = sensors_pollrates[i]; + } + + return min; +} + +void *SensorBase::ThreadWork(void *context) +{ + SensorBase *mypointer = (SensorBase *)context; + + mypointer->ThreadTask(); + + return mypointer; +} + +void SensorBase::ThreadTask() +{ + pthread_exit(NULL); +} diff --git a/libsensors_iio/src/SensorBase.h b/libsensors_iio/src/SensorBase.h new file mode 100644 index 0000000..6187ec3 --- /dev/null +++ b/libsensors_iio/src/SensorBase.h @@ -0,0 +1,157 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_SENSOR_BASE_H +#define ST_SENSOR_BASE_H + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "common_data.h" +#include + +#define SENSOR_BASE_0AXIS (0) +#define SENSOR_BASE_1AXIS (1) +#define SENSOR_BASE_3AXIS (3) +#define SENSOR_BASE_4AXIS (4) + +#define SENSOR_BASE_DEPENDENCY_0 (0) +#define SENSOR_BASE_DEPENDENCY_1 (1) +#define SENSOR_BASE_DEPENDENCY_2 (2) +#define SENSOR_BASE_DEPENDENCY_3 (3) +#define SENSOR_BASE_DEPENDENCY_4 (4) +#define SENSOR_BASE_MAX_DEPENDENCY (5) + +#define SENSOR_BASE_ANDROID_NAME_MAX (40) + +#define GAUSS_TO_UTESLA(x) ((x) * 100.0f) +#define NS_TO_FREQUENCY(x) (1E9 / x) +#define FREQUENCY_TO_NS(x) (1E9 / x) +#define FREQUENCY_TO_US(x) (1E6 / x) +#define NS_TO_MS(x) (x / 1E6) +#define NS_TO_S(x) (x / 1E9) + +typedef struct trigger_mutex { + pthread_mutex_t trigger_mutex; + pthread_cond_t trigger_data_cond; +} trigger_mutex; + +class SensorBase; + +/* + * class SensorBase + */ +class SensorBase { +private: + int64_t enabled_sensors_mask; + int64_t sensors_timeout[ST_HAL_IIO_MAX_DEVICES]; + int64_t sensors_pollrates[ST_HAL_IIO_MAX_DEVICES]; + int64_t last_timestap_pushed[ST_HAL_IIO_MAX_DEVICES]; + + CircularBuffer *circular_buffer_data[SENSOR_BASE_MAX_DEPENDENCY]; + +protected: + bool valid_class; + char android_name[SENSOR_BASE_ANDROID_NAME_MAX]; + + int android_pipe_fd; + int type_sensor_need_trigger; + int handle_remapping[ST_HAL_IIO_MAX_DEVICES]; + int type_dependencies[SENSOR_BASE_MAX_DEPENDENCY]; + int sensors_to_push_data_type[SENSOR_BASE_MAX_DEPENDENCY]; + + unsigned int num_data_axis; + unsigned int dependencies_num; + unsigned int sensors_to_trigger_num; + unsigned int sensors_to_push_data_num; + + int64_t real_pollrate; + int64_t last_data_timestamp; + + SensorBase *sensors_to_push_data[SENSOR_BASE_MAX_DEPENDENCY]; + SensorBase *sensors_to_trigger[SENSOR_BASE_MAX_DEPENDENCY]; + SensorBase *dependencies[SENSOR_BASE_MAX_DEPENDENCY]; + sensors_event_t sensor_event; + struct sensor_t sensor_t_data; + + trigger_mutex mutext; + + int64_t GetMinTimeout(); + int64_t GetMinPeriod(); + + void SetBitEnableMask(int handle); + void ResetBitEnableMask(int handle); + + bool GetStatusExcludeHandle(int handle); + bool GetStatusOfHandle(int handle); + + int AllocateBufferForDependencyData(int dependency_id, unsigned int max_fifo_len); + void DeAllocateBufferForDependencyData(int dependency_id); + + int AddSensorToDataPush(SensorBase *t); + void RemoveSensorToDataPush(SensorBase *t); + +public: + SensorBase(const char *name, int handle, int type, int pipe_data_fd); + virtual ~SensorBase(); + bool IsValidClass(); + + int GetHandle(); + int GetType(); + int GetMaxFifoLenght(); + + char* GetName(); + + virtual int Enable(int handle, bool enable); + bool GetStatus(); + + virtual int SetDelay(int handle, int64_t period_ns, int64_t timeout); + int64_t GetDelay(); + int64_t GetRealPollrate(); + + void GetDepenciesTypeList(int type[SENSOR_BASE_MAX_DEPENDENCY]); + int AddSensorDependency(SensorBase *p); + void RemoveSensorDependency(SensorBase *p); + + trigger_mutex* GetMutexForTrigger(); + int GetSensorNeedTriggerType(); + int AddSensorToTrigger(SensorBase *t); + + bool FillSensor_tData(struct sensor_t *data); + + virtual int FlushData(); + + virtual void ProcessData(SensorBaseData *data); + virtual void ProcessEvent(struct iio_event_data *event_data); + virtual void TriggerEventReceived(); + virtual void WriteDataToPipe(); + virtual void ReceiveDataFromDependency(int handle, SensorBaseData *data); + + virtual int GetLatestValidDataFromDependency(int dependency_id, SensorBaseData *data); + + static void *ThreadWork(void *context); + virtual void ThreadTask(); +}; + +#endif /* ST_SENSOR_BASE_H */ diff --git a/libsensors_iio/src/SensorHAL.cpp b/libsensors_iio/src/SensorHAL.cpp new file mode 100644 index 0000000..6cba89d --- /dev/null +++ b/libsensors_iio/src/SensorHAL.cpp @@ -0,0 +1,976 @@ +/* + * STMicroelectronics SensorHAL core + * + * Version 3.1.0 + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#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] * (pow(2, channels[0].bits_used - 1) - 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; + + return hal_data->sensor_classes[handle]->FlushData(); +} + +/** + * 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; +} + +/** + * 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); + + 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(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; + + *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, +}; diff --git a/libsensors_iio/src/SensorHAL.h b/libsensors_iio/src/SensorHAL.h new file mode 100644 index 0000000..99d4646 --- /dev/null +++ b/libsensors_iio/src/SensorHAL.h @@ -0,0 +1,104 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_SENSOR_HAL_H +#define ST_SENSOR_HAL_H + +#include +#include +#include + +#include "SWSensorBase.h" +#include "common_data.h" + +#define ARRAY_SIZE(a) (int)((sizeof(a) / sizeof(*(a))) / \ + static_cast(!(sizeof(a) % sizeof(*(a))))) + +/* + * Android string version + */ +#define ST_HAL_KITKAT_VERSION 0 +#define ST_HAL_LOLLIPOP_VERSION 1 + +/* + * IIO driver sensors names + */ +#define ST_SENSORS_LIST_1 "lsm6ds3" + +/* + * IIO driver sensors suffix for sensors + */ +#define ACCEL_NAME_SUFFIX_IIO "_accel" +#define MAGN_NAME_SUFFIX_IIO "_magn" +#define GYRO_NAME_SUFFIX_IIO "_gyro" +#define SIGN_MOTION_NAME_SUFFIX_IIO "_sign_motion" +#define STEP_DETECTOR_NAME_SUFFIX_IIO "_step_d" +#define STEP_COUNTER_NAME_SUFFIX_IIO "_step_c" +#define TILT_NAME_SUFFIX_IIO "_tilt" +#define PRESSURE_NAME_SUFFIX_IIO "_press" + +#define ST_HAL_WAKEUP_SUFFIX_IIO "_wk" + +#define CONCATENATE_STRING(x, y) (x y) + +#if (CONFIG_ST_HAL_ANDROID_VERSION == ST_HAL_LOLLIPOP_VERSION) +#define ST_HAL_IIO_DEVICE_API_VERSION SENSORS_DEVICE_API_VERSION_1_3 +#else /* CONFIG_ST_HAL_ANDROID_VERSION */ +#define ST_HAL_IIO_DEVICE_API_VERSION SENSORS_DEVICE_API_VERSION_1_1 +#endif /* CONFIG_ST_HAL_ANDROID_VERSION */ + +#if defined(CONFIG_ST_HAL_HAS_GEOMAG_FUSION) && \ + (defined(CONFIG_ST_HAL_GEOMAG_ROT_VECTOR_AP_ENABLED)) +#define ST_HAL_NEEDS_GEOMAG_FUSION 1 +#endif /* CONFIG_ST_HAL_GAME_ROT_VECTOR_AP_ENABLED */ + +#if defined(CONFIG_ST_HAL_HAS_6AX_FUSION) && \ + (defined(CONFIG_ST_HAL_GAME_ROT_VECTOR_AP_ENABLED)) +#define ST_HAL_NEEDS_6AX_FUSION 1 +#endif /* CONFIG_ST_HAL_GAME_ROT_VECTOR_AP_ENABLED */ + +#if defined(CONFIG_ST_HAL_HAS_9AX_FUSION) && \ + (defined(CONFIG_ST_HAL_ROT_VECTOR_AP_ENABLED) || \ + defined(CONFIG_ST_HAL_ORIENTATION_AP_ENABLED) || \ + defined(CONFIG_ST_HAL_GRAVITY_AP_ENABLED) || \ + defined(CONFIG_ST_HAL_LINEAR_AP_ENABLED)) +#define ST_HAL_NEEDS_9AX_FUSION 1 +#endif /* CONFIG_ST_HAL_GAME_ROT_VECTOR_AP_ENABLED */ + + +struct tmp_dicovery_data { + char *driver_name; + char *trigger_name; + char *dev_buffer_path; + + char *iio_sysfs_path; + char *iio_sysfs_custom_trigger_path; +}; + +struct STSensorHAL_data { + struct sensors_poll_device_1 poll_device; + + pthread_t *threads; + SensorBase *sensor_classes[ST_HAL_IIO_MAX_DEVICES]; + + unsigned int sensor_available; + struct sensor_t *sensor_t_list; + + struct pollfd android_pollfd; +} typedef STSensorHAL_data; + +#endif /* ST_SENSOR_HAL_H */ diff --git a/libsensors_iio/src/SignificantMotion.cpp b/libsensors_iio/src/SignificantMotion.cpp new file mode 100644 index 0000000..0a77625 --- /dev/null +++ b/libsensors_iio/src/SignificantMotion.cpp @@ -0,0 +1,50 @@ +/* + * STMicroelectronics SignificantMotion Base Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "SignificantMotion.h" + +SignMotion::SignMotion(HWSensorBaseCommonData *data, const char *name, int handle, + int pipe_data_fd, float power_consumption) : + HWSensorBase(data, name, handle, + SENSOR_TYPE_SIGNIFICANT_MOTION, 0, pipe_data_fd, power_consumption) +{ + sensor_t_data.stringType = SENSOR_STRING_TYPE_SIGNIFICANT_MOTION; + sensor_t_data.flags = SENSOR_FLAG_ONE_SHOT_MODE | SENSOR_FLAG_WAKE_UP; + sensor_t_data.minDelay = -1; + sensor_t_data.resolution = 1.0f; + sensor_t_data.maxRange = 1.0f; + + num_data_axis = SENSOR_BASE_0AXIS; +} + +SignMotion::~SignMotion() +{ + +} + +int SignMotion::SetDelay(int __attribute__((unused))handle, + int64_t __attribute__((unused))period_ns, + int64_t __attribute__((unused))timeout) +{ + return 0; +} + +void SignMotion::ProcessEvent(struct iio_event_data *event_data) +{ + sensor_event.data[0] = 1.0f; + sensor_event.timestamp = event_data->timestamp; + + HWSensorBase::WriteDataToPipe(); + HWSensorBase::ProcessEvent(event_data); + Enable(sensor_t_data.handle, false); +} diff --git a/libsensors_iio/src/SignificantMotion.h b/libsensors_iio/src/SignificantMotion.h new file mode 100644 index 0000000..f8115a9 --- /dev/null +++ b/libsensors_iio/src/SignificantMotion.h @@ -0,0 +1,36 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_SIGN_MOTION_BASE_H +#define ST_SIGN_MOTION_BASE_H + +#include "HWSensorBase.h" + +/* + * class SignMotion + */ +class SignMotion : public HWSensorBase { +public: + SignMotion(HWSensorBaseCommonData *data, const char *name, + int handle, int pipe_data_fd, float power_consumption); + virtual ~SignMotion(); + + virtual int SetDelay(int handle, int64_t period_ns, int64_t timeout); + virtual void ProcessEvent(struct iio_event_data *event_data); +}; + +#endif /* ST_SIGN_MOTION_BASE_H */ diff --git a/libsensors_iio/src/StepCounter.cpp b/libsensors_iio/src/StepCounter.cpp new file mode 100644 index 0000000..ec14292 --- /dev/null +++ b/libsensors_iio/src/StepCounter.cpp @@ -0,0 +1,83 @@ +/* + * STMicroelectronics Step Counter Sensor Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "StepCounter.h" + +StepCounter::StepCounter(HWSensorBaseCommonData *data, const char *name, + int handle, unsigned int hw_fifo_len, int pipe_data_fd, + float power_consumption, bool wakeup) : + HWSensorBase(data, name, handle, + SENSOR_TYPE_STEP_COUNTER, hw_fifo_len, pipe_data_fd, power_consumption) +{ + sensor_t_data.stringType = SENSOR_STRING_TYPE_STEP_COUNTER; + sensor_t_data.flags = SENSOR_FLAG_ON_CHANGE_MODE; + + if (wakeup) + sensor_t_data.flags |= SENSOR_FLAG_WAKE_UP; + + sensor_t_data.resolution = 1.0f; + sensor_t_data.maxRange = pow(2, data->channels[0].bits_used) - 1; + + num_data_axis = SENSOR_BASE_1AXIS; +} + +StepCounter::~StepCounter() +{ + +} + +int StepCounter::Enable(int handle, bool enable) +{ + int err; + + err = HWSensorBase::Enable(handle, enable); + if (err < 0) + return err; + + if (!GetStatus()) + last_data_timestamp = 0; + + return 0; +} + +int StepCounter::SetDelay(int handle, int64_t period_ns, int64_t timeout) +{ + int err; + int64_t min_pollrate_ns; + + err = SensorBase::SetDelay(handle, period_ns, timeout); + if (err < 0) + return err; + + min_pollrate_ns = GetMinPeriod(); + + err = write_sysfs_int((char *)FILENAME_MAX_DELIVERY_RATE, + common_data.iio_sysfs_path, (int)NS_TO_MS(min_pollrate_ns)); + if (err < 0) { + ALOGE("%s: Failed to write max rate delivery \"%s/%s\".", + common_data.device_name, common_data.iio_sysfs_path, + FILENAME_MAX_DELIVERY_RATE); + return err; + } + + return 0; +} + +void StepCounter::ProcessData(SensorBaseData *data) +{ + sensor_event.u64.step_counter = (uint64_t)data->raw[0]; + sensor_event.timestamp = data->timestamp; + + HWSensorBase::WriteDataToPipe(); + HWSensorBase::ProcessData(data); +} diff --git a/libsensors_iio/src/StepCounter.h b/libsensors_iio/src/StepCounter.h new file mode 100644 index 0000000..ffe057e --- /dev/null +++ b/libsensors_iio/src/StepCounter.h @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_STEP_COUNTER_SENSOR_H +#define ST_STEP_COUNTER_SENSOR_H + +#include "HWSensorBase.h" + +#define FILENAME_MAX_DELIVERY_RATE "max_delivery_rate" + +/* + * class StepCounter + */ +class StepCounter : public HWSensorBase { +public: + StepCounter(HWSensorBaseCommonData *data, const char *name, + int handle, unsigned int hw_fifo_len, int pipe_data_fd, + float power_consumption, bool wakeup); + ~StepCounter(); + + virtual int Enable(int handle, bool enable); + virtual int SetDelay(int handle, int64_t period_ns, int64_t timeout); + virtual void ProcessData(SensorBaseData *data); +}; + +#endif /* ST_STEP_COUNTER_SENSOR_H */ diff --git a/libsensors_iio/src/StepDetector.cpp b/libsensors_iio/src/StepDetector.cpp new file mode 100644 index 0000000..40b0cd5 --- /dev/null +++ b/libsensors_iio/src/StepDetector.cpp @@ -0,0 +1,53 @@ +/* + * STMicroelectronics Step Detector Base Class + * + * Copyright 2013-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "StepDetector.h" + +StepDetector::StepDetector(HWSensorBaseCommonData *data, const char *name, + int handle, unsigned int hw_fifo_len, int pipe_data_fd, + float power_consumption, bool wakeup) : + HWSensorBase(data, name, handle, + SENSOR_TYPE_STEP_DETECTOR, hw_fifo_len, pipe_data_fd, power_consumption) +{ + sensor_t_data.stringType = SENSOR_STRING_TYPE_STEP_DETECTOR; + sensor_t_data.flags = SENSOR_FLAG_SPECIAL_REPORTING_MODE; + + if (wakeup) + sensor_t_data.flags |= SENSOR_FLAG_WAKE_UP; + + sensor_t_data.resolution = 1.0f; + sensor_t_data.maxRange = 1.0f; + + num_data_axis = SENSOR_BASE_0AXIS; +} + +StepDetector::~StepDetector() +{ + +} + +int StepDetector::SetDelay(int __attribute__((unused))handle, + int64_t __attribute__((unused))period_ns, + int64_t __attribute__((unused))timeout) +{ + return 0; +} + +void StepDetector::ProcessEvent(struct iio_event_data *event_data) +{ + sensor_event.data[0] = 1.0f; + sensor_event.timestamp = event_data->timestamp; + + HWSensorBase::WriteDataToPipe(); + HWSensorBase::ProcessEvent(event_data); +} diff --git a/libsensors_iio/src/StepDetector.h b/libsensors_iio/src/StepDetector.h new file mode 100644 index 0000000..8a90377 --- /dev/null +++ b/libsensors_iio/src/StepDetector.h @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_STEP_DETECTOR_SENSOR_H +#define ST_STEP_DETECTOR_SENSOR_H + +#include "HWSensorBase.h" + +/* + * class StepDetector + */ +class StepDetector : public HWSensorBase { +public: + StepDetector(HWSensorBaseCommonData *data, const char *name, + int handle, unsigned int hw_fifo_len, int pipe_data_fd, + float power_consumption, bool wakeup); + virtual ~StepDetector(); + + virtual int SetDelay(int handle, int64_t period_ns, int64_t timeout); + virtual void ProcessEvent(struct iio_event_data *event_data); +}; + +#endif /* ST_STEP_DETECTOR_SENSOR_H */ diff --git a/libsensors_iio/src/TiltSensor.cpp b/libsensors_iio/src/TiltSensor.cpp new file mode 100644 index 0000000..67e0e79 --- /dev/null +++ b/libsensors_iio/src/TiltSensor.cpp @@ -0,0 +1,48 @@ +/* + * STMicroelectronics Tilt Sensor Class + * + * Copyright 2014-2015 STMicroelectronics Inc. + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"). + */ + +#include +#include +#include + +#include "TiltSensor.h" + +TiltSensor::TiltSensor(HWSensorBaseCommonData *data, const char *name, int handle, + int pipe_data_fd, float power_consumption) : + HWSensorBase(data, name, handle, SENSOR_TYPE_TILT_DETECTOR, 0, + pipe_data_fd, power_consumption) +{ + sensor_t_data.stringType = SENSOR_STRING_TYPE_TILT_DETECTOR; + sensor_t_data.flags = SENSOR_FLAG_SPECIAL_REPORTING_MODE | SENSOR_FLAG_WAKE_UP; + sensor_t_data.resolution = 1.0f; + sensor_t_data.maxRange = 1.0f; + + num_data_axis = SENSOR_BASE_0AXIS; +} + +TiltSensor::~TiltSensor() +{ + +} + +int TiltSensor::SetDelay(int __attribute__((unused))handle, + int64_t __attribute__((unused))period_ns, + int64_t __attribute__((unused))timeout) +{ + return 0; +} + +void TiltSensor::ProcessEvent(struct iio_event_data *event_data) +{ + sensor_event.data[0] = 1.0f; + sensor_event.timestamp = event_data->timestamp; + + HWSensorBase::WriteDataToPipe(); + HWSensorBase::ProcessEvent(event_data); +} diff --git a/libsensors_iio/src/TiltSensor.h b/libsensors_iio/src/TiltSensor.h new file mode 100644 index 0000000..66d0021 --- /dev/null +++ b/libsensors_iio/src/TiltSensor.h @@ -0,0 +1,36 @@ +/* + * Copyright (C) 2014-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ST_TILT_SENSOR_H +#define ST_TILT_SENSOR_H + +#include "HWSensorBase.h" + +/* + * class TiltSensor + */ +class TiltSensor : public HWSensorBase { +public: + TiltSensor(HWSensorBaseCommonData *data, const char *name, int handle, + int pipe_data_fd, float power_consumption); + virtual ~TiltSensor(); + + virtual int SetDelay(int handle, int64_t period_ns, int64_t timeout); + virtual void ProcessEvent(struct iio_event_data *event_data); +}; + +#endif /* ST_TILT_SENSOR_H */ diff --git a/libsensors_iio/src/android_KK_defconfig b/libsensors_iio/src/android_KK_defconfig new file mode 100644 index 0000000..10c5252 --- /dev/null +++ b/libsensors_iio/src/android_KK_defconfig @@ -0,0 +1,151 @@ +# +# Automatically generated file; DO NOT EDIT. +# SensorHAL IIO Configuration +# +CONFIG_ST_HAL_ANDROID_VERSION=0 + +# +# Enable/disable sensors +# + +# +# Accelerometer sensor +# +CONFIG_ST_HAL_ACCEL_ENABLED=y + +# +# Magnetometer sensor +# +CONFIG_ST_HAL_MAGN_ENABLED=y +# CONFIG_ST_HAL_MAGN_CALIB_ENABLED is not set + +# +# Gyroscope sensor +# +CONFIG_ST_HAL_GYRO_ENABLED=y +# CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED is not set + +# +# Magnetometer Uncalibrated sensor +# +CONFIG_ST_HAL_MAGN_UNCALIB_DISABLED=y +# CONFIG_ST_HAL_MAGN_UNCALIB_HW_ENABLED is not set +# CONFIG_ST_HAL_MAGN_UNCALIB_AP_ENABLED is not set + +# +# Gyroscope Uncalibrated sensor +# +CONFIG_ST_HAL_GYRO_UNCALIB_DISABLED=y +# CONFIG_ST_HAL_GYRO_UNCALIB_HW_ENABLED is not set +# CONFIG_ST_HAL_GYRO_UNCALIB_AP_ENABLED is not set +CONFIG_ST_HAL_SIGN_MOTION_ENABLED=y + +# +# Step Detector sensor +# +CONFIG_ST_HAL_STEP_DETECTOR_ENABLED=y + +# +# Step Counter sensor +# +CONFIG_ST_HAL_STEP_COUNTER_ENABLED=y +# CONFIG_ST_HAL_PRESSURE_ENABLED is not set +CONFIG_ST_HAL_TILT_ENABLED=y +# CONFIG_ST_HAL_TAP_TAP_ENABLED is not set +# CONFIG_ST_HAL_ACTIVITY_ENABLED is not set + +# +# Orientation sensor +# +CONFIG_ST_HAL_ORIENTATION_DISABLED=y +# CONFIG_ST_HAL_ORIENTATION_HW_ENABLED is not set +# CONFIG_ST_HAL_ORIENTATION_AP_ENABLED is not set + +# +# Gravity sensor +# +CONFIG_ST_HAL_GRAVITY_DISABLED=y +# CONFIG_ST_HAL_GRAVITY_HW_ENABLED is not set +# CONFIG_ST_HAL_GRAVITY_AP_ENABLED is not set + +# +# Linear Acceleration sensor +# +CONFIG_ST_HAL_LINEAR_DISABLED=y +# CONFIG_ST_HAL_LINEAR_HW_ENABLED is not set +# CONFIG_ST_HAL_LINEAR_AP_ENABLED is not set + +# +# Rotation Vector sensor +# +CONFIG_ST_HAL_ROT_VECTOR_DISABLED=y +# CONFIG_ST_HAL_ROT_VECTOR_HW_ENABLED is not set +# CONFIG_ST_HAL_ROT_VECTOR_AP_ENABLED is not set + +# +# Game Rotation Vector sensor +# +CONFIG_ST_HAL_GAME_ROT_VECTOR_DISABLED=y +# CONFIG_ST_HAL_GAME_ROT_VECTOR_HW_ENABLED is not set +# CONFIG_ST_HAL_GAME_ROT_VECTOR_AP_ENABLED is not set + +# +# Geomagnetic Rotation Vector sensor +# +CONFIG_ST_HAL_GEOMAG_ROT_VECTOR_DISABLED=y +# CONFIG_ST_HAL_GEOMAG_ROT_VECTOR_HW_ENABLED is not set +# CONFIG_ST_HAL_GEOMAG_ROT_VECTOR_AP_ENABLED is not set +# CONFIG_ST_HAL_VIRTUAL_GYRO_ENABLED is not set +CONFIG_ST_HAL_FACEUP_FACEDOWN_DISABLED=y +# CONFIG_ST_HAL_FACEUP_FACEDOWN_HW_ENABLED is not set +# CONFIG_ST_HAL_FACEUP_FACEDOWN_AP_ENABLED is not set +CONFIG_ST_HAL_RELATIVE_TILT_DISABLED=y +# CONFIG_ST_HAL_RELATIVE_TILT_HW_ENABLED is not set +# CONFIG_ST_HAL_RELATIVE_TILT_AP_ENABLED is not set + +# +# Common configuration +# +CONFIG_ST_HAL_MAX_SAMPLING_FREQUENCY=200 +CONFIG_ST_HAL_DEBUG_LEVEL=0 +CONFIG_ST_HAL_ACCEL_ROT_MATRIX="1,0,0,0,1,0,0,0,1" +CONFIG_ST_HAL_ACCEL_RANGE=17 +CONFIG_ST_HAL_MAGN_ROT_MATRIX="1,0,0,0,1,0,0,0,1" +CONFIG_ST_HAL_MAGN_RANGE=1000 +CONFIG_ST_HAL_GYRO_ROT_MATRIX="1,0,0,0,1,0,0,0,1" +CONFIG_ST_HAL_GYRO_RANGE=35 +# CONFIG_ST_HAL_SENSORHUB_DEBUG_INFO is not set + +# +# Power consumption +# +CONFIG_ST_HAL_ACCEL_POWER_CONSUMPTION=1 +CONFIG_ST_HAL_MAGN_POWER_CONSUMPTION=1 +CONFIG_ST_HAL_GYRO_POWER_CONSUMPTION=1 +CONFIG_ST_HAL_SIGN_MOTION_POWER_CONSUMPTION=1 +CONFIG_ST_HAL_STEP_DETECTOR_POWER_CONSUMPTION=1 +CONFIG_ST_HAL_STEP_COUNTER_POWER_CONSUMPTION=1 +CONFIG_ST_HAL_TILT_POWER_CONSUMPTION=1 + +# +# SensorFusion configuration +# +CONFIG_ST_HAL_MIN_FUSION_POLLRATE=50 +CONFIG_ST_HAL_INEMO_GBIAS_THRESOLD_ACCEL="86999e-9" +CONFIG_ST_HAL_INEMO_GBIAS_THRESOLD_MAGN="599999e-9" +CONFIG_ST_HAL_INEMO_GBIAS_THRESOLD_GYRO="114999e-9" + +# +# IIO drivers configuration +# +CONFIG_ST_HAL_DRIVER_AUTO_SCAN=y + +# +# IIO triggers name +# +CONFIG_ST_HAL_ACCEL_TRIGGER_NAME_DEFAULT=y +# CONFIG_ST_HAL_ACCEL_TRIGGER_NAME_HRTIMER is not set +CONFIG_ST_HAL_MAGN_TRIGGER_NAME_DEFAULT=y +# CONFIG_ST_HAL_MAGN_TRIGGER_NAME_HRTIMER is not set +CONFIG_ST_HAL_GYRO_TRIGGER_NAME_DEFAULT=y +# CONFIG_ST_HAL_GYRO_TRIGGER_NAME_HRTIMER is not set diff --git a/libsensors_iio/src/android_L_defconfig b/libsensors_iio/src/android_L_defconfig new file mode 100644 index 0000000..2add11a --- /dev/null +++ b/libsensors_iio/src/android_L_defconfig @@ -0,0 +1,67 @@ +# +# Automatically generated file; DO NOT EDIT. +# SensorHAL IIO Configuration +# +CONFIG_ST_HAL_ANDROID_VERSION=1 +# CONFIG_ST_HAL_HAS_6AX_FUSION is not set +# CONFIG_ST_HAL_HAS_9AX_FUSION is not set +# CONFIG_ST_HAL_HAS_GEOMAG_FUSION is not set +CONFIG_ST_HAL_HAS_MAGN_CALIB=y +CONFIG_ST_HAL_HAS_GYRO_GBIAS_ESTIMATION=y +# CONFIG_ST_HAL_HAS_TILT_FU_FD is not set + +# +# Enable/disable sensors +# +CONFIG_ST_HAL_ACCEL_ENABLED=y +CONFIG_ST_HAL_MAGN_ENABLED=y +# CONFIG_ST_HAL_MAGN_CALIB_ENABLED is not set +CONFIG_ST_HAL_GYRO_ENABLED=y +# CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED is not set +CONFIG_ST_HAL_MAGN_UNCALIB_DISABLED=y +# CONFIG_ST_HAL_MAGN_UNCALIB_HW_ENABLED is not set +# CONFIG_ST_HAL_MAGN_UNCALIB_AP_ENABLED is not set +CONFIG_ST_HAL_GYRO_UNCALIB_DISABLED=y +# CONFIG_ST_HAL_GYRO_UNCALIB_HW_ENABLED is not set +# CONFIG_ST_HAL_GYRO_UNCALIB_AP_ENABLED is not set +CONFIG_ST_HAL_SIGN_MOTION_ENABLED=y +CONFIG_ST_HAL_STEP_DETECTOR_ENABLED=y +CONFIG_ST_HAL_STEP_COUNTER_ENABLED=y +# CONFIG_ST_HAL_PRESSURE_ENABLED is not set +# CONFIG_ST_HAL_LIGHT_ENABLED is not set +# CONFIG_ST_HAL_PROXIMITY_ENABLED is not set +CONFIG_ST_HAL_TILT_ENABLED=y +# CONFIG_ST_HAL_TAP_TAP_ENABLED is not set +# CONFIG_ST_HAL_ACTIVITY_ENABLED is not set +CONFIG_ST_HAL_ORIENTATION_DISABLED=y +# CONFIG_ST_HAL_ORIENTATION_HW_ENABLED is not set +CONFIG_ST_HAL_GRAVITY_DISABLED=y +# CONFIG_ST_HAL_GRAVITY_HW_ENABLED is not set +CONFIG_ST_HAL_LINEAR_DISABLED=y +# CONFIG_ST_HAL_LINEAR_HW_ENABLED is not set +CONFIG_ST_HAL_ROT_VECTOR_DISABLED=y +# CONFIG_ST_HAL_ROT_VECTOR_HW_ENABLED is not set +CONFIG_ST_HAL_GAME_ROT_VECTOR_DISABLED=y +# CONFIG_ST_HAL_GAME_ROT_VECTOR_HW_ENABLED is not set +CONFIG_ST_HAL_GEOMAG_ROT_VECTOR_DISABLED=y +# CONFIG_ST_HAL_GEOMAG_ROT_VECTOR_HW_ENABLED is not set +CONFIG_ST_HAL_FACEUP_FACEDOWN_DISABLED=y +# CONFIG_ST_HAL_FACEUP_FACEDOWN_HW_ENABLED is not set +CONFIG_ST_HAL_RELATIVE_TILT_DISABLED=y +# CONFIG_ST_HAL_RELATIVE_TILT_HW_ENABLED is not set + +# +# Common configuration +# +CONFIG_ST_HAL_MAX_SAMPLING_FREQUENCY=200 +CONFIG_ST_HAL_DEBUG_LEVEL=0 +CONFIG_ST_HAL_ACCEL_ROT_MATRIX="1,0,0,0,1,0,0,0,1" +CONFIG_ST_HAL_ACCEL_RANGE=17 +CONFIG_ST_HAL_MAGN_ROT_MATRIX="1,0,0,0,1,0,0,0,1" +CONFIG_ST_HAL_MAGN_RANGE=1000 +CONFIG_ST_HAL_GYRO_ROT_MATRIX="1,0,0,0,1,0,0,0,1" +CONFIG_ST_HAL_GYRO_RANGE=35 + +# +# IIO drivers configuration +# diff --git a/libsensors_iio/src/common_data.h b/libsensors_iio/src/common_data.h new file mode 100644 index 0000000..1a8f8f4 --- /dev/null +++ b/libsensors_iio/src/common_data.h @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2013-2015 STMicroelectronics + * Author: Denis Ciocca - + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_SENSOR_HAL_COMMON_DATA +#define ANDROID_SENSOR_HAL_COMMON_DATA + +#include + +#include "../configuration.h" + +#define SENSOR_TYPE_ST_CUSTOM_NO_SENSOR (SENSOR_TYPE_DEVICE_PRIVATE_BASE + 20) +#define SENSOR_TYPE_ST_ACCEL_MAGN_FUSION6X (SENSOR_TYPE_ST_CUSTOM_NO_SENSOR + 0) +#define SENSOR_TYPE_ST_ACCEL_GYRO_FUSION6X (SENSOR_TYPE_ST_CUSTOM_NO_SENSOR + 1) +#define SENSOR_TYPE_ST_ACCEL_MAGN_GYRO_FUSION9X (SENSOR_TYPE_ST_CUSTOM_NO_SENSOR + 2) + +#define ST_HAL_IIO_MAX_DEVICES (50) + +#define SENSOR_DATA_X(datax, datay, dataz, x1, y1, z1, x2, y2, z2, x3, y3, z3) \ + ((x1 == 1 ? datax : (x1 == -1 ? -datax : 0)) + \ + (x2 == 1 ? datay : (x2 == -1 ? -datay : 0)) + \ + (x3 == 1 ? dataz : (x3 == -1 ? -dataz : 0))) + +#define SENSOR_DATA_Y(datax, datay, dataz, x1, y1, z1, x2, y2, z2, x3, y3, z3) \ + ((y1 == 1 ? datax : (y1 == -1 ? -datax : 0)) + \ + (y2 == 1 ? datay : (y2 == -1 ? -datay : 0)) + \ + (y3 == 1 ? dataz : (y3 == -1 ? -dataz : 0))) + +#define SENSOR_DATA_Z(datax, datay, dataz, x1, y1, z1, x2, y2, z2, x3, y3, z3) \ + ((z1 == 1 ? datax : (z1 == -1 ? -datax : 0)) + \ + (z2 == 1 ? datay : (z2 == -1 ? -datay : 0)) + \ + (z3 == 1 ? dataz : (z3 == -1 ? -dataz : 0))) + +#define SENSOR_X_DATA(...) SENSOR_DATA_X(__VA_ARGS__) +#define SENSOR_Y_DATA(...) SENSOR_DATA_Y(__VA_ARGS__) +#define SENSOR_Z_DATA(...) SENSOR_DATA_Z(__VA_ARGS__) + +#define ST_HAL_DEBUG_INFO (1) +#define ST_HAL_DEBUG_VERBOSE (2) +#define ST_HAL_DEBUG_EXTRA_VERBOSE (3) + +#endif /* ANDROID_SENSOR_HAL_COMMON_DATA */ diff --git a/libsensors_iio/src/events.h b/libsensors_iio/src/events.h new file mode 100644 index 0000000..e924fe9 --- /dev/null +++ b/libsensors_iio/src/events.h @@ -0,0 +1,106 @@ +/* The industrial I/O - event passing to userspace + * + * Copyright (c) 2008-2011 Jonathan Cameron + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 2 as published by + * the Free Software Foundation. + */ +#ifndef _IIO_EVENTS_H_ +#define _IIO_EVENTS_H_ + +#include +#include +#include "types.h" + +/** + * struct iio_event_data - The actual event being pushed to userspace + * @id: event identifier + * @timestamp: best estimate of time of event occurrence (often from + * the interrupt handler) + */ +struct iio_event_data { + __u64 id; + __s64 timestamp; +}; + +#define IIO_GET_EVENT_FD_IOCTL _IOR('i', 0x90, int) + +enum iio_event_type { + IIO_EV_TYPE_THRESH, + IIO_EV_TYPE_MAG, + IIO_EV_TYPE_ROC, + IIO_EV_TYPE_THRESH_ADAPTIVE, + IIO_EV_TYPE_MAG_ADAPTIVE, + IIO_EV_TYPE_FIFO_EMPTIED, +}; + +enum iio_event_direction { + IIO_EV_DIR_EITHER, + IIO_EV_DIR_RISING, + IIO_EV_DIR_FALLING, +}; + +/** + * IIO_EVENT_CODE() - create event identifier + * @chan_type: Type of the channel. Should be one of enum iio_chan_type. + * @diff: Whether the event is for an differential channel or not. + * @modifier: Modifier for the channel. Should be one of enum iio_modifier. + * @direction: Direction of the event. One of enum iio_event_direction. + * @type: Type of the event. Should be one of enum iio_event_type. + * @chan: Channel number for non-differential channels. + * @chan1: First channel number for differential channels. + * @chan2: Second channel number for differential channels. + */ + +#define IIO_EVENT_CODE(chan_type, diff, modifier, direction, \ + type, chan, chan1, chan2) \ + (((u64)type << 56) | ((u64)diff << 55) | \ + ((u64)direction << 48) | ((u64)modifier << 40) | \ + ((u64)chan_type << 32) | (((u16)chan2) << 16) | ((u16)chan1) | \ + ((u16)chan)) + + +#define IIO_EV_DIR_MAX 4 +#define IIO_EV_BIT(type, direction) \ + (1 << (type*IIO_EV_DIR_MAX + direction)) + +/** + * IIO_MOD_EVENT_CODE() - create event identifier for modified channels + * @chan_type: Type of the channel. Should be one of enum iio_chan_type. + * @number: Channel number. + * @modifier: Modifier for the channel. Should be one of enum iio_modifier. + * @type: Type of the event. Should be one of enum iio_event_type. + * @direction: Direction of the event. One of enum iio_event_direction. + */ + +#define IIO_MOD_EVENT_CODE(chan_type, number, modifier, \ + type, direction) \ + IIO_EVENT_CODE(chan_type, 0, modifier, direction, type, number, 0, 0) + +/** + * IIO_UNMOD_EVENT_CODE() - create event identifier for unmodified channels + * @chan_type: Type of the channel. Should be one of enum iio_chan_type. + * @number: Channel number. + * @type: Type of the event. Should be one of enum iio_event_type. + * @direction: Direction of the event. One of enum iio_event_direction. + */ + +#define IIO_UNMOD_EVENT_CODE(chan_type, number, type, direction) \ + IIO_EVENT_CODE(chan_type, 0, 0, direction, type, number, 0, 0) + +#define IIO_EVENT_CODE_EXTRACT_TYPE(mask) ((mask >> 56) & 0xFF) + +#define IIO_EVENT_CODE_EXTRACT_DIR(mask) ((mask >> 48) & 0xCF) + +#define IIO_EVENT_CODE_EXTRACT_CHAN_TYPE(mask) ((mask >> 32) & 0xFF) + +/* Event code number extraction depends on which type of event we have. + * Perhaps review this function in the future*/ +#define IIO_EVENT_CODE_EXTRACT_CHAN(mask) ((__s16)(mask & 0xFFFF)) +#define IIO_EVENT_CODE_EXTRACT_CHAN2(mask) ((__s16)(((mask) >> 16) & 0xFFFF)) + +#define IIO_EVENT_CODE_EXTRACT_MODIFIER(mask) ((mask >> 40) & 0xFF) +#define IIO_EVENT_CODE_EXTRACT_DIFF(mask) (((mask) >> 55) & 0x1) + +#endif /* _IIO_EVENTS_H_ */ diff --git a/libsensors_iio/src/iio_utils.c b/libsensors_iio/src/iio_utils.c new file mode 100644 index 0000000..ab2aedc --- /dev/null +++ b/libsensors_iio/src/iio_utils.c @@ -0,0 +1,987 @@ +/* IIO - useful set of util functionality + * + * Copyright (c) 2008 Jonathan Cameron + * Modified by Denis Ciocca + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 2 as published by + * the Free Software Foundation. + */ + +#include + +#include "iio_utils.h" +#include "common_data.h" + + +const char *iio_dir = "/sys/bus/iio/devices/"; +const char *iio_sampling_frequency_available_filename = "sampling_frequency_available"; +const char *iio_hw_fifo_lenght = "hw_fifo_lenght"; +const char *iio_buffer_enable = "buffer/enable"; + + +/** + * iioutils_break_up_name() - extract generic name from full channel name + * @full_name: the full channel name + * @generic_name: the output generic channel name + **/ +inline int iioutils_break_up_name(const char *full_name, char **generic_name) +{ + char *current; + char *w, *r; + char *working; + + current = strdup(full_name); + working = strtok(current, "_\0"); + w = working; + r = working; + + while (*r != '\0') { + if (!isdigit(*r)) { + *w = *r; + w++; + } + r++; + } + *w = '\0'; + *generic_name = strdup(working); + free(current); + + return 0; +} + +/** + * iioutils_get_type() - find and process _type attribute data + * @is_signed: output whether channel is signed + * @bytes: output how many bytes the channel storage occupies + * @mask: output a bit mask for the raw data + * @be: big endian + * @device_dir: the iio device directory + * @name: the channel name + * @generic_name: the channel type name + **/ +inline int iioutils_get_type(unsigned *is_signed, unsigned *bytes, + unsigned *bits_used, unsigned *shift, uint64_t *mask, unsigned *be, + const char *device_dir, const char *name, const char *generic_name) +{ + char *scan_el_dir, *builtname, *builtname_generic, *filename = 0; + char signchar, endianchar; + const struct dirent *ent; + unsigned padint; + FILE *sysfsfp; + int ret; + DIR *dp; + + ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir); + if (ret < 0) { + ret = -ENOMEM; + goto error_ret; + } + + ret = asprintf(&builtname, FORMAT_TYPE_FILE, name); + if (ret < 0) { + ret = -ENOMEM; + goto error_free_scan_el_dir; + } + + ret = asprintf(&builtname_generic, FORMAT_TYPE_FILE, generic_name); + if (ret < 0) { + ret = -ENOMEM; + goto error_free_builtname; + } + + dp = opendir(scan_el_dir); + if (dp == NULL) { + ret = -errno; + goto error_free_builtname_generic; + } + + while (ent = readdir(dp), ent != NULL) + /* + * Do we allow devices to override a generic name with + * a specific one? + */ + if ((strcmp(builtname, ent->d_name) == 0) || + (strcmp(builtname_generic, ent->d_name) == 0)) { + ret = asprintf(&filename, "%s/%s", scan_el_dir, ent->d_name); + if (ret < 0) { + ret = -ENOMEM; + goto error_closedir; + } + + sysfsfp = fopen(filename, "r"); + if (sysfsfp == NULL) { + printf("failed to open %s\n", filename); + ret = -errno; + goto error_free_filename; + } + + ret = fscanf(sysfsfp, "%ce:%c%u/%u>>%u", &endianchar, + &signchar, bits_used, &padint, shift); + if (ret < 0) { + ret = -errno; + goto error_close_sysfsfp; + } + + *be = (endianchar == 'b'); + *bytes = padint / 8; + if (*bits_used == 64) + *mask = ~0; + else + *mask = (1 << *bits_used) - 1; + + if (signchar == 's') + *is_signed = 1; + else + *is_signed = 0; + + fclose(sysfsfp); + free(filename); + + filename = 0; + sysfsfp = 0; + } + +error_close_sysfsfp: + if (sysfsfp) + fclose(sysfsfp); +error_free_filename: + if (filename) + free(filename); +error_closedir: + closedir(dp); +error_free_builtname_generic: + free(builtname_generic); +error_free_builtname: + free(builtname); +error_free_scan_el_dir: + free(scan_el_dir); +error_ret: + return ret; +} + +inline int iioutils_get_param_float(float *output, const char *param_name, + const char *device_dir, const char *name, const char *generic_name) +{ + char *builtname, *builtname_generic; + const struct dirent *ent; + char *filename = NULL; + FILE *sysfsfp; + int ret; + DIR *dp; + + ret = asprintf(&builtname, "%s_%s", name, param_name); + if (ret < 0) { + ret = -ENOMEM; + goto error_ret; + } + + ret = asprintf(&builtname_generic, "%s_%s", generic_name, param_name); + if (ret < 0) { + ret = -ENOMEM; + goto error_free_builtname; + } + + dp = opendir(device_dir); + if (dp == NULL) { + ret = -errno; + goto error_free_builtname_generic; + } + + while (ent = readdir(dp), ent != NULL) + if ((strcmp(builtname, ent->d_name) == 0) || + (strcmp(builtname_generic, ent->d_name) == 0)) { + ret = asprintf(&filename, "%s/%s", device_dir, ent->d_name); + if (ret < 0) { + ret = -ENOMEM; + goto error_closedir; + } + + sysfsfp = fopen(filename, "r"); + if (!sysfsfp) { + ret = -errno; + goto error_free_filename; + } + fscanf(sysfsfp, "%f", output); + break; + } +error_free_filename: + if (filename) + free(filename); +error_closedir: + closedir(dp); +error_free_builtname_generic: + free(builtname_generic); +error_free_builtname: + free(builtname); +error_ret: + return ret; +} + +/** + * bsort_channel_array_by_index() - reorder so that the array is in index order + * + **/ +inline void bsort_channel_array_by_index(struct iio_channel_info **ci_array, + int cnt) +{ + + struct iio_channel_info temp; + int x, y; + + for (x = 0; x < cnt; x++) { + for (y = 0; y < (cnt - 1); y++) { + if ((*ci_array)[y].index > (*ci_array)[y+1].index) { + temp = (*ci_array)[y + 1]; + (*ci_array)[y + 1] = (*ci_array)[y]; + (*ci_array)[y] = temp; + } + } + } +} + +int iio_utils_enable_sensor(const char *device_dir, bool enable) +{ + return write_sysfs_int_and_verify((char *)iio_buffer_enable, (char *)device_dir, (int)enable); +} + +int iio_utils_get_hw_fifo_lenght(const char *device_dir) +{ + int len; + + len = read_sysfs_posint((char *)iio_hw_fifo_lenght, (char *)device_dir); + if (len < 0) + return 0; + + return len; +} + +int iio_utils_set_scale(const char *device_dir, float value, int device_type) +{ + int err; + char *scale_file_name; + + switch (device_type) { + case SENSOR_TYPE_ACCELEROMETER: + scale_file_name = (char *)"in_accel_x_scale"; + break; + case SENSOR_TYPE_MAGNETIC_FIELD: + scale_file_name = (char *)"in_magn_x_scale"; + break; + case SENSOR_TYPE_GYROSCOPE: + scale_file_name = (char *)"in_anglvel_x_scale"; + break; + case SENSOR_TYPE_PRESSURE: + scale_file_name = (char *)"in_press_scale"; + break; + default: + return -EINVAL; + } + + err = write_sysfs_float_and_verify(scale_file_name, (char *)device_dir, value); + if (err < 0) + return err; + + return 0; +} + +int iio_utils_get_scale_available(const char *device_dir, struct iio_scale_available *sa, int device_type) +{ + int err; + FILE *fp; + char *tmp_name, *avl_name, *pch, line[200]; + + sa->num_available = 0; + + switch (device_type) { + case SENSOR_TYPE_ACCELEROMETER: + avl_name = (char *)"in_accel_scale_available"; + break; + case SENSOR_TYPE_MAGNETIC_FIELD: + avl_name = (char *)"in_magn_scale_available"; + break; + case SENSOR_TYPE_GYROSCOPE: + avl_name = (char *)"in_anglvel_scale_available"; + break; + case SENSOR_TYPE_PRESSURE: + avl_name = (char *)"in_press_scale_available"; + break; + default: + return -EINVAL; + } + + err = asprintf(&tmp_name, "%s/%s", device_dir, avl_name); + if (err < 0) + return err; + + fp = fopen(tmp_name, "r"); + if (fp == NULL) { + err = 0; + goto open_file_error; + } + + fgets(line, sizeof(line), fp); + if (line == NULL) { + ALOGE("Scale available file format error: \"%s\".", tmp_name); + err = -EINVAL; + goto read_error; + } + + pch = strtok(line," "); + while (pch != NULL) { + sa->values[sa->num_available] = atof(pch); + pch = strtok(NULL, " "); + sa->num_available++; + + if (sa->num_available >= IIO_UTILS_SCALE_AVAILABLE) + break; + } + +read_error: + fclose(fp); +open_file_error: + free(tmp_name); + return err < 0 ? err : 0; +} + +int iio_utils_get_sampling_frequency_available(const char *device_dir, + struct iio_sampling_frequency_available *sfa) +{ + int err; + FILE *fp; + char *tmp_name, *pch, line[200]; + + sfa->num_available = 0; + + err = asprintf(&tmp_name, "%s/%s", device_dir, iio_sampling_frequency_available_filename); + if (err < 0) + return err; + + fp = fopen(tmp_name, "r"); + if (fp == NULL) { + ALOGE("Failed to open sampling frequency available file: \"%s\".", tmp_name); + err = -errno; + goto tmp_name_free; + } + + fgets(line, sizeof(line), fp); + if (line == NULL) { + ALOGE("Sampling frequency file format error: \"%s\".", tmp_name); + err = -EINVAL; + goto close_file; + } + + pch = strtok(line," ,."); + while (pch != NULL) { + sfa->hz[sfa->num_available] = atoi(pch); + pch = strtok(NULL, " ,."); + sfa->num_available++; + + if (sfa->num_available >= IIO_UTILS_MAX_SAMP_FREQ_AVAILABLE) + break; + } + +close_file: + fclose(fp); +tmp_name_free: + free(tmp_name); + return err < 0 ? err : 0; +} + +int iio_utils_build_channel_array(const char *device_dir, + struct iio_channel_info **ci_array, int *counter, bool read_offset) +{ + DIR *dp; + FILE *sysfsfp; + int count, i; + struct iio_channel_info *current; + int ret; + const struct dirent *ent; + char *scan_el_dir; + char *filename; + + *counter = 0; + ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, device_dir); + if (ret < 0) { + ret = -ENOMEM; + goto error_ret; + } + + dp = opendir(scan_el_dir); + if (dp == NULL) { + ret = -errno; + goto error_free_name; + } + + while (ent = readdir(dp), ent != NULL) + if (strcmp(ent->d_name + strlen(ent->d_name) + - strlen("_en"), "_en") == 0) { + ret = asprintf(&filename, + "%s/%s", scan_el_dir, ent->d_name); + if (ret < 0) { + ret = -ENOMEM; + goto error_close_dir; + } + + sysfsfp = fopen(filename, "r+"); + if (sysfsfp == NULL) { + ret = -errno; + free(filename); + goto error_close_dir; + } + + fprintf(sysfsfp, "%d", 1); + rewind(sysfsfp); + + fscanf(sysfsfp, "%u", &ret); + if (ret == 1) + (*counter)++; + + fclose(sysfsfp); + free(filename); + } + *ci_array = malloc(sizeof(**ci_array) * (*counter)); + if (*ci_array == NULL) { + ret = -ENOMEM; + goto error_close_dir; + } + + rewinddir(dp); + count = 0; + while (ent = readdir(dp), ent != NULL) { + if (strcmp(ent->d_name + strlen(ent->d_name) + - strlen("_en"), "_en") == 0) { + current = &(*ci_array)[count++]; + ret = asprintf(&filename, "%s/%s", scan_el_dir, ent->d_name); + if (ret < 0) { + ret = -ENOMEM; + /* decrement count to avoid freeing name */ + count--; + goto error_cleanup_array; + } + + sysfsfp = fopen(filename, "r"); + if (sysfsfp == NULL) { + free(filename); + ret = -errno; + goto error_cleanup_array; + } + + fscanf(sysfsfp, "%u", ¤t->enabled); + fclose(sysfsfp); + + if (!current->enabled) { + free(filename); + count--; + continue; + } + + current->scale = 1.0f; + current->offset = 0.0f; + current->name = strndup(ent->d_name, + strlen(ent->d_name) - strlen("_en")); + if (current->name == NULL) { + free(filename); + ret = -ENOMEM; + goto error_cleanup_array; + } + + /* Get the generic and specific name elements */ + ret = iioutils_break_up_name(current->name, + ¤t->generic_name); + if (ret) { + free(filename); + goto error_cleanup_array; + } + + ret = asprintf(&filename, "%s/%s_index", + scan_el_dir, current->name); + if (ret < 0) { + free(filename); + ret = -ENOMEM; + goto error_cleanup_array; + } + + sysfsfp = fopen(filename, "r"); + fscanf(sysfsfp, "%u", ¤t->index); + fclose(sysfsfp); + free(filename); + + /* Find the scale */ + ret = iioutils_get_param_float(¤t->scale, "scale", + device_dir, current->name, current->generic_name); + if (ret < 0) + goto error_cleanup_array; + + if (read_offset) { + ret = iioutils_get_param_float(¤t->offset, + "offset", device_dir, current->name, + current->generic_name); + if (ret < 0) + goto error_cleanup_array; + } + + ret = iioutils_get_type(¤t->is_signed, + ¤t->bytes, + ¤t->bits_used, + ¤t->shift, + ¤t->mask, + ¤t->be, + device_dir, + current->name, + current->generic_name); + } + } + + closedir(dp); + /* reorder so that the array is in index order */ + bsort_channel_array_by_index(ci_array, *counter); + + return 1; + +error_cleanup_array: + for (i = count - 1; i >= 0; i--) + free((*ci_array)[i].name); + free(*ci_array); +error_close_dir: + closedir(dp); +error_free_name: + free(scan_el_dir); +error_ret: + return ret; +} + +int find_type_by_name(char *name, const char *type) +{ + char thisname[IIO_MAX_NAME_LENGTH]; + const struct dirent *ent; + int number, numstrlen; + char *filename; + FILE *nameFile; + DIR *dp; + + dp = opendir(iio_dir); + if (dp == NULL) + return -ENODEV; + + while (ent = readdir(dp), ent != NULL) { + if (strcmp(ent->d_name, ".") != 0 && + strcmp(ent->d_name, "..") != 0 && + strlen(ent->d_name) > strlen(type) && + strncmp(ent->d_name, type, strlen(type)) == 0) { + + numstrlen = sscanf(ent->d_name + strlen(type), "%d", &number); + + /* verify the next character is not a colon */ + if (strncmp(ent->d_name + strlen(type) + numstrlen, + ":", 1) != 0) { + filename = (char *)malloc(strlen(iio_dir) + + strlen(type) + + numstrlen + + 6); + if (filename == NULL) { + closedir(dp); + return -ENOMEM; + } + + sprintf(filename, "%s%s%d/name", iio_dir, + type, number); + nameFile = fopen(filename, "r"); + if (!nameFile) { + free(filename); + continue; + } + + free(filename); + fscanf(nameFile, "%s", thisname); + fclose(nameFile); + if (strcmp(name, thisname) == 0) { + closedir(dp); + return number; + } + } + } + } + closedir(dp); + + return -ENODEV; +} + +int iio_utils_get_devices_name(struct iio_device devices[], unsigned int max_list) +{ + unsigned int device_num = 0; + char thisname[IIO_MAX_NAME_LENGTH]; + const struct dirent *ent; + int number, numstrlen; + char *filename; + FILE *nameFile; + DIR *dp; + + dp = opendir(iio_dir); + if (dp == NULL) + return -ENODEV; + + while (ent = readdir(dp), ent != NULL) { + if (strcmp(ent->d_name, ".") != 0 && + strcmp(ent->d_name, "..") != 0 && + strlen(ent->d_name) > strlen("iio:device") && + strncmp(ent->d_name, "iio:device", strlen("iio:device")) == 0) { + + numstrlen = sscanf(ent->d_name + strlen("iio:device"), "%d", &number); + + /* verify the next character is not a colon */ + if (strncmp(ent->d_name + strlen("iio:device") + numstrlen, + ":", 1) != 0) { + filename = (char *)malloc(strlen(iio_dir) + + strlen("iio:device") + + numstrlen + + 6); + if (filename == NULL) { + closedir(dp); + return -ENOMEM; + } + + sprintf(filename, "%s%s%d/name", iio_dir, + "iio:device", number); + nameFile = fopen(filename, "r"); + if (!nameFile) { + free(filename); + continue; + } + + free(filename); + fscanf(nameFile, "%s", thisname); + fclose(nameFile); + + memcpy(devices[device_num].name, thisname, strlen(thisname)); + devices[device_num].name[strlen(thisname)] = '\0'; + + devices[device_num].dev_num = number; + device_num++; + + if (device_num >= max_list) { + closedir(dp); + return (int)device_num; + } + } + } + } + closedir(dp); + + return (int)device_num; +} + +inline int _write_sysfs_int(char *filename, char *basedir, int val, int verify) +{ + int ret = 0; + FILE *sysfsfp; + int test; + + char *temp = (char *)malloc(strlen(basedir) + strlen(filename) + 2); + if (temp == NULL) + return -ENOMEM; + + sprintf(temp, "%s/%s", basedir, filename); + sysfsfp = fopen(temp, "w"); + if (sysfsfp == NULL) { + ret = -errno; + goto error_free; + } + + fprintf(sysfsfp, "%d", val); + fclose(sysfsfp); + + if (verify) { + sysfsfp = fopen(temp, "r"); + if (sysfsfp == NULL) { + ret = -errno; + goto error_free; + } + fscanf(sysfsfp, "%d", &test); + fclose(sysfsfp); + + if (test != val) { + ALOGE("Failed to write \"%d\" to \"%s/%s\" file.", + val, basedir, filename); + ret = -1; + } + } + +error_free: + free(temp); + return ret; +} + +int write_sysfs_int(char *filename, char *basedir, int val) +{ + return _write_sysfs_int(filename, basedir, val, 0); +} + +int write_sysfs_int_and_verify(char *filename, char *basedir, int val) +{ + return _write_sysfs_int(filename, basedir, val, 1); +} + +inline int _write_sysfs_ulong_and_verify(char *filename, char *basedir, + unsigned long val, int verify) +{ + int ret = 0; + FILE *sysfsfp; + unsigned long test; + + char *temp = (char *)malloc(strlen(basedir) + strlen(filename) + 2); + if (temp == NULL) + return -ENOMEM; + + sprintf(temp, "%s/%s", basedir, filename); + sysfsfp = fopen(temp, "w"); + if (sysfsfp == NULL) { + ret = -errno; + goto error_free; + } + + fprintf(sysfsfp, "%lu", val); + fclose(sysfsfp); + + if (verify) { + sysfsfp = fopen(temp, "r"); + if (sysfsfp == NULL) { + ret = -errno; + goto error_free; + } + fscanf(sysfsfp, "%lu", &test); + fclose(sysfsfp); + + if (test != val) { + ALOGE("Failed to write \"%lu\" to \"%s/%s\" file.", + val, basedir, filename); + ret = -1; + } + } + +error_free: + free(temp); + return ret; +} + +int write_sysfs_ulong_and_verify(char *filename, char *basedir, unsigned long val) +{ + return _write_sysfs_ulong_and_verify(filename, basedir, val, 1); +} + +inline int _write_sysfs_float(char *filename, char *basedir, float val, int verify) +{ + int ret = 0; + FILE *sysfsfp; + float test; + + char *temp = (char *)malloc(strlen(basedir) + strlen(filename) + 2); + if (temp == NULL) + return -ENOMEM; + + sprintf(temp, "%s/%s", basedir, filename); + sysfsfp = fopen(temp, "w"); + if (sysfsfp == NULL) { + ret = -errno; + goto error_free; + } + + fprintf(sysfsfp, "%f", val); + fclose(sysfsfp); + + if (verify) { + sysfsfp = fopen(temp, "r"); + if (sysfsfp == NULL) { + ret = -errno; + goto error_free; + } + fscanf(sysfsfp, "%f", &test); + fclose(sysfsfp); + + if (test != val) { + ALOGE("Failed to write \"%f\" to \"%s/%s\" file.", + val, basedir, filename); + ret = -1; + } + } + +error_free: + free(temp); + return ret; +} + +int write_sysfs_float(char *filename, char *basedir, float val) +{ + return _write_sysfs_float(filename, basedir, val, 0); +} + +int write_sysfs_float_and_verify(char *filename, char *basedir, float val) +{ + return _write_sysfs_float(filename, basedir, val, 1); +} + +int _write_sysfs_string(char *filename, char *basedir, char *val, int verify) +{ + FILE *sysfsfp; + int ret = 0; + + char *temp = (char *)malloc(strlen(basedir) + strlen(filename) + 2); + if (temp == NULL) + return -ENOMEM; + + sprintf(temp, "%s/%s", basedir, filename); + sysfsfp = fopen(temp, "w"); + if (sysfsfp == NULL) { + ret = -EIO; + goto error_free; + } + + fprintf(sysfsfp, "%s", val); + fclose(sysfsfp); + if (verify) { + sysfsfp = fopen(temp, "r"); + if (sysfsfp == NULL) { + ret = -EIO; + goto error_free; + } + + fscanf(sysfsfp, "%s", temp); + fclose(sysfsfp); + if (strcmp(temp, val) != 0) + ret = -1; + } +error_free: + free(temp); + + return ret; +} + +int write_sysfs_string_and_verify(char *filename, char *basedir, char *val) +{ + return _write_sysfs_string(filename, basedir, val, 1); +} + +int write_sysfs_string(char *filename, char *basedir, char *val) +{ + return _write_sysfs_string(filename, basedir, val, 0); +} + +int read_sysfs_posint(char *filename, char *basedir) +{ + int ret; + FILE *sysfsfp; + + char *temp = (char *)malloc(strlen(basedir) + strlen(filename) + 2); + if (temp == NULL) + return -ENOMEM; + + sprintf(temp, "%s/%s", basedir, filename); + sysfsfp = fopen(temp, "r"); + if (sysfsfp == NULL) { + ret = -errno; + goto error_free; + } + + fscanf(sysfsfp, "%d\n", &ret); + fclose(sysfsfp); + +error_free: + free(temp); + return ret; +} + +int read_sysfs_float(char *filename, char *basedir, float *val) +{ + float ret = 0; + FILE *sysfsfp; + + char *temp = (char *)malloc(strlen(basedir) + strlen(filename) + 2); + if (temp == NULL) + return -ENOMEM; + + sprintf(temp, "%s/%s", basedir, filename); + sysfsfp = fopen(temp, "r"); + if (sysfsfp == NULL) { + ret = -errno; + goto error_free; + } + + fscanf(sysfsfp, "%f\n", val); + fclose(sysfsfp); + +error_free: + free(temp); + return ret; +} + +int read_sysfs_string(char *filename, char *basedir, char *str) +{ + int ret = 0; + FILE *sysfsfp; + + char *temp = (char *)malloc(strlen(basedir) + strlen(filename) + 2); + if (temp == NULL) + return -ENOMEM; + + sprintf(temp, "%s/%s", basedir, filename); + sysfsfp = fopen(temp, "r"); + if (sysfsfp == NULL) { + ret = -errno; + goto error_free; + } + + fscanf(sysfsfp, "%s\n", str); + fclose(sysfsfp); + +error_free: + free(temp); + return ret; +} + +int read_sysfs_byte(char *filename, char *basedir, uint8_t *data, size_t len) +{ + int ret = 0; + FILE *sysfsfp; + + char *temp = (char *)malloc(strlen(basedir) + strlen(filename) + 2); + if (temp == NULL) + return -ENOMEM; + + sprintf(temp, "%s/%s", basedir, filename); + sysfsfp = fopen(temp, "r"); + if (sysfsfp == NULL) { + ret = -errno; + goto error_free; + } + + ret = fread(data, len, 1, sysfsfp); + fclose(sysfsfp); + +error_free: + free(temp); + return ret <= 0 ? -errno : (int)len; +} + +int write_sysfs_byte(char *filename, char *basedir, uint8_t *data, size_t len) +{ + int ret = 0; + FILE *sysfsfp; + + char *temp = (char *)malloc(strlen(basedir) + strlen(filename) + 2); + if (temp == NULL) + return -ENOMEM; + + sprintf(temp, "%s/%s", basedir, filename); + sysfsfp = fopen(temp, "w"); + if (sysfsfp == NULL) { + ret = -errno; + goto error_free; + } + + ret = fwrite(data, len, 1, sysfsfp); + fclose(sysfsfp); + +error_free: + free(temp); + return ret <= 0 ? -errno : (int)len; +} diff --git a/libsensors_iio/src/iio_utils.h b/libsensors_iio/src/iio_utils.h new file mode 100644 index 0000000..bb0d7c3 --- /dev/null +++ b/libsensors_iio/src/iio_utils.h @@ -0,0 +1,146 @@ +/* IIO - useful set of util functionality + * + * Copyright (c) 2008 Jonathan Cameron + * Modified by Denis Ciocca + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 2 as published by + * the Free Software Foundation. + */ + +#ifndef IIO_UTILS +#define IIO_UTILS + +#include +#include +#include +#include +#include +#include +#include +#include + + +/* Made up value to limit allocation sizes */ +#define IIO_MAX_NAME_LENGTH (70) + +#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements" +#define FORMAT_TYPE_FILE "%s_type" + +#define IIO_DEFAULT_BUFFER_LEN (2) + +/** + * struct iio_channel_info - information about a given channel + * @name: channel name + * @generic_name: general name for channel type + * @scale: scale factor to be applied for conversion to si units + * @offset: offset to be applied for conversion to si units + * @index: the channel index in the buffer output + * @bytes: number of bytes occupied in buffer output + * @mask: a bit mask for the raw output + * @is_signed: is the raw value stored signed + * @enabled: is this channel enabled + **/ +struct iio_channel_info { + char *name; + char *generic_name; + float scale; + float offset; + unsigned index; + unsigned bytes; + unsigned bits_used; + unsigned shift; + uint64_t mask; + unsigned be; + unsigned is_signed; + unsigned enabled; + unsigned location; + bool isfloat; + bool multi_data; +}; + + +/** + * build_channel_array() - function to figure out what channels are present + * @device_dir: the IIO device directory in sysfs + * @ + **/ +int iio_utils_build_channel_array(const char *device_dir, + struct iio_channel_info **ci_array, int *counter, bool read_offset); + +/** + * find_type_by_name() - function to match top level types by name + * @name: top level type instance name + * @type: the type of top level instance being sort + * + * Typical types this is used for are device and trigger. + **/ +int find_type_by_name(char *name, const char *type); + +int write_sysfs_int(char *filename, char *basedir, int val); + +int write_sysfs_int_and_verify(char *filename, char *basedir, int val); + +int write_sysfs_ulong_and_verify(char *filename, char *basedir, unsigned long val); + +int write_sysfs_float(char *filename, char *basedir, float val); + +int write_sysfs_float_and_verify(char *filename, char *basedir, float val); + + + + + +#define IIO_UTILS_MAX_SAMP_FREQ_AVAILABLE (10) +#define IIO_UTILS_SCALE_AVAILABLE (10) + +struct iio_scale_available { + float values[IIO_UTILS_SCALE_AVAILABLE]; + unsigned int num_available; +}; + +struct iio_sampling_frequency_available { + unsigned int hz[IIO_UTILS_MAX_SAMP_FREQ_AVAILABLE]; + unsigned int num_available; +}; + +struct iio_device { + unsigned int dev_num; + char name[IIO_MAX_NAME_LENGTH]; +}; + +int iio_utils_get_devices_name(struct iio_device devices[], unsigned int max_list); + +int iio_utils_get_sampling_frequency_available(const char *device_dir, + struct iio_sampling_frequency_available *sfa); + +int iio_utils_get_scale_available(const char *device_dir, struct iio_scale_available *sa, int device_type); + +int iio_utils_set_scale(const char *device_dir, float value, int device_type); + +int iio_utils_get_hw_fifo_lenght(const char *device_dir); + + +int iio_utils_enable_sensor(const char *device_dir, bool enable); + +/** + * write_sysfs_string_and_verify() - string write, readback and verify + * @filename: name of file to write to + * @basedir: the sysfs directory in which the file is to be found + * @val: the string to write + **/ +int write_sysfs_string_and_verify(char *filename, char *basedir, char *val); + +int write_sysfs_string(char *filename, char *basedir, char *val); + +int read_sysfs_posint(char *filename, char *basedir); + +int read_sysfs_float(char *filename, char *basedir, float *val); + +int read_sysfs_string(char *filename, char *basedir, char *str); + +int read_sysfs_byte(char *filename, char *basedir, uint8_t *data, size_t len); + +int write_sysfs_byte(char *filename, char *basedir, uint8_t *data, size_t len); + +#endif /* IIO_UTILS */ diff --git a/libsensors_iio/src/types.h b/libsensors_iio/src/types.h new file mode 100644 index 0000000..1ab3a95 --- /dev/null +++ b/libsensors_iio/src/types.h @@ -0,0 +1,65 @@ +/* industrial I/O data types needed both in and out of kernel + * + * Copyright (c) 2008 Jonathan Cameron + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 2 as published by + * the Free Software Foundation. + */ + +#ifndef _IIO_TYPES_H_ +#define _IIO_TYPES_H_ + +enum iio_chan_type { + IIO_VOLTAGE, + IIO_CURRENT, + IIO_POWER, + IIO_ACCEL, + IIO_ANGL_VEL, + IIO_MAGN, + IIO_LIGHT, + IIO_INTENSITY, + IIO_PROXIMITY, + IIO_TEMP, + IIO_INCLI, + IIO_ROT, + IIO_ANGL, + IIO_TIMESTAMP, + IIO_CAPACITANCE, + IIO_ALTVOLTAGE, + IIO_CCT, + IIO_PRESSURE, + IIO_SIGN_MOTION, +}; + +enum iio_modifier { + IIO_NO_MOD, + IIO_MOD_X, + IIO_MOD_Y, + IIO_MOD_Z, + IIO_MOD_X_AND_Y, + IIO_MOD_X_AND_Z, + IIO_MOD_Y_AND_Z, + IIO_MOD_X_AND_Y_AND_Z, + IIO_MOD_X_OR_Y, + IIO_MOD_X_OR_Z, + IIO_MOD_Y_OR_Z, + IIO_MOD_X_OR_Y_OR_Z, + IIO_MOD_LIGHT_BOTH, + IIO_MOD_LIGHT_IR, + IIO_MOD_ROOT_SUM_SQUARED_X_Y, + IIO_MOD_SUM_SQUARED_X_Y_Z, + IIO_MOD_LIGHT_CLEAR, + IIO_MOD_LIGHT_RED, + IIO_MOD_LIGHT_GREEN, + IIO_MOD_LIGHT_BLUE, +}; + +#define IIO_VAL_INT 1 +#define IIO_VAL_INT_PLUS_MICRO 2 +#define IIO_VAL_INT_PLUS_NANO 3 +#define IIO_VAL_INT_PLUS_MICRO_DB 4 +#define IIO_VAL_FRACTIONAL 10 +#define IIO_VAL_FRACTIONAL_LOG2 11 + +#endif /* _IIO_TYPES_H_ */ diff --git a/libsensors_iio/tools/cfgdefine.c b/libsensors_iio/tools/cfgdefine.c new file mode 100644 index 0000000..5fe5b14 --- /dev/null +++ b/libsensors_iio/tools/cfgdefine.c @@ -0,0 +1,222 @@ +/**************************************************************************** + * tools/cfgdefine.c + * + * Copyright (C) 2007-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Modified by Denis Ciocca + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include "cfgdefine.h" +#include + +#define CUTOM_MACRO_LIST 6 + +char line[LINESIZE + 1]; + +static struct custom_macro { + char name[45]; +} custom_macro[] = { + { .name = "CONFIG_ST_HAL_INEMO_GBIAS_THRESOLD_ACCEL" }, + { .name = "CONFIG_ST_HAL_INEMO_GBIAS_THRESOLD_MAGN" }, + { .name = "CONFIG_ST_HAL_INEMO_GBIAS_THRESOLD_GYRO" }, + { .name = "CONFIG_ST_HAL_ACCEL_ROT_MATRIX" }, + { .name = "CONFIG_ST_HAL_MAGN_ROT_MATRIX" }, + { .name = "CONFIG_ST_HAL_GYRO_ROT_MATRIX" }, +}; + +/* Skip over any spaces */ +static char *skip_space(char *ptr) +{ + while (*ptr && isspace((int)*ptr)) ptr++; + + return ptr; +} + +/* Find the end of a variable string */ +static char *find_name_end(char *ptr) +{ + while (*ptr && (isalnum((int)*ptr) || *ptr == '_')) ptr++; + + return ptr; +} + +/* Find the end of a value string */ +static char *find_value_end(char *ptr) +{ + while (*ptr && !isspace((int)*ptr)) { + if (*ptr == '"') { + do ptr++; while (*ptr && *ptr != '"'); + if (*ptr) ptr++; + } else + do ptr++; while (*ptr && !isspace((int)*ptr) && *ptr != '"'); + + } + + return ptr; +} + +/* Read the next line from the configuration file */ +static char *read_line(FILE *stream) +{ + char *ptr; + + for (;;) { + line[LINESIZE] = '\0'; + + if (!fgets(line, LINESIZE, stream)) + return NULL; + else { + ptr = skip_space(line); + if (*ptr && *ptr != '#' && *ptr != '\n') + return ptr; + else if ((*ptr == '#') && (*(ptr+1) == '\n')) + printf("\n"); + } + } + + return NULL; +} + +static void dequote_custom_values(char *var, char *value) +{ + int i, n, ret; + char *t1; + + if (!var) + return; + + for (i = 0; i < CUTOM_MACRO_LIST; i++) { + ret = strncmp(custom_macro[i].name, var, strlen(custom_macro[i].name)); + if (ret == 0) { + for (n = 1; n < strlen(value) - 1; n++) + value[n - 1] = value[n]; + + value[n - 1] = '\0'; + return; + } + } + + return; +} + +/* Parse the line from the configuration file into a variable name + * string and a value string. + */ +static void parse_line(char *ptr, char **varname, char **varval) +{ + /* Skip over any leading spaces */ + ptr = skip_space(ptr); + + /* The first no-space is the beginning of the variable name */ + *varname = skip_space(ptr); + *varval = NULL; + + /* Parse to the end of the variable name */ + ptr = find_name_end(ptr); + + /* An equal sign is expected next, perhaps after some white space */ + if (*ptr && *ptr != '=') { + /* Some else follows the variable name. Terminate the variable + * name and skip over any spaces. + */ + + *ptr = '\0'; + ptr = skip_space(ptr + 1); + } + + /* Verify that the equal sign is present */ + if (*ptr == '=') { + /* Make sure that the variable name is terminated (this was already + * done if the name was followed by white space. + */ + + *ptr = '\0'; + + /* The variable value should follow =, perhaps separated by some + * white space. + */ + + ptr = skip_space(ptr + 1); + if (*ptr) { + /* Yes.. a variable follows. Save the pointer to the start + * of the variable string. + */ + + *varval = ptr; + + /* Find the end of the variable string and make sure that it + * is terminated. + */ + + ptr = find_value_end(ptr); + *ptr = '\0'; + } + } +} + +void generate_definitions(FILE *stream) +{ + char *varname; + char *varval; + char *ptr; + + /* Loop until the entire file has been parsed. */ + do { + /* Read the next line from the file */ + ptr = read_line(stream); + if (ptr) { + /* Parse the line into a variable and a value field */ + parse_line(ptr, &varname, &varval); + + dequote_custom_values(varname, varval); + + /* Was a variable name found? */ + if (varname) { + /* If no value was provided or if the special value 'n' was provided, + * then undefine the configuration variable. + */ + + if (!varval || strcmp(varval, "n") == 0) + printf("#undef %s\n", varname); + else if (strcmp(varval, "y") == 0) + printf("#define %s 1\n", varname); + else + printf("#define %s %s\n", varname, varval); + } + } + } while (ptr); +} diff --git a/libsensors_iio/tools/cfgdefine.h b/libsensors_iio/tools/cfgdefine.h new file mode 100644 index 0000000..fd7d9a2 --- /dev/null +++ b/libsensors_iio/tools/cfgdefine.h @@ -0,0 +1,50 @@ +/**************************************************************************** + * tools/cfgdefine.h + * + * Copyright (C) 2007-2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Modified by Denis Ciocca + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef __TOOLS_CFGDEFINE_H +#define __TOOLS_CFGDEFINE_H + +#include +#include + +#define LINESIZE ( PATH_MAX > 256 ? PATH_MAX : 256 ) + +extern char line[LINESIZE+1]; + +void generate_definitions(FILE *stream); + +#endif /* __TOOLS_CFGDEFINE_H */ diff --git a/libsensors_iio/tools/kconfig-language.txt b/libsensors_iio/tools/kconfig-language.txt new file mode 100644 index 0000000..350f733 --- /dev/null +++ b/libsensors_iio/tools/kconfig-language.txt @@ -0,0 +1,395 @@ +Introduction +------------ + +The configuration database is a collection of configuration options +organized in a tree structure: + + +- Code maturity level options + | +- Prompt for development and/or incomplete code/drivers + +- General setup + | +- Networking support + | +- System V IPC + | +- BSD Process Accounting + | +- Sysctl support + +- Loadable module support + | +- Enable loadable module support + | +- Set version information on all module symbols + | +- Kernel module loader + +- ... + +Every entry has its own dependencies. These dependencies are used +to determine the visibility of an entry. Any child entry is only +visible if its parent entry is also visible. + +Menu entries +------------ + +Most entries define a config option; all other entries help to organize +them. A single configuration option is defined like this: + +config MODVERSIONS + bool "Set version information on all module symbols" + depends on MODULES + help + Usually, modules have to be recompiled whenever you switch to a new + kernel. ... + +Every line starts with a key word and can be followed by multiple +arguments. "config" starts a new config entry. The following lines +define attributes for this config option. Attributes can be the type of +the config option, input prompt, dependencies, help text and default +values. A config option can be defined multiple times with the same +name, but every definition can have only a single input prompt and the +type must not conflict. + +Menu attributes +--------------- + +A menu entry can have a number of attributes. Not all of them are +applicable everywhere (see syntax). + +- type definition: "bool"/"tristate"/"string"/"hex"/"int" + Every config option must have a type. There are only two basic types: + tristate and string; the other types are based on these two. The type + definition optionally accepts an input prompt, so these two examples + are equivalent: + + bool "Networking support" + and + bool + prompt "Networking support" + +- input prompt: "prompt" ["if" ] + Every menu entry can have at most one prompt, which is used to display + to the user. Optionally dependencies only for this prompt can be added + with "if". + +- default value: "default" ["if" ] + A config option can have any number of default values. If multiple + default values are visible, only the first defined one is active. + Default values are not limited to the menu entry where they are + defined. This means the default can be defined somewhere else or be + overridden by an earlier definition. + The default value is only assigned to the config symbol if no other + value was set by the user (via the input prompt above). If an input + prompt is visible the default value is presented to the user and can + be overridden by him. + Optionally, dependencies only for this default value can be added with + "if". + +- type definition + default value: + "def_bool"/"def_tristate" ["if" ] + This is a shorthand notation for a type definition plus a value. + Optionally dependencies for this default value can be added with "if". + +- dependencies: "depends on" + This defines a dependency for this menu entry. If multiple + dependencies are defined, they are connected with '&&'. Dependencies + are applied to all other options within this menu entry (which also + accept an "if" expression), so these two examples are equivalent: + + bool "foo" if BAR + default y if BAR + and + depends on BAR + bool "foo" + default y + +- reverse dependencies: "select" ["if" ] + While normal dependencies reduce the upper limit of a symbol (see + below), reverse dependencies can be used to force a lower limit of + another symbol. The value of the current menu symbol is used as the + minimal value can be set to. If is selected multiple + times, the limit is set to the largest selection. + Reverse dependencies can only be used with boolean or tristate + symbols. + Note: + select should be used with care. select will force + a symbol to a value without visiting the dependencies. + By abusing select you are able to select a symbol FOO even + if FOO depends on BAR that is not set. + In general use select only for non-visible symbols + (no prompts anywhere) and for symbols with no dependencies. + That will limit the usefulness but on the other hand avoid + the illegal configurations all over. + +- limiting menu display: "visible if" + This attribute is only applicable to menu blocks, if the condition is + false, the menu block is not displayed to the user (the symbols + contained there can still be selected by other symbols, though). It is + similar to a conditional "prompt" attribute for individual menu + entries. Default value of "visible" is true. + +- numerical ranges: "range" ["if" ] + This allows to limit the range of possible input values for int + and hex symbols. The user can only input a value which is larger than + or equal to the first symbol and smaller than or equal to the second + symbol. + +- help text: "help" or "---help---" + This defines a help text. The end of the help text is determined by + the indentation level, this means it ends at the first line which has + a smaller indentation than the first line of the help text. + "---help---" and "help" do not differ in behaviour, "---help---" is + used to help visually separate configuration logic from help within + the file as an aid to developers. + +- misc options: "option" [=] + Various less common options can be defined via this option syntax, + which can modify the behaviour of the menu entry and its config + symbol. These options are currently possible: + + - "defconfig_list" + This declares a list of default entries which can be used when + looking for the default configuration (which is used when the main + .config doesn't exists yet.) + + - "modules" + This declares the symbol to be used as the MODULES symbol, which + enables the third modular state for all config symbols. + At most one symbol may have the "modules" option set. + + - "env"= + This imports the environment variable into Kconfig. It behaves like + a default, except that the value comes from the environment, this + also means that the behaviour when mixing it with normal defaults is + undefined at this point. The symbol is currently not exported back + to the build environment (if this is desired, it can be done via + another symbol). + + - "allnoconfig_y" + This declares the symbol as one that should have the value y when + using "allnoconfig". Used for symbols that hide other symbols. + +Menu dependencies +----------------- + +Dependencies define the visibility of a menu entry and can also reduce +the input range of tristate symbols. The tristate logic used in the +expressions uses one more state than normal boolean logic to express the +module state. Dependency expressions have the following syntax: + + ::= (1) + '=' (2) + '!=' (3) + '(' ')' (4) + '!' (5) + '&&' (6) + '||' (7) + +Expressions are listed in decreasing order of precedence. + +(1) Convert the symbol into an expression. Boolean and tristate symbols + are simply converted into the respective expression values. All + other symbol types result in 'n'. +(2) If the values of both symbols are equal, it returns 'y', + otherwise 'n'. +(3) If the values of both symbols are equal, it returns 'n', + otherwise 'y'. +(4) Returns the value of the expression. Used to override precedence. +(5) Returns the result of (2-/expr/). +(6) Returns the result of min(/expr/, /expr/). +(7) Returns the result of max(/expr/, /expr/). + +An expression can have a value of 'n', 'm' or 'y' (or 0, 1, 2 +respectively for calculations). A menu entry becomes visible when its +expression evaluates to 'm' or 'y'. + +There are two types of symbols: constant and non-constant symbols. +Non-constant symbols are the most common ones and are defined with the +'config' statement. Non-constant symbols consist entirely of alphanumeric +characters or underscores. +Constant symbols are only part of expressions. Constant symbols are +always surrounded by single or double quotes. Within the quote, any +other character is allowed and the quotes can be escaped using '\'. + +Menu structure +-------------- + +The position of a menu entry in the tree is determined in two ways. First +it can be specified explicitly: + +menu "Network device support" + depends on NET + +config NETDEVICES + ... + +endmenu + +All entries within the "menu" ... "endmenu" block become a submenu of +"Network device support". All subentries inherit the dependencies from +the menu entry, e.g. this means the dependency "NET" is added to the +dependency list of the config option NETDEVICES. + +The other way to generate the menu structure is done by analyzing the +dependencies. If a menu entry somehow depends on the previous entry, it +can be made a submenu of it. First, the previous (parent) symbol must +be part of the dependency list and then one of these two conditions +must be true: +- the child entry must become invisible, if the parent is set to 'n' +- the child entry must only be visible, if the parent is visible + +config MODULES + bool "Enable loadable module support" + +config MODVERSIONS + bool "Set version information on all module symbols" + depends on MODULES + +comment "module support disabled" + depends on !MODULES + +MODVERSIONS directly depends on MODULES, this means it's only visible if +MODULES is different from 'n'. The comment on the other hand is always +visible when MODULES is visible (the (empty) dependency of MODULES is +also part of the comment dependencies). + + +Kconfig syntax +-------------- + +The configuration file describes a series of menu entries, where every +line starts with a keyword (except help texts). The following keywords +end a menu entry: +- config +- menuconfig +- choice/endchoice +- comment +- menu/endmenu +- if/endif +- source +The first five also start the definition of a menu entry. + +config: + + "config" + + +This defines a config symbol and accepts any of above +attributes as options. + +menuconfig: + "menuconfig" + + +This is similar to the simple config entry above, but it also gives a +hint to front ends, that all suboptions should be displayed as a +separate list of options. + +choices: + + "choice" [symbol] + + + "endchoice" + +This defines a choice group and accepts any of the above attributes as +options. A choice can only be of type bool or tristate, while a boolean +choice only allows a single config entry to be selected, a tristate +choice also allows any number of config entries to be set to 'm'. This +can be used if multiple drivers for a single hardware exists and only a +single driver can be compiled/loaded into the kernel, but all drivers +can be compiled as modules. +A choice accepts another option "optional", which allows to set the +choice to 'n' and no entry needs to be selected. +If no [symbol] is associated with a choice, then you can not have multiple +definitions of that choice. If a [symbol] is associated to the choice, +then you may define the same choice (ie. with the same entries) in another +place. + +comment: + + "comment" + + +This defines a comment which is displayed to the user during the +configuration process and is also echoed to the output files. The only +possible options are dependencies. + +menu: + + "menu" + + + "endmenu" + +This defines a menu block, see "Menu structure" above for more +information. The only possible options are dependencies and "visible" +attributes. + +if: + + "if" + + "endif" + +This defines an if block. The dependency expression is appended +to all enclosed menu entries. + +source: + + "source" + +This reads the specified configuration file. This file is always parsed. + +mainmenu: + + "mainmenu" + +This sets the config program's title bar if the config program chooses +to use it. It should be placed at the top of the configuration, before any +other statement. + + +Kconfig hints +------------- +This is a collection of Kconfig tips, most of which aren't obvious at +first glance and most of which have become idioms in several Kconfig +files. + +Adding common features and make the usage configurable +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +It is a common idiom to implement a feature/functionality that are +relevant for some architectures but not all. +The recommended way to do so is to use a config variable named HAVE_* +that is defined in a common Kconfig file and selected by the relevant +architectures. +An example is the generic IOMAP functionality. + +We would in lib/Kconfig see: + +# Generic IOMAP is used to ... +config HAVE_GENERIC_IOMAP + +config GENERIC_IOMAP + depends on HAVE_GENERIC_IOMAP && FOO + +And in lib/Makefile we would see: +obj-$(CONFIG_GENERIC_IOMAP) += iomap.o + +For each architecture using the generic IOMAP functionality we would see: + +config X86 + select ... + select HAVE_GENERIC_IOMAP + select ... + +Note: we use the existing config option and avoid creating a new +config variable to select HAVE_GENERIC_IOMAP. + +Note: the use of the internal config variable HAVE_GENERIC_IOMAP, it is +introduced to overcome the limitation of select which will force a +config option to 'y' no matter the dependencies. +The dependencies are moved to the symbol GENERIC_IOMAP and we avoid the +situation where select forces a symbol equals to 'y'. + +Build as module only +~~~~~~~~~~~~~~~~~~~~ +To restrict a component build to module-only, qualify its config symbol +with "depends on m". E.g.: + +config FOO + depends on BAR && m + +limits FOO to module (=m) or disabled (=n). diff --git a/libsensors_iio/tools/kconfig-mconf b/libsensors_iio/tools/kconfig-mconf new file mode 100755 index 0000000..3f84ffb Binary files /dev/null and b/libsensors_iio/tools/kconfig-mconf differ diff --git a/libsensors_iio/tools/mkconfig b/libsensors_iio/tools/mkconfig new file mode 100755 index 0000000..c2b1af1 Binary files /dev/null and b/libsensors_iio/tools/mkconfig differ diff --git a/libsensors_iio/tools/mkconfig.c b/libsensors_iio/tools/mkconfig.c new file mode 100644 index 0000000..8655711 --- /dev/null +++ b/libsensors_iio/tools/mkconfig.c @@ -0,0 +1,90 @@ +/**************************************************************************** + * tools/mkconfig.c + * + * Copyright (C) 2007-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Modified by Denis Ciocca + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +#include "cfgdefine.h" + +#define DEFCONFIG ".config" + + static inline char *getfilepath(const char *name) +{ + snprintf(line, PATH_MAX, "%s/" DEFCONFIG, name); + line[PATH_MAX] = '\0'; + return strdup(line); +} + +static void show_usage(const char *progname) +{ + fprintf(stderr, "USAGE: %s \n", progname); + exit(1); +} + +int main(int argc, char **argv, char **envp) +{ + char *filepath; + FILE *stream; + + if (argc != 2) { + fprintf(stderr, "Unexpected number of arguments\n"); + show_usage(argv[0]); + } + + filepath = getfilepath(argv[1]); + if (!filepath) { + fprintf(stderr, "getfilepath failed\n"); + exit(2); + } + + stream = fopen(filepath, "r"); + if (!stream) { + fprintf(stderr, "open %s failed: %s\n", filepath, strerror(errno)); + exit(3); + } + + printf("/* configuration.h -- Autogenerated file! Do not edit. */\n\n"); + printf("#ifndef __ST_SENSOR_HAL_CONFIG_H\n"); + printf("#define __ST_SENSOR_HAL_CONFIG_H\n\n"); + generate_definitions(stream); + printf("\n"); + printf("#endif /* __ST_SENSOR_HAL_CONFIG_H */\n"); + fclose(stream); + + return 0; +} -- cgit v1.2.3