mawei/agnos-kernel-sdm845
1
0
Fork 0
mirror of https://github.com/commaai/agnos-kernel-sdm845.git synced 2026-06-08 11:24:51 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge "kernel: Bring up ICM42600" into robotics-x-lu-16.04-sda845-p1-dev

Browse Source
This commit is contained in:
李政春
2019-01-14 17:41:50 +08:00
committed by Gerrit Code Review
parent 540a0195fc ea57e90140
commit 1ab4fc966f
27 changed files with 9626 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
arch/arm64/boot/dts/qcom/sdm845-mtp.dtsi
View File

@@ -460,6 +460,33 @@
status = "ok";
};
&qupv3_se2_spi {
status = "ok";
invensense_icm@0 {
compatible = "invensense,icm42600";
reg = <0>;
spi-max-frequency = <8000000>;
interrupt-parent = <&tlmm>;
interrupts = <10 0>;
interrupt-names = "inv_irq";
axis_map_x = <1>;
axis_map_y = <0>;
axis_map_z = <2>;
negate_x = <1>;
negate_y = <0>;
negate_z = <0>;
inven,secondary_type = "none";
invenux_type = "none";
inven,read_only_slave_type = "none";
//pinctrl-names = "inv_active","inv_suspend";
//pinctrl-0 = <&inv_int_active>;
//pinctrl-1 = <&inv_int_suspend>;
status= "ok";
};
};
&usb1 {
status = "okay";
extcon = <&extcon_usb1>;
@@ -700,7 +727,7 @@
lt,video-format-id = <4>;
lt,mipi-lanes-type = <0>;//4 lane
lt,mipi-ports-type = <0>;//single port
};
};

3
arch/arm64/configs/sdm845_defconfig
View File

@@ -740,3 +740,6 @@ CONFIG_JOYSTICK_XPAD_LEDS=y
CONFIG_USB_NET_QMI_WWAN=y
CONFIG_USB_WDM=y
CONFIG_USB_NET_EM7565=y
CONFIG_INV_MPU_IIO=y
CONFIG_INV_MPU_IIO_ICM42600=y
CONFIG_INV_MPU_IIO_SPI=y

1
drivers/iio/imu/Kconfig
View File

@@ -40,6 +40,7 @@ config KMX61
source "drivers/iio/imu/inv_mpu6050/Kconfig"
source "drivers/iio/imu/inv_icm20602/Kconfig"
source "drivers/iio/imu/inv_mpu/Kconfig"
endmenu

1
drivers/iio/imu/Makefile
View File

@@ -16,5 +16,6 @@ obj-$(CONFIG_IIO_ADIS_LIB) += adis_lib.o
obj-y += bmi160/
obj-y += inv_mpu6050/
obj-y += inv_icm20602/
obj-y += inv_mpu/
obj-$(CONFIG_KMX61) += kmx61.o

65
drivers/iio/imu/inv_mpu/Kconfig Normal file
View File

@@ -0,0 +1,65 @@
#
# inv-mpu-iio driver for Invensense MPU devices
#
config INV_MPU_IIO
tristate
select IIO_BUFFER
select IIO_TRIGGERED_BUFFER
select CRC32
choice
prompt "Chip name"
depends on INV_MPU_IIO
config INV_MPU_IIO_ICM20648
bool "ICM20648/ICM20948"
help
Select this if you are using a ICM20648/ICM20948 chip.
config INV_MPU_IIO_ICM20608D
bool "ICM20608D/ICM206x9/ICM20789"
help
Select this if you are using a ICM20608D/ICM206x9/ICM20789 chip.
config INV_MPU_IIO_ICM20602
bool "ICM20602"
help
Select this if you are using a ICM20602 chip.
config INV_MPU_IIO_ICM20690
bool "ICM20690"
help
Select this if you are using a ICM20690 chip.
config INV_MPU_IIO_IAM20680
bool "IAM20680"
help
Select this if you are using a IAM20680 chip.
config INV_MPU_IIO_ICM42600
bool "ICM42600"
help
Select this if you are using a ICM42600 chip.
endchoice
config INV_MPU_IIO_I2C
tristate "Invensense ICM20xxx devices (I2C)"
depends on I2C && !INV_MPU6050_IIO
select INV_MPU_IIO
help
This driver supports Invensense ICM20xxx devices over I2C.
This driver can be built as a module. The module will be called
inv-mpu-iio-i2c.
config INV_MPU_IIO_SPI
tristate "Invensense ICM20xxx devices (SPI)"
depends on SPI_MASTER && !INV_MPU6050_IIO
select INV_MPU_IIO
help
This driver supports Invensense ICM20xxx devices over SPI.
This driver can be built as a module. The module will be called
inv-mpu-iio-spi.
source "drivers/iio/imu/inv_mpu/inv_test/Kconfig"

68
drivers/iio/imu/inv_mpu/Makefile Normal file
View File

@@ -0,0 +1,68 @@
#
# Makefile for Invensense inv-mpu-iio device.
#
obj-$(CONFIG_INV_MPU_IIO) += inv-mpu-iio.o
inv-mpu-iio-objs += inv_mpu_common.o
inv-mpu-iio-objs += inv_mpu_ring.o
inv-mpu-iio-objs += inv_mpu_timestamp.o
inv-mpu-iio-objs += inv_mpu_dts.o
# chip support
ifeq ($(CONFIG_INV_MPU_IIO_ICM20648), y)
inv-mpu-iio-objs += icm20648/inv_mpu_init.o
inv-mpu-iio-objs += icm20648/inv_mpu_core.o
inv-mpu-iio-objs += icm20648/inv_mpu_parsing.o
inv-mpu-iio-objs += icm20648/inv_mpu_setup.o
inv-mpu-iio-objs += icm20648/inv_mpu_dmp_fifo.o
inv-mpu-iio-objs += icm20648/inv_slave_compass.o
inv-mpu-iio-objs += icm20648/inv_slave_pressure.o
inv-mpu-iio-objs += icm20648/inv_slave_als.o
inv-mpu-iio-objs += icm20648/inv_mpu_load_dmp.o
inv-mpu-iio-objs += icm20648/inv_mpu_selftest.o
inv-mpu-iio-objs += dmp_support/inv_mpu_misc.o
else ifeq ($(CONFIG_INV_MPU_IIO_ICM20690), y)
inv-mpu-iio-objs += icm20690/inv_mpu_init_20690.o
inv-mpu-iio-objs += icm20690/inv_mpu_core_20690.o
inv-mpu-iio-objs += icm20690/inv_mpu_parsing_20690.o
inv-mpu-iio-objs += icm20690/inv_mpu_setup_20690.o
inv-mpu-iio-objs += icm20690/inv_mpu_selftest_20690.o
inv-mpu-iio-objs += icm20690/inv_slave_compass.o
else ifeq ($(CONFIG_INV_MPU_IIO_ICM20602), y)
inv-mpu-iio-objs += icm20602/inv_mpu_init_20602.o
inv-mpu-iio-objs += icm20602/inv_mpu_core_20602.o
inv-mpu-iio-objs += icm20602/inv_mpu_parsing_20602.o
inv-mpu-iio-objs += icm20602/inv_mpu_setup_20602.o
inv-mpu-iio-objs += icm20602/inv_mpu_selftest_20602.o
else ifeq ($(CONFIG_INV_MPU_IIO_ICM20608D), y)
inv-mpu-iio-objs += icm20608d/inv_mpu_init_20608.o
inv-mpu-iio-objs += icm20608d/inv_mpu_core_20608.o
inv-mpu-iio-objs += icm20608d/inv_mpu_parsing_20608.o
inv-mpu-iio-objs += icm20608d/inv_mpu_setup_20608D.o
inv-mpu-iio-objs += icm20608d/inv_mpu_dmp_fifo.o
inv-mpu-iio-objs += icm20608d/inv_mpu_load_dmp.o
inv-mpu-iio-objs += icm20608d/inv_mpu_selftest_20608.o
inv-mpu-iio-objs += dmp_support/inv_mpu_misc.o
else ifeq ($(CONFIG_INV_MPU_IIO_IAM20680), y)
inv-mpu-iio-objs += iam20680/inv_mpu_init_20680.o
inv-mpu-iio-objs += iam20680/inv_mpu_core_20680.o
inv-mpu-iio-objs += iam20680/inv_mpu_parsing_20680.o
inv-mpu-iio-objs += iam20680/inv_mpu_setup_20680.o
inv-mpu-iio-objs += iam20680/inv_mpu_selftest_20680.o
else ifeq ($(CONFIG_INV_MPU_IIO_ICM42600), y)
inv-mpu-iio-objs += icm42600/inv_mpu_init_42600.o
inv-mpu-iio-objs += icm42600/inv_mpu_core_42600.o
inv-mpu-iio-objs += icm42600/inv_mpu_parsing_42600.o
inv-mpu-iio-objs += icm42600/inv_mpu_setup_42600.o
inv-mpu-iio-objs += icm42600/inv_mpu_misc_42600.o
inv-mpu-iio-objs += icm42600/inv_mpu_selftest_42600.o
endif
# Bus support
obj-$(CONFIG_INV_MPU_IIO_I2C) += inv-mpu-iio-i2c.o
inv-mpu-iio-i2c-objs := inv_mpu_i2c.o
obj-$(CONFIG_INV_MPU_IIO_SPI) += inv-mpu-iio-spi.o
inv-mpu-iio-spi-objs := inv_mpu_spi.o
obj-y += inv_test/

104
drivers/iio/imu/inv_mpu/README Normal file
View File

@@ -0,0 +1,104 @@
Kernel driver inv-mpu-iio
Author: InvenSense, Inc.
Table of Contents:
==================
- Description
- Integrating the Driver in the Linux Kernel
- Dts file
- Communicating with the Driver in Userspace
Description
===========
This document describes how to install the Invensense device driver into a
Linux kernel. The supported chips are listed in Kconfig and user selects an
appropriate one from .e.g. menuconfig.
Integrating the Driver in the Linux Kernel
==========================================
Please add the files as follows (kernel 3.10):
- Copy mpu.h to <kernel_root>/include/linux/iio/imu/
- Copy inv_mpu folder under <kernel_root>/drivers/iio/imu/
In order to see the driver in menuconfig when building the kernel, please
make modifications as shown below:
add "source "drivers/iio/imu/inv_mpu/Kconfig""
in <kernel_root>/drivers/iio/imu/Kconfig
add "obj-y += inv_mpu/"
in <kernel_root>/drivers/iio/imu/Makefile
Dts file
=======================
In order to recognize the Invensense device on the I2C/SPI bus, dts(or dtsi)
file must be modified.
Example)
ICM20648 + AK09911/BMP280/APDS9930 on AUX I2C
i2c@f9968000 {
/* Invensense */
mpu6515_acc@68 {
compatible = "inven,icm20648";
reg = <0x68>;
interrupt-parent = <&msmgpio>;
interrupts = <73 0x2>;
inven,vdd_ana-supply = <&pm8941_l17>;
inven,vcc_i2c-supply = <&pm8941_lvs1>;
inven,gpio_int1 = <&msmgpio 73 0x00>;
fs_range = <0x00>;
/* mount matrix */
axis_map_x = <1>;
axis_map_y = <0>;
axis_map_z = <2>;
negate_x = <0>;
negate_y = <0>;
negate_z = <1>;
poll_interval = <200>;
min_interval = <5>;
inven,secondary_reg = <0x0c>;
/* If no compass sensor,
* replace "compass" with "none"
*/
inven,secondary_type = "compass";
inven,secondary_name = "ak09911";
inven,secondary_axis_map_x = <1>;
inven,secondary_axis_map_y = <0>;
inven,secondary_axis_map_z = <2>;
inven,secondary_negate_x = <1>;
inven,secondary_negate_y = <1>;
inven,secondary_negate_z = <1>;
/* If no pressure sensor,
* replace "pressure" with "none"
*/
inven,aux_type = "pressure";
inven,aux_name = "bmp280";
inven,aux_reg = <0x76>;
/* If no ALS sensor
* replace "als" with "none"
*/
inven,read_only_slave_type = "als";
inven,read_only_slave_name = "apds9930";
inven,read_only_slave_reg = <0x39>;
};
};
Communicating with the Driver in Userspace
------------------------------------------
The driver generates several files in sysfs upon installation.
These files are used to communicate with the driver. The files can be found
at:
(I2C) /sys/devices/*.i2c/i2c-*/*-*/iio:device*
(SPI) /sys/devices/*.spi/spi_master/spi*/spi*.*/iio:device*
Group and Owner for all entries should be updated to system/system at
boot time to allow userspace to access properly.

1362
drivers/iio/imu/inv_mpu/icm42600/inv_mpu_core_42600.c Normal file
View File

File diff suppressed because it is too large Load Diff

609
drivers/iio/imu/inv_mpu/icm42600/inv_mpu_iio_reg_42600.h Normal file
View File

@@ -0,0 +1,609 @@
/*
* Copyright (C) 2017-2018 InvenSense, Inc.
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#ifndef _INV_MPU_IIO_REG_42600_H_
#define _INV_MPU_IIO_REG_42600_H_
/* Comment out to use lower power feature
* by BIT_DMP_POWER_SAVE_EN
*/
#define NOT_SET_DMP_POWER_SAVE
/* Comment out not to use lower power mode on accel */
#define SUPPORT_ACCEL_LPM
/* Registers and associated bit definitions */
/* Bank 0 */
#define REG_CHIP_CONFIG_REG 0x11
#define REG_INT_CONFIG_REG 0x14
#define REG_FIFO_CONFIG_REG 0x16
#define REG_TEMP_DATA0_UI 0x1D
#define REG_TEMP_DATA1_UI 0x1E
#define REG_ACCEL_DATA_X0_UI 0x1F
#define REG_ACCEL_DATA_X1_UI 0x20
#define REG_ACCEL_DATA_Y0_UI 0x21
#define REG_ACCEL_DATA_Y1_UI 0x22
#define REG_ACCEL_DATA_Z0_UI 0x23
#define REG_ACCEL_DATA_Z1_UI 0x24
#define REG_GYRO_DATA_X0_UI 0x25
#define REG_GYOR_DATA_X1_UI 0x26
#define REG_GYRO_DATA_Y0_UI 0x27
#define REG_GYRO_DATA_Y1_UI 0x28
#define REG_GYRO_DATA_Z0_UI 0x29
#define REG_GYRO_DATA_Z1_UI 0x2A
#define REG_TMST_FSYNC1 0x2B
#define REG_TMST_FSYNC2 0x2C
#define REG_INT_STATUS 0x2D
#define REG_FIFO_BYTE_COUNT1 0x2E
#define REG_FIFO_BYTE_COUNT2 0x2F
#define REG_FIFO_DATA_REG 0x30
#define REG_APEX_DATA0 0x31
#define REG_APEX_DATA1 0x32
#define REG_APEX_DATA2 0x33
#define REG_APEX_DATA3 0x34
#define REG_APEX_DATA4 0x35
#define REG_APEX_DATA5 0x36
#define REG_INT_STATUS2 0x37
#define REG_INT_STATUS3 0x38
#define REG_SIGNAL_PATH_RESET 0x4B
#define REG_INTF_CONFIG0 0x4C
#define REG_INTF_CONFIG1 0x4D
#define REG_PWR_MGMT_0 0x4E
#define REG_GYRO_CONFIG0 0x4F
#define REG_ACCEL_CONFIG0 0x50
#define REG_GYRO_CONFIG1 0x51
#define REG_GYRO_ACCEL_CONFIG0 0x52
#define REG_ACCEL_CONFIG1 0x53
#define REG_TMST_CONFIG 0x54
#define REG_APEX_CONFIG0 0x56
#define REG_SMD_CONFIG 0x57
#define REG_FIFO_CONFIG1 0x5F
#define REG_FIFO_CONFIG2 0x60
#define REG_FIFO_CONFIG3 0x61
#define REG_FSYNC_CONFIG 0x62
#define REG_INT_CONFIG0 0x63
#define REG_INT_CONFIG1 0x64
#define REG_INT_SOURCE0 0x65
#define REG_INT_SOURCE1 0x66
#define REG_INT_SOURCE2 0x67
#define REG_INT_SOURCE3 0x68
#define REG_INT_SOURCE4 0x69
#define REG_INT_SOURCE5 0x6A
#define REG_SENSOR_SELFTEST_REG1 0x6B
#define REG_FIFO_LOST_PKT0 0x6C
#define REG_FIFO_LOST_PKT1 0x6D
#define REG_SELF_TEST_CONFIG 0x70
#define REG_MEM_BANK_SEL 0x72
#define REG_MEM_START_ADDR 0x73
#define REG_MEM_R_W 0x74
#define REG_WHO_AM_I 0x75
#define REG_REG_BANK_SEL 0x76
/* Bank 1 */
#define REG_GYRO_CONFIG_STATIC2 0x0B
#define REG_XG_ST_DATA 0x5F
#define REG_YG_ST_DATA 0x60
#define REG_ZG_ST_DATA 0x61
#define REG_TMSTVAL0 0x62
#define REG_TMSTVAL1 0x63
#define REG_TMSTVAL2 0x64
#define REG_HTR_CONFIG 0x77
#define REG_INTF_CONFIG2 0x78
#define REG_INTF_CONFIG3 0x79
#define REG_INTF_CONFIG4 0x7A
#define REG_INTF_CONFIG5 0x7B
#define REG_INTF_CONFIG6 0x7C
#define REG_INTF_CONFIG7 0x7D
#define REG_INTF_CONFIG8 0x7E
#define REG_INTF_CONFIG9 0x7F
/* Bank 2 */
#define REG_XA_ST_DATA 0x3B
#define REG_YA_ST_DATA 0x3C
#define REG_ZA_ST_DATA 0x3D
#define REG_AUX1_FIFO_DATA 0x3F
#define REG_AUX1_FIFO_COUNT1 0x40
#define REG_AUX1_FIFO_COUNT2 0x41
#define REG_AUX1_FIFO_LOST_PKT0 0x42
#define REG_AUX1_FIFO_LOST_PKT1 0x43
#define REG_OIS1_CONFIG1 0x44
#define REG_OIS1_CONFIG2 0x45
#define REG_OIS1_CONFIG3 0x46
#define REG_TEMP_DATA0_OIS1 0x47
#define REG_TEMP_DATA1_OIS1 0x48
#define REG_ACCEL_DATA_X0_OIS1 0x49
#define REG_ACCEL_DATA_X1_OIS1 0x4A
#define REG_ACCEL_DATA_Y0_OIS1 0x4B
#define REG_ACCEL_DATA_Y1_OIS1 0x4C
#define REG_ACCEL_DATA_Z0_OIS1 0x4D
#define REG_ACCEL_DATA_Z1_OIS1 0x4E
#define REG_GYRO_DATA_X0_OIS1 0x4F
#define REG_GYRO_DATA_X1_OIS1 0x50
#define REG_GYRO_DATA_Y0_OIS1 0x51
#define REG_GYRO_DATA_Y1_OIS1 0x52
#define REG_GYRO_DATA_Z0_OIS1 0x53
#define REG_GYRO_DATA_Z1_OIS1 0x54
#define REG_TMSTVAL0_OIS1 0x55
#define REG_TMSTVAL1_OIS1 0x56
#define REG_INT_STATUS_OIS1 0x57
/* Bank 3 */
#define REG_AFSR_CONFIG0 0x6E
#define REG_AFSR_CONFIG1 0x6F
#define REG_DMP_CONFIG 0x71
#define REG_ABORT_RST_CONFIG 0x72
#define REG_S4S_CONFIG 0x77
#define REG_S4S_FREQ_RATIO1 0x79
#define REG_S4S_FREQ_RATIO2 0x7A
#define REG_S4S_GYRO_TPH1 0x7B
#define REG_S4S_GYRO_TPH2 0x7C
#define REG_S4S_ACCEL_TPH1 0x7D
#define REG_S4S_ACCEL_TPH2 0x7E
#define REG_S4S_RR 0x7F
/* Bank 4 */
#define REG_DRV_GYR_CFG0_REG 0x10
#define REG_DRV_GYR_CFG1_REG 0x11
#define REG_DRV_GYR_CFG2_REG 0x12
#define REG_APEX_CONFIG1 0x40
#define REG_APEX_CONFIG2 0x41
#define REG_APEX_CONFIG3 0x42
#define REG_APEX_CONFIG4 0x43
#define REG_APEX_CONFIG5 0x44
#define REG_APEX_CONFIG6 0x45
#define REG_APEX_CONFIG7 0x46
#define REG_APEX_CONFIG8 0x47
#define REG_APEX_CONFIG9 0x48
#define REG_APEX_CONFIG10 0x49
#define REG_ACCEL_WOM_X_THR 0x4A
#define REG_ACCEL_WOM_Y_THR 0x4B
#define REG_ACCEL_WOM_Z_THR 0x4C
#define REG_INT_SOURCE6 0x4D
#define REG_INT_SOURCE7 0x4E
#define REG_INT_SOURCE8 0x4F
#define REG_INT_SOURCE9 0x50
#define REG_INT_SOURCE10 0x51
#define REG_INTF_CONFIG10 0x52
#define REG_INTF_CONFIG11 0x53
#define REG_COMM_ERR 0x54
#define REG_AFSR_CONFIG2 0x70
#define REG_GOS_USER0 0x77
#define REG_GOS_USER1 0x78
#define REG_GOS_USER2 0x79
#define REG_GOS_USER3 0x7A
#define REG_GOS_USER4 0x7B
#define REG_GOS_USER5 0x7C
#define REG_GOS_USER6 0x7D
#define REG_GOS_USER7 0x7E
#define REG_GOS_USER8 0x7F
/* REG_REG_BANK_SEL */
#define BIT_BANK_SEL_0 0x00
#define BIT_BANK_SEL_1 0x01
#define BIT_BANK_SEL_2 0x02
#define BIT_BANK_SEL_3 0x03
#define BIT_BANK_SEL_4 0x04
/* Bank0 REG_WHO_AM_I */
#define WHO_AM_I_ICM42602 0x41
#define WHO_AM_I_ICM42605 0x42
/* Bank0 REG_CHIP_CONFIG_REG */
#define BIT_SOFT_RESET 0x01
/* Bank0 REG_GYRO_CONFIG0/REG_ACCEL_CONFIG0 */
#define SHIFT_GYRO_FS_SEL 5
#define SHIFT_ACCEL_FS_SEL 5
#define SHIFT_ODR_CONF 0
/* Bank0 REG_GYRO_CONFIG1 */
#define BIT_TEMP_FILT_BW_BYPASS 0x00
#define BIT_TEMP_FILT_BW_170 0x20
#define BIT_TEMP_FILT_BW_82 0x40
#define BIT_TEMP_FILT_BW_40 0x60
#define BIT_TEMP_FILT_BW_20 0x80
#define BIT_TEMP_FILT_BW_10 0x90
#define BIT_TEMP_FILT_BW_5 0xC0
#define BIT_GYR_AVG_FLT_RATE_8KHZ 0x10
#define BIT_GYR_AVG_FLT_RATE_1KHZ 0x00
#define BIT_GYR_UI_FILT_ORD_IND_1 0x00
#define BIT_GYR_UI_FILT_ORD_IND_2 0x04
#define BIT_GYR_UI_FILT_ORD_IND_3 0x08
#define BIT_GYR_DEC2_M2_ORD_1 0x00
#define BIT_GYR_DEC2_M2_ORD_2 0x01
#define BIT_GYR_DEC2_M2_ORD_3 0x02
/* Bank0 REG_ACCEL_CONFIG1 */
#define BIT_ACC_UI_FILT_ODR_IND_1 0x00
#define BIT_ACC_UI_FILT_ODR_IND_2 0x08
#define BIT_ACC_UI_FILT_ODR_IND_3 0x10
#define BIT_ACC_DEC2_M2_ORD_1 0x00
#define BIT_ACC_DEC2_M2_ORD_2 0x02
#define BIT_ACC_DEC2_M2_ORD_3 0x04
#define BIT_ACC_AVG_FLT_RATE_8KHZ 0x01
#define BIT_ACC_AVG_FLT_RATE_1KHZ 0x00
/* Bank0 REG_INT_CONFIG_REG */
#define SHIFT_INT1_POLARITY 0
#define SHIFT_INT1_DRIVE_CIRCUIT 1
#define SHIFT_INT1_MODE 2
/* Bank0 REG_PWR_MGMT_0 */
#define BIT_TEMP_DIS 0x20
#define BIT_IDLE 0x10
#define BIT_GYRO_MODE_OFF 0x00
#define BIT_GYRO_MODE_STBY 0x04
#define BIT_GYRO_MODE_LPM 0x08
#define BIT_GYRO_MODE_LNM 0x0C
#define BIT_ACCEL_MODE_OFF 0x00
#define BIT_ACCEL_MODE_LPM 0x02
#define BIT_ACCEL_MODE_LNM 0x03
/* Bank0 REG_SIGNAL_PATH_RESET */
#define BIT_TEMP_RST 0x01
#define BIT_FIFO_FLUSH 0x02
#define BIT_TMST_STROBE 0x04
#define BIT_ABORT_AND_RESET 0x08
#define BIT_S4S_RESTART 0x10
#define BIT_DMP_MEM_RESET_EN 0x20
#define BIT_DMP_INIT_EN 0x40
/* Bank0 REG_INTF_CONFIG0 */
#define BIT_FIFO_COUNT_REC 0x40
#define BIT_COUNT_BIG_ENDIAN 0x20
#define BIT_SENS_DATA_BIG_ENDIAN 0x10
#define BIT_UI_SIFS_DISABLE_SPI 0x02
#define BIT_UI_SIFS_DISABLE_I2C 0x03
/* Bank0 REG_INTF_CONFIG1 */
#define BIT_GYRO_AFSR_MODE_LFS 0x00
#define BIT_GYRO_AFSR_MODE_HFS 0x40
#define BIT_GYRO_AFSR_MODE_DYN 0xC0
#define BIT_ACCEL_AFSR_MODE_LFS 0x00
#define BIT_ACCEL_AFSR_MODE_HFS 0x10
#define BIT_ACCEL_AFSR_MODE_DYN 0x30
#define BIT_ACCEL_LP_CLK_SEL 0x08
#define BIT_RTC_MODE 0x04
#define BIT_CLK_SEL_RC 0x00
#define BIT_CLK_SEL_PLL 0x01
#define BIT_CLK_SEL_DIS 0x03
/* Bank0 REG_FIFO_CONFIG1 */
#define BIT_FIFO_ACCEL_EN 0x01
#define BIT_FIFO_GYRO_EN 0x02
#define BIT_FIFO_TEMP_EN 0x04
#define BIT_FIFO_TMST_FSYNC_EN 0x08
#define BIT_FIFO_HIRES_EN 0x10
#define BIT_FIFO_WM_TH 0x20
#define BIT_FIFO_RESUME_PART_RD 0x40
/* Bank0 REG_INT_CONFIG1 */
#define BIT_INT_ASY_RST_DISABLE 0x10
/* Bank0 REG_INT_SOURCE0 */
#define BIT_INT_UI_AGC_RDY_INT1_EN 0x01
#define BIT_INT_FIFO_FULL_INT1_EN 0x02
#define BIT_INT_FIFO_THS_INT1_EN 0x04
#define BIT_INT_UI_DRDY_INT1_EN 0x08
#define BIT_INT_RESET_DONE_INT1_EN 0x10
#define BIT_INT_PLL_RDY_INT1_EN 0x20
#define BIT_INT_UI_FSYNC_INT1_EN 0x40
/* Bank0 REG_INT_SOURCE1 */
#define BIT_INT_WOM_X_INT1_EN 0x01
#define BIT_INT_WOM_Y_INT1_EN 0x02
#define BIT_INT_WOM_Z_INT1_EN 0x04
#define BIT_INT_SMD_INT1_EN 0x08
#define BIT_INT_WOM_XYZ_INT1_EN \
(BIT_INT_WOM_X_INT1_EN | BIT_INT_WOM_Y_INT1_EN | BIT_INT_WOM_Z_INT1_EN)
/* Bank0 REG_SENSOR_SELFTEST_REG1 */
#define BIT_ACCEL_SELF_TEST_PASS 0x08
#define BIT_GYRO_SELF_TEST_PASS 0x04
#define BIT_ACCEL_SELF_TEST_DONE 0x02
#define BIT_GYRO_SELF_TEST_DONE 0x01
/* Bank0 REG_SELF_TEST_CONFIG */
#define BIT_SELF_TEST_REGULATOR_EN 0x40
#define BIT_TEST_AZ_EN 0x20
#define BIT_TEST_AY_EN 0x10
#define BIT_TEST_AX_EN 0x08
#define BIT_TEST_GZ_EN 0x04
#define BIT_TEST_GY_EN 0x02
#define BIT_TEST_GX_EN 0x01
/* Bank0 REG_INT_STATUS */
#define BIT_INT_STATUS_AGC_RDY 0x01
#define BIT_INT_STATUS_FIFO_FULL 0x02
#define BIT_INT_STATUS_FIFO_THS 0x04
#define BIT_INT_STATUS_DRDY 0x08
#define BIT_INT_STATUS_RESET_DONE 0x10
#define BIT_INT_STATUS_PLL_DRY 0x20
#define BIT_INT_STATUS_UI_FSYNC 0x40
/* Bank0 REG_INT_STATUS2 */
#define BIT_INT_STATUS_WOM_X 0x01
#define BIT_INT_STATUS_WOM_Y 0x02
#define BIT_INT_STATUS_WOM_Z 0x04
#define BIT_INT_STATUS_SMD 0x08
#define BIT_INT_STATUS_WOM_XYZ \
(BIT_INT_STATUS_WOM_X | BIT_INT_STATUS_WOM_Y | BIT_INT_STATUS_WOM_Z)
/* Bank0 REG_INT_STATUS3 */
#define BIT_INT_STATUS_TAP_DET 0x01
#define BIT_INT_STATUS_SLEEP_DET 0x02
#define BIT_INT_STATUS_RAISE_DET 0x04
#define BIT_INT_STATUS_TILT_DET 0x08
#define BIT_INT_STATUS_STEP_CNT_OVFL 0x10
#define BIT_INT_STATUS_STEP_DET 0x20
#define BIT_INT_STATUS_DMP_POWER_SAVE 0x40
/* Bank0 REG_FIFO_CONFIG_REG */
#define BIT_FIFO_MODE_BYPASS 0x00
#define BIT_FIFO_MODE_STREAM 0x40
#define BIT_FIFO_MODE_STOP_FULL 0x80
/* Bank0 REG_GYRO_ACCEL_CONFIG0 */
#define BIT_ACCEL_UI_LNM_BW_2_FIR 0x00
#define BIT_ACCEL_UI_LNM_BW_4_IIR 0x10
#define BIT_ACCEL_UI_LNM_BW_5_IIR 0x20
#define BIT_ACCEL_UI_LNM_BW_8_IIR 0x30
#define BIT_ACCEL_UI_LNM_BW_10_IIR 0x40
#define BIT_ACCEL_UI_LNM_BW_16_IIR 0x50
#define BIT_ACCEL_UI_LNM_BW_20_IIR 0x60
#define BIT_ACCEL_UI_LNM_BW_40_IIR 0x70
#define BIT_ACCEL_UI_LNM_AVG_1 0xF0
#define BIT_ACCEL_UI_LPM_BW_2_FIR 0x00
#define BIT_ACCEL_UI_LPM_AVG_1 0x10
#define BIT_ACCEL_UI_LPM_AVG_2 0x20
#define BIT_ACCEL_UI_LPM_AVG_3 0x30
#define BIT_ACCEL_UI_LPM_AVG_4 0x40
#define BIT_ACCEL_UI_LPM_AVG_8 0x50
#define BIT_ACCEL_UI_LPM_AVG_16 0x60
#define BIT_ACCEL_UI_LPM_AVG_32 0x70
#define BIT_ACCEL_UI_LPM_AVG_64 0x80
#define BIT_ACCEL_UI_LPM_AVG_128 0x90
#define BIT_GYRO_UI_LNM_BW_2_FIR 0x00
#define BIT_GYRO_UI_LNM_BW_4_IIR 0x01
#define BIT_GYRO_UI_LNM_BW_5_IIR 0x02
#define BIT_GYRO_UI_LNM_BW_8_IIR 0x03
#define BIT_GYRO_UI_LNM_BW_10_IIR 0x04
#define BIT_GYRO_UI_LNM_BW_16_IIR 0x05
#define BIT_GYRO_UI_LNM_BW_20_IIR 0x06
#define BIT_GYRO_UI_LNM_BW_40_IIR 0x07
#define BIT_GYRO_UI_LNM_AVG_1 0xF0
#define BIT_GYRO_UI_LPM_BW_2_FIR 0x00
#define BIT_GYRO_UI_LPM_AVG_1 0x01
#define BIT_GYRO_UI_LPM_AVG_2 0x02
#define BIT_GYRO_UI_LPM_AVG_3 0x03
#define BIT_GYRO_UI_LPM_AVG_4 0x04
#define BIT_GYRO_UI_LPM_AVG_8 0x05
#define BIT_GYRO_UI_LPM_AVG_16 0x06
#define BIT_GYRO_UI_LPM_AVG_32 0x07
#define BIT_GYRO_UI_LPM_AVG_64 0x08
#define BIT_GYRO_UI_LPM_AVG_128 0x09
/* Bank0 REG_SMD_CONFIG */
#define BIT_WOM_INT_MODE_OR 0x00
#define BIT_WOM_INT_MODE_AND 0x08
#define BIT_WOM_MODE_INITIAL 0x00
#define BIT_WOM_MODE_PREV 0x04
#define BIT_SMD_MODE_OFF 0x00
#define BIT_SMD_MODE_OLD 0x01
#define BIT_SMD_MODE_SHORT 0x02
#define BIT_SMD_MODE_LONG 0x03
/* Bank0 REG_TMST_CONFIG */
#define BIT_FIFO_RAM_ISO_ENA 0x40
#define BIT_EN_DREG_FIFO_D2A 0x20
#define BIT_TMST_TO_REGS_EN 0x10
#define BIT_TMST_RESOL 0x08
#define BIT_TMST_DELTA_EN 0x04
#define BIT_TMST_FSYNC_EN 0x02
#define BIT_TMST_EN 0x01
/* Bank0 REG_APEX_CONFIG0 */
#define BIT_DMP_ODR_25HZ 0x00
#define BIT_DMP_ODR_50HZ 0x02
#define BIT_DMP_ODR_100HZ 0x03
#define BIT_RAISE_ENABLE 0x08
#define BIT_TILT_ENABLE 0x10
#define BIT_PEDO_ENABLE 0x20
#define BIT_TAP_ENABLE 0x40
#define BIT_DMP_POWER_SAVE_EN 0x80
/* Bank0 REG_ACCEL_CONFIG0 */
#define BIT_ACCEL_FSR 0xE0
#define BIT_ACCEL_ODR 0x0F
#define BIT_ACCEL_ODR_1000 0x06
#define BIT_ACCEL_ODR_500 0x0F
#define BIT_ACCEL_ODR_200 0x07
#define BIT_ACCEL_ODR_100 0x08
#define BIT_ACCEL_ODR_50 0x09
#define BIT_ACCEL_ODR_25 0x0A
#define BIT_ACCEL_ODR_12 0x0B
#define BIT_ACCEL_ODR_6 0x0C
#define BIT_ACCEL_ODR_3 0x0D
#define BIT_ACCEL_ODR_1 0x0E
/* Bank0 REG_GYRO_CONFIG0 */
#define BIT_GYRO_FSR 0xE0
#define BIT_GYRO_ODR 0x0F
#define BIT_GYRO_ODR_1000 0x06
#define BIT_GYRO_ODR_500 0x0F
#define BIT_GYRO_ODR_200 0x07
#define BIT_GYRO_ODR_100 0x08
#define BIT_GYRO_ODR_50 0x09
#define BIT_GYRO_ODR_25 0x0A
#define BIT_GYRO_ODR_12 0x0B
/* Bank4 REG_DRV_GYR_CFG0_REG */
#define GYRO_DRV_TEST_FSMFORCE_D2A_LINEAR_START_MODE 0x0D
#define GYRO_DRV_TEST_FSMFORCE_D2A_STEADY_STATE_AGC_REG_MODE 0x2A
/* Bank4 REG_DRV_GYR_CFG2_REG */
#define GYRO_DRV_SPARE2_D2A_EN 0x01
/* Bank4 REG_INT_SOURCE6 */
#define BIT_INT_TAP_DET_INT1_EN 0x01
#define BIT_INT_SLEEP_DET_INT1_EN 0x02
#define BIT_INT_RAISE_DET_INT1_EN 0x04
#define BIT_INT_TILT_DET_INT1_EN 0x08
#define BIT_INT_STEP_CNT_OVFL_INT1_EN 0x10
#define BIT_INT_STEP_DET_INT1_EN 0x20
#define BIT_INT_DMP_POWER_SAVE_INT1_EN 0x40
/* Bank4 REG_INT_SOURCE7 */
#define BIT_INT_TAP_DET_INT2_EN 0x01
#define BIT_INT_HIGHG_DET_INT2_EN 0x02
#define BIT_INT_LOWG_DET_INT2_EN 0x04
#define BIT_INT_TILT_DET_INT2_EN 0x80
#define BIT_INT_STEP_CNT_OVFL_INT2_EN 0x10
#define BIT_INT_STEP_DET_INT2_EN 0x20
#define BIT_INT_DMP_POWER_SAVE_INT2_EN 0x40
/* Bank4 REG_INT_SOURCE8 */
#define BIT_INT_AGC_RDY_IBI_EN 0x01
#define BIT_INT_FIFO_FULL_IBI_EN 0x02
#define BIT_INT_FIFO_THS_IBI_EN 0x04
#define BIT_INT_UI_DRDY_IBI_EN 0x08
#define BIT_INT_PLL_RDY_IBI_EN 0x10
#define BIT_INT_FSYNC_IBI_EN 0x20
#define BIT_INT_OIS1_DRDY_IBI_EN 0x40
/* Bank4 REG_INT_SOURCE9 */
#define BIT_INT_DMP_POWER_SAVE_IBI_EN 0x01
#define BIT_INT_WOM_X_IBI_EN 0x02
#define BIT_INT_WOM_Y_IBI_EN 0x04
#define BIT_INT_WOM_Z_IBI_EN 0x08
#define BIT_INT_SMD_IBI_EN 0x10
/* Bank4 REG_INT_SOURCE10 */
#define BIT_INT_TAP_DET_IBI_EN 0x01
#define BIT_INT_HIGHG_DET_IBI_EN 0x02
#define BIT_INT_LOWG_DET_IBI_EN 0x04
#define BIT_INT_TILT_DET_IBI_EN 0x08
#define BIT_INT_STEP_CNT_OVFL_IBI_EN 0x10
#define BIT_INT_STEP_DET_IBI_EN 0x20
/* fifo data packet header */
#define BIT_FIFO_HEAD_MSG 0x80
#define BIT_FIFO_HEAD_ACCEL 0x40
#define BIT_FIFO_HEAD_GYRO 0x20
#define BIT_FIFO_HEAD_20 0x10
#define BIT_FIFO_HEAD_TMSP_ODR 0x08
#define BIT_FIFO_HEAD_TMSP_NO_ODR 0x04
#define BIT_FIFO_HEAD_TMSP_FSYNC 0x0C
#define BIT_FIFO_HEAD_ODR_ACCEL 0x02
#define BIT_FIFO_HEAD_ODR_GYRO 0x01
/* data definitions */
#define FIFO_PACKET_BYTE_SINGLE 8
#define FIFO_PACKET_BYTE_6X 16
#define FIFO_PACKET_BYTE_HIRES 20
#define FIFO_COUNT_BYTE 2
/* sensor startup time */
#define INV_ICM42600_GYRO_START_TIME 100
#define INV_ICM42600_ACCEL_START_TIME 50
/* temperature sensor */
/* scale by 100, 1LSB=1degC, 9447 */
#define TEMP_SCALE 100
/* 25 degC */
#define TEMP_OFFSET (25 * TEMP_SCALE)
/* enum for sensor */
enum INV_SENSORS {
SENSOR_ACCEL = 0,
SENSOR_TEMP,
SENSOR_GYRO,
SENSOR_COMPASS,
SENSOR_NUM_MAX,
SENSOR_INVALID,
};
#define BASE_SAMPLE_RATE 1000
#define GESTURE_ACCEL_RATE 50
#define ESI_GYRO_RATE 1000
#define MPU_INIT_SENSOR_RATE_LNM 12 /* min Hz in LNM */
#define MPU_INIT_SENSOR_RATE_LPM 3 /* min Hz in LPM */
#define MAX_FIFO_PACKET_READ 16
#define HARDWARE_FIFO_SIZE 2048
#define FIFO_SIZE (HARDWARE_FIFO_SIZE * 7 / 8)
#define LEFT_OVER_BYTES 128
#define POWER_UP_TIME 100
#define REG_UP_TIME_USEC 100
#define IIO_BUFFER_BYTES 8
#define REG_FIFO_COUNT_H REG_FIFO_BYTE_COUNT1
#define BYTES_PER_SENSOR 6
#define BYTES_FOR_TEMP 1
#define MAX_BATCH_FIFO_SIZE FIFO_SIZE
#define FIRST_DROP_SAMPLES_ACC_500HZ 20
#define FIRST_DROP_SAMPLES_ACC_200HZ 10
#define FIRST_DROP_SAMPLES_GYR_500HZ 20
#define FIRST_DROP_SAMPLES_GYR_200HZ 10
#define WOM_THRESHOLD 13 /* 1000 / 256 * 13 = 50.7mg */
#define BIT_GYRO_FSR 0xE0
#define BIT_GYRO_ODR 0x0F
#define BIT_ACCEL_FSR 0xE0
#define BIT_ACCEL_ODR 0x0F
/*
* INT configurations
* Polarity: 0 -> Active Low, 1 -> Active High
* Drive circuit: 0 -> Open Drain, 1 -> Push-Pull
* Mode: 0 -> Pulse, 1 -> Latch
*/
#define INT_POLARITY 1
#define INT_DRIVE_CIRCUIT 1
#define INT_MODE 0
#define ACC_LPM_MAX_RATE (500)
#define GYR_LPM_MAX_RATE (200)
typedef union {
unsigned char Byte;
struct {
unsigned char g_odr_change_bit:1;
unsigned char a_odr_change_bit:1;
unsigned char timestamp_bit:2;
unsigned char twentybits_bit:1;
unsigned char gyro_bit:1;
unsigned char accel_bit:1;
unsigned char msg_bit:1;
} bits;
} icm406xx_fifo_header_t;
enum inv_devices {
ICM20608D,
ICM20789,
ICM20690,
ICM20602,
IAM20680,
ICM42600,
INV_NUM_PARTS,
};
/* chip specific functions */
struct inv_mpu_state;
int inv_get_42600_pedometer_steps(struct inv_mpu_state *st,
int *ped, int *update);
bool inv_get_apex_enabled(struct inv_mpu_state *st);
int inv_get_apex_odr(struct inv_mpu_state *st);
#endif /* #ifndef _INV_MPU_IIO_REG_42600_H_ */

430
drivers/iio/imu/inv_mpu/icm42600/inv_mpu_init_42600.c Normal file
View File

@@ -0,0 +1,430 @@
/*
* Copyright (C) 2017-2018 InvenSense, Inc.
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#define pr_fmt(fmt) "inv_mpu: " fmt
#include "../inv_mpu_iio.h"
static int inv_read_timebase(struct inv_mpu_state *st)
{
st->eng_info[ENGINE_ACCEL].base_time = NSEC_PER_SEC;
st->eng_info[ENGINE_ACCEL].base_time_1k = NSEC_PER_SEC;
st->eng_info[ENGINE_ACCEL].base_time_vr = NSEC_PER_SEC;
/* talor expansion to calculate base time unit */
st->eng_info[ENGINE_GYRO].base_time = NSEC_PER_SEC;
st->eng_info[ENGINE_GYRO].base_time_1k = NSEC_PER_SEC;
st->eng_info[ENGINE_GYRO].base_time_vr = NSEC_PER_SEC;
st->eng_info[ENGINE_I2C].base_time = NSEC_PER_SEC;
st->eng_info[ENGINE_I2C].base_time_1k = NSEC_PER_SEC;
st->eng_info[ENGINE_I2C].base_time_vr = NSEC_PER_SEC;
st->eng_info[ENGINE_ACCEL].orig_rate = BASE_SAMPLE_RATE;
st->eng_info[ENGINE_GYRO].orig_rate = BASE_SAMPLE_RATE;
st->eng_info[ENGINE_I2C].orig_rate = BASE_SAMPLE_RATE;
return 0;
}
int inv_set_gyro_sf(struct inv_mpu_state *st)
{
int result;
u8 data;
result = inv_plat_read(st, REG_GYRO_CONFIG0, 1, &data);
if (result)
return result;
data &= ~BIT_GYRO_FSR;
data |= (3 - st->chip_config.fsr) << SHIFT_GYRO_FS_SEL;
result = inv_plat_single_write(st, REG_GYRO_CONFIG0,
data);
return result;
}
int inv_set_accel_sf(struct inv_mpu_state *st)
{
int result;
u8 data;
result = inv_plat_read(st, REG_ACCEL_CONFIG0, 1, &data);
if (result)
return result;
data &= ~BIT_ACCEL_FSR;
data |= (3 - st->chip_config.accel_fs) << SHIFT_ACCEL_FS_SEL;
result = inv_plat_single_write(st, REG_ACCEL_CONFIG0,
data);
return result;
}
int inv_set_accel_intel(struct inv_mpu_state *st)
{
int result = 0;
int8_t val, accel_rate;
result = inv_plat_single_write(st, REG_REG_BANK_SEL, BIT_BANK_SEL_4);
if (result)
return result;
accel_rate = 1000 / st->eng_info[ENGINE_ACCEL].divider;
if (accel_rate > 50)
val = WOM_THRESHOLD / (accel_rate / 50);
else
val = WOM_THRESHOLD;
result |= inv_plat_single_write(st, REG_ACCEL_WOM_X_THR, val);
result |= inv_plat_single_write(st, REG_ACCEL_WOM_Y_THR, val);
result |= inv_plat_single_write(st, REG_ACCEL_WOM_Z_THR, val);
result |= inv_plat_single_write(st, REG_REG_BANK_SEL, BIT_BANK_SEL_0);
if (result)
return result;
return result;
}
int inv_config_apex_gestures(struct inv_mpu_state *st)
{
int result = 0;
int8_t rw;
int8_t mount_matrix = 0;
int8_t tilt_wait_time = 0x01;
int8_t pedo_low_energy_amp_th = 0xA;
int8_t pedo_amp_th_sel = 0x8;
int8_t pedo_step_cnt_th_sel = 0x5;
int8_t pedo_hi_enrgy_th_sel = 0x1;
int8_t pedo_sb_timer_th_sel = 0x4;
int8_t pedo_step_det_th_sel = 0x2;
int8_t pedo_sensitivity_mode = 0x0;
int8_t tap_tmax = 0x2;
int8_t tap_tmin = 0x3;
int8_t tap_tavg = 0x3;
int8_t tap_min_jerk_thr = 0x11;
int8_t tap_max_peak_tol = 0x1;
int8_t sleep_time_out = 0x4;
int8_t sleep_gesture_delay = 0x4;
/* mount matrix */
if (st->plat_data.orientation[1] && st->plat_data.orientation[3])
mount_matrix |= 0x04; /* swap(x,y) */
if (st->plat_data.orientation[0] < 0 || st->plat_data.orientation[1] < 0)
mount_matrix |= 0x02; /* x = -x */
if (st->plat_data.orientation[3] < 0 || st->plat_data.orientation[4] < 0)
mount_matrix |= 0x01; /* y = -y */
result = inv_plat_single_write(st, REG_SIGNAL_PATH_RESET,
BIT_DMP_MEM_RESET_EN);
if (result)
return result;
usleep_range(1000, 1001);
result = inv_plat_single_write(st, REG_REG_BANK_SEL, BIT_BANK_SEL_4);
if (result)
return result;
/* APEX_CONFIG1 */
result = inv_plat_read(st, REG_APEX_CONFIG1, 1, &rw);
if (result)
goto restore_bank;
rw &= 0x0f;
rw |= (pedo_low_energy_amp_th << 4) & 0xf0;
result = inv_plat_single_write(st, REG_APEX_CONFIG1, rw);
if (result)
goto restore_bank;
/* APEX_CONFIG2 */
rw = 0x00;
rw |= (pedo_amp_th_sel << 4) & 0xf0;
rw |= pedo_step_cnt_th_sel & 0x0f;
result = inv_plat_single_write(st, REG_APEX_CONFIG2, rw);
if (result)
goto restore_bank;
/* APEX_CONFIG2 */
rw = 0x00;
rw |= (pedo_step_det_th_sel << 5) & 0xe0;
rw |= (pedo_sb_timer_th_sel << 2) & 0x1c;
rw |= pedo_hi_enrgy_th_sel & 0x03;
result = inv_plat_single_write(st, REG_APEX_CONFIG3, rw);
if (result)
goto restore_bank;
/* APEX_CONFIG4 */
result = inv_plat_read(st, REG_APEX_CONFIG4, 1, &rw);
if (result)
goto restore_bank;
rw &= 0x07;
rw |= (tilt_wait_time << 6) & 0xc0;
rw |= (sleep_time_out << 3) & 0x38;
result = inv_plat_single_write(st, REG_APEX_CONFIG4, rw);
if (result)
goto restore_bank;
/* APEX_CONFIG5 */
result = inv_plat_read(st, REG_APEX_CONFIG5, 1, &rw);
if (result)
goto restore_bank;
rw &= 0xf8;
rw |= mount_matrix & 0x07;
result = inv_plat_single_write(st, REG_APEX_CONFIG5, rw);
if (result)
goto restore_bank;
/* APEX_CONFIG6 */
result = inv_plat_read(st, REG_APEX_CONFIG6, 1, &rw);
if (result)
goto restore_bank;
rw &= 0xf8;
rw |= sleep_gesture_delay & 0x07;
result = inv_plat_single_write(st, REG_APEX_CONFIG6, rw);
if (result)
goto restore_bank;
/* APEX_CONFIG7 */
rw = 0x00;
rw |= (tap_min_jerk_thr << 2) & 0xfc;
rw |= tap_max_peak_tol & 0x03;
result = inv_plat_single_write(st, REG_APEX_CONFIG7, rw);
if (result)
goto restore_bank;
/* APEX_CONFIG8 */
result = inv_plat_read(st, REG_APEX_CONFIG8, 1, &rw);
if (result)
goto restore_bank;
rw &= 0x80;
rw |= (tap_tmax << 5) & 0x60;
rw |= (tap_tmin << 3) & 0x18;
rw |= tap_tavg & 0x07;
result = inv_plat_single_write(st, REG_APEX_CONFIG8, rw);
if (result)
goto restore_bank;
/* APEX_CONFIG9 */
result = inv_plat_read(st, REG_APEX_CONFIG9, 1, &rw);
if (result)
goto restore_bank;
rw &= 0xfe;
rw |= pedo_sensitivity_mode & 0x01;
result = inv_plat_single_write(st, REG_APEX_CONFIG9, rw);
restore_bank:
if (result)
pr_err("failed to access apex config register\n");
result |= inv_plat_single_write(st, REG_REG_BANK_SEL, BIT_BANK_SEL_0);
if (result)
return result;
#ifdef NOT_SET_DMP_POWER_SAVE
result = inv_plat_single_write(st, REG_APEX_CONFIG0,
BIT_DMP_ODR_50HZ);
#else
result = inv_plat_single_write(st, REG_APEX_CONFIG0,
BIT_DMP_ODR_50HZ | BIT_DMP_POWER_SAVE_EN);
#endif
return result;
}
static void inv_init_sensor_struct(struct inv_mpu_state *st)
{
int i;
#ifdef SUPPORT_ACCEL_LPM
for (i = 0; i < SENSOR_NUM_MAX; i++)
st->sensor[i].rate = MPU_INIT_SENSOR_RATE_LPM;
#else
for (i = 0; i < SENSOR_NUM_MAX; i++)
st->sensor[i].rate = MPU_INIT_SENSOR_RATE_LNM;
#endif
st->sensor[SENSOR_GYRO].rate = MPU_INIT_SENSOR_RATE_LNM;
st->sensor[SENSOR_ACCEL].sample_size = BYTES_PER_SENSOR;
st->sensor[SENSOR_TEMP].sample_size = BYTES_FOR_TEMP;
st->sensor[SENSOR_GYRO].sample_size = BYTES_PER_SENSOR;
st->sensor_l[SENSOR_L_SIXQ].base = SENSOR_GYRO;
st->sensor_l[SENSOR_L_PEDQ].base = SENSOR_GYRO;
st->sensor_l[SENSOR_L_SIXQ_WAKE].base = SENSOR_GYRO;
st->sensor_l[SENSOR_L_PEDQ_WAKE].base = SENSOR_GYRO;
st->sensor[SENSOR_ACCEL].a_en = true;
st->sensor[SENSOR_GYRO].a_en = false;
st->sensor[SENSOR_ACCEL].g_en = false;
st->sensor[SENSOR_GYRO].g_en = true;
st->sensor[SENSOR_ACCEL].c_en = false;
st->sensor[SENSOR_GYRO].c_en = false;
st->sensor[SENSOR_ACCEL].p_en = false;
st->sensor[SENSOR_GYRO].p_en = false;
st->sensor[SENSOR_ACCEL].engine_base = ENGINE_ACCEL;
st->sensor[SENSOR_GYRO].engine_base = ENGINE_GYRO;
st->sensor_l[SENSOR_L_ACCEL].base = SENSOR_ACCEL;
st->sensor_l[SENSOR_L_GESTURE_ACCEL].base = SENSOR_ACCEL;
st->sensor_l[SENSOR_L_GYRO].base = SENSOR_GYRO;
st->sensor_l[SENSOR_L_GYRO_CAL].base = SENSOR_GYRO;
st->sensor_l[SENSOR_L_EIS_GYRO].base = SENSOR_GYRO;
st->sensor_l[SENSOR_L_ACCEL_WAKE].base = SENSOR_ACCEL;
st->sensor_l[SENSOR_L_GYRO_WAKE].base = SENSOR_GYRO;
st->sensor_l[SENSOR_L_GYRO_CAL_WAKE].base = SENSOR_GYRO;
st->sensor_l[SENSOR_L_ACCEL].header = ACCEL_HDR;
st->sensor_l[SENSOR_L_GESTURE_ACCEL].header = ACCEL_HDR;
st->sensor_l[SENSOR_L_GYRO].header = GYRO_HDR;
st->sensor_l[SENSOR_L_GYRO_CAL].header = GYRO_CALIB_HDR;
st->sensor_l[SENSOR_L_EIS_GYRO].header = EIS_GYRO_HDR;
st->sensor_l[SENSOR_L_SIXQ].header = SIXQUAT_HDR;
st->sensor_l[SENSOR_L_THREEQ].header = LPQ_HDR;
st->sensor_l[SENSOR_L_NINEQ].header = NINEQUAT_HDR;
st->sensor_l[SENSOR_L_PEDQ].header = PEDQUAT_HDR;
st->sensor_l[SENSOR_L_ACCEL_WAKE].header = ACCEL_WAKE_HDR;
st->sensor_l[SENSOR_L_GYRO_WAKE].header = GYRO_WAKE_HDR;
st->sensor_l[SENSOR_L_GYRO_CAL_WAKE].header = GYRO_CALIB_WAKE_HDR;
st->sensor_l[SENSOR_L_MAG_WAKE].header = COMPASS_WAKE_HDR;
st->sensor_l[SENSOR_L_MAG_CAL_WAKE].header = COMPASS_CALIB_WAKE_HDR;
st->sensor_l[SENSOR_L_SIXQ_WAKE].header = SIXQUAT_WAKE_HDR;
st->sensor_l[SENSOR_L_NINEQ_WAKE].header = NINEQUAT_WAKE_HDR;
st->sensor_l[SENSOR_L_PEDQ_WAKE].header = PEDQUAT_WAKE_HDR;
st->sensor_l[SENSOR_L_ACCEL].wake_on = false;
st->sensor_l[SENSOR_L_GYRO].wake_on = false;
st->sensor_l[SENSOR_L_GYRO_CAL].wake_on = false;
st->sensor_l[SENSOR_L_MAG].wake_on = false;
st->sensor_l[SENSOR_L_MAG_CAL].wake_on = false;
st->sensor_l[SENSOR_L_EIS_GYRO].wake_on = false;
st->sensor_l[SENSOR_L_SIXQ].wake_on = false;
st->sensor_l[SENSOR_L_NINEQ].wake_on = false;
st->sensor_l[SENSOR_L_PEDQ].wake_on = false;
st->sensor_l[SENSOR_L_ACCEL_WAKE].wake_on = true;
st->sensor_l[SENSOR_L_GYRO_WAKE].wake_on = true;
st->sensor_l[SENSOR_L_GYRO_CAL_WAKE].wake_on = true;
st->sensor_l[SENSOR_L_MAG_WAKE].wake_on = true;
st->sensor_l[SENSOR_L_SIXQ_WAKE].wake_on = true;
st->sensor_l[SENSOR_L_NINEQ_WAKE].wake_on = true;
st->sensor_l[SENSOR_L_PEDQ_WAKE].wake_on = true;
}
static int inv_init_config(struct inv_mpu_state *st)
{
int res, i;
st->batch.overflow_on = 0;
st->chip_config.fsr = MPU_INIT_GYRO_SCALE;
st->chip_config.accel_fs = MPU_INIT_ACCEL_SCALE;
st->ped.int_thresh = MPU_INIT_PED_INT_THRESH;
st->ped.step_thresh = MPU_INIT_PED_STEP_THRESH;
st->chip_config.low_power_gyro_on = 1;
st->eis.count_precision = NSEC_PER_MSEC;
st->firmware = 0;
st->fifo_count_mode = BYTE_MODE;
st->eng_info[ENGINE_GYRO].base_time = NSEC_PER_SEC;
st->eng_info[ENGINE_ACCEL].base_time = NSEC_PER_SEC;
inv_init_sensor_struct(st);
res = inv_read_timebase(st);
if (res)
return res;
res = inv_set_gyro_sf(st);
if (res)
return res;
res = inv_set_accel_sf(st);
if (res)
return res;
res = inv_set_accel_intel(st);
if (res)
return res;
res = inv_config_apex_gestures(st);
if (res)
return res;
for (i = 0; i < SENSOR_NUM_MAX; i++)
st->sensor[i].ts = 0;
for (i = 0; i < SENSOR_NUM_MAX; i++)
st->sensor[i].previous_ts = 0;
return res;
}
int inv_mpu_initialize(struct inv_mpu_state *st)
{
u8 v;
int result;
/* verify whoami */
result = inv_plat_read(st, REG_WHO_AM_I, 1, &v);
if (result)
return result;
pr_info("whoami= %x\n", v);
if (v == 0x00 || v == 0xff)
return -ENODEV;
/* reset to make sure previous state are not there */
result = inv_plat_read(st, REG_CHIP_CONFIG_REG, 1, &v);
if (result)
return result;
v |= BIT_SOFT_RESET;
result = inv_plat_single_write(st, REG_CHIP_CONFIG_REG, v);
if (result)
return result;
msleep(100);
v = BIT_GYRO_AFSR_MODE_HFS | BIT_ACCEL_AFSR_MODE_HFS | BIT_CLK_SEL_PLL;
result = inv_plat_single_write(st, REG_INTF_CONFIG1, v);
if (result)
return result;
/* enable chip timestamp */
v = BIT_EN_DREG_FIFO_D2A |
BIT_TMST_TO_REGS_EN |
BIT_TMST_EN;
result = inv_plat_single_write(st, REG_TMST_CONFIG, v);
if (result)
return result;
result = inv_plat_read(st, REG_INTF_CONFIG0, 1, &v);
if (result)
return result;
v |= st->i2c_dis;
result = inv_plat_single_write(st, REG_INTF_CONFIG0, v);
if (result)
return result;
v = (INT_POLARITY << SHIFT_INT1_POLARITY) |
(INT_DRIVE_CIRCUIT << SHIFT_INT1_DRIVE_CIRCUIT) |
(INT_MODE << SHIFT_INT1_MODE);
result = inv_plat_single_write(st, REG_INT_CONFIG_REG, v);
if (result)
return result;
result = inv_plat_single_write(st, REG_FIFO_CONFIG_REG,
BIT_FIFO_MODE_BYPASS);
if (result)
return result;
result = inv_plat_single_write(st, REG_INT_CONFIG1,
BIT_INT_ASY_RST_DISABLE);
if (result)
return result;
result = inv_plat_single_write(st, REG_PWR_MGMT_0, 0);
if (result)
return result;
result = inv_init_config(st);
if (result)
return result;
st->chip_config.lp_en_mode_off = 0;
result = inv_set_power(st, false);
pr_info("%s: initialize result is %d....\n", __func__, result);
return 0;
}

71
drivers/iio/imu/inv_mpu/icm42600/inv_mpu_misc_42600.c Normal file
View File

@@ -0,0 +1,71 @@
/*
* Copyright (C) 2018-2018 InvenSense, Inc.
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#define pr_fmt(fmt) "inv_mpu: " fmt
#include "../inv_mpu_iio.h"
/**
* inv_get_apex_enabled() - Check if any APEX feature is enabled
* @st: struct inv_mpu_state.
*
* Return: true when any is enabled, otherwise false.
*/
bool inv_get_apex_enabled(struct inv_mpu_state *st)
{
if (st->step_detector_l_on ||
st->step_detector_wake_l_on ||
st->step_counter_l_on ||
st->step_counter_wake_l_on)
return true;
if (st->chip_config.tilt_enable)
return true;
if (st->chip_config.tap_enable)
return true;
if (st->chip_config.pick_up_enable)
return true;
if (st->smd.on)
return true;
return false;
}
/**
* inv_get_apex_odr() - Get min accel ODR according to enabled APEX feature
* @st: struct inv_mpu_state.
*
* Return: min accel ODR in Hz
*/
int inv_get_apex_odr(struct inv_mpu_state *st)
{
int odr_hz;
#ifdef SUPPORT_ACCEL_LPM
odr_hz = MPU_INIT_SENSOR_RATE_LPM;
#else
odr_hz = MPU_INIT_SENSOR_RATE_LNM;
#endif
/* returns min accel rate for each algorithm */
if (st->chip_config.tap_enable)
odr_hz = 200;
else if (st->step_detector_l_on ||
st->step_detector_wake_l_on ||
st->step_counter_l_on ||
st->step_counter_wake_l_on ||
st->chip_config.tilt_enable ||
st->chip_config.pick_up_enable ||
st->smd.on)
odr_hz = 50;
return odr_hz;
}

559
drivers/iio/imu/inv_mpu/icm42600/inv_mpu_parsing_42600.c Normal file
View File

@@ -0,0 +1,559 @@
/*
* Copyright (C) 2017-2018 InvenSense, Inc.
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#define pr_fmt(fmt) "inv_mpu: " fmt
#include <linux/module.h>
#include <linux/init.h>
#include <linux/slab.h>
#include <linux/err.h>
#include <linux/delay.h>
#include <linux/sysfs.h>
#include <linux/jiffies.h>
#include <linux/irq.h>
#include <linux/interrupt.h>
#include <linux/kfifo.h>
#include <linux/poll.h>
#include <linux/miscdevice.h>
#include <linux/math64.h>
#include "../inv_mpu_iio.h"
static char iden[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
static int inv_process_gyro(struct inv_mpu_state *st, u8 *d, u64 t)
{
s16 raw[3];
s32 calib[3];
int i;
#define BIAS_UNIT 2859
for (i = 0; i < 3; i++)
raw[i] = be16_to_cpup((__be16 *) (d + i * 2));
for (i = 0; i < 3; i++)
calib[i] = (raw[i] << 15);
inv_push_gyro_data(st, raw, calib, t);
return 0;
}
static int inv_check_fsync(struct inv_mpu_state *st, u8 fsync_status)
{
u8 data[1];
if (!st->chip_config.eis_enable)
return 0;
inv_plat_read(st, REG_INT_STATUS, 1, data);
if (data[0] & BIT_INT_STATUS_UI_FSYNC) {
pr_debug("fsync\n");
st->eis.eis_triggered = true;
st->eis.fsync_delay = 1;
st->eis.prev_state = 1;
st->eis.frame_count++;
st->eis.eis_frame = true;
}
st->header_count--;
return 0;
}
static int inv_push_sensor(struct inv_mpu_state *st, int ind, u64 t, u8 *d)
{
#ifdef ACCEL_BIAS_TEST
s16 acc[3], avg[3];
#endif
switch (ind) {
case SENSOR_ACCEL:
inv_convert_and_push_8bytes(st, ind, d, t, iden);
#ifdef ACCEL_BIAS_TEST
acc[0] = be16_to_cpup((__be16 *) (d));
acc[1] = be16_to_cpup((__be16 *) (d + 2));
acc[2] = be16_to_cpup((__be16 *) (d + 4));
if (inv_get_3axis_average(acc, avg, 0)) {
pr_debug("accel 200 samples average = %5d, %5d, %5d\n",
avg[0], avg[1], avg[2]);
}
#endif
break;
case SENSOR_TEMP:
inv_check_fsync(st, d[1]);
break;
case SENSOR_GYRO:
inv_process_gyro(st, d, t);
break;
default:
break;
}
return 0;
}
static bool inv_validate_fifo_data(u8 *data)
{
bool ret = true;
s16 axis[3];
axis[0] = be16_to_cpup((__be16 *) (data));
axis[1] = be16_to_cpup((__be16 *) (data + 2));
axis[2] = be16_to_cpup((__be16 *) (data + 4));
if ((axis[0] == -32768) && (axis[1] == -32768) && (axis[2] == -32768))
ret = false;
return ret;
}
static int inv_push_42600_data(struct inv_mpu_state *st, u8 *d)
{
u8 *dptr;
icm406xx_fifo_header_t header;
dptr = d;
header.Byte = *dptr;
/* skip 1 byte header */
dptr++;
if (header.bits.msg_bit)
return 0;
/* accel */
if (header.bits.accel_bit) {
if (st->sensor[SENSOR_ACCEL].on &&
inv_validate_fifo_data(dptr)) {
inv_get_dmp_ts(st, SENSOR_ACCEL);
if (st->sensor[SENSOR_ACCEL].send &&
(!st->ts_algo.first_drop_samples[SENSOR_ACCEL])) {
st->sensor[SENSOR_ACCEL].sample_calib++;
inv_push_sensor(st, SENSOR_ACCEL,
st->sensor[SENSOR_ACCEL].ts, dptr);
}
}
if (st->ts_algo.first_drop_samples[SENSOR_ACCEL])
st->ts_algo.first_drop_samples[SENSOR_ACCEL]--;
dptr += st->sensor[SENSOR_ACCEL].sample_size;
}
/* gyro */
if (header.bits.gyro_bit) {
if (st->sensor[SENSOR_GYRO].on &&
inv_validate_fifo_data(dptr)) {
inv_get_dmp_ts(st, SENSOR_GYRO);
if (st->sensor[SENSOR_GYRO].send &&
(!st->ts_algo.first_drop_samples[SENSOR_GYRO])) {
st->sensor[SENSOR_GYRO].sample_calib++;
inv_push_sensor(st, SENSOR_GYRO,
st->sensor[SENSOR_GYRO].ts, dptr);
}
}
if (st->ts_algo.first_drop_samples[SENSOR_GYRO])
st->ts_algo.first_drop_samples[SENSOR_GYRO]--;
dptr += st->sensor[SENSOR_GYRO].sample_size;
}
/* temperature */
dptr += 1;
if (header.bits.timestamp_bit == 2)
dptr += 2;
st->header_count--;
return 0;
}
static int inv_prescan_fifo_data(struct inv_mpu_state *st, u8 *data, int len)
{
int i;
u8 *dptr;
icm406xx_fifo_header_t header;
for (i = 0; i < SENSOR_NUM_MAX; i++)
st->sensor[i].count = 0;
dptr = data;
while (dptr < (data + len)) {
/* count the number of valid samples
* in the buffer for accel and gyro
*/
header.Byte = *dptr;
if (header.bits.msg_bit) {
pr_warn("Unexpected FIFO msg bit\n");
return 0;
}
/* header */
dptr++;
/* accel */
if (header.bits.accel_bit) {
if (inv_validate_fifo_data(dptr))
st->sensor[SENSOR_ACCEL].count++;
dptr += st->sensor[SENSOR_ACCEL].sample_size;
}
/* gyro */
if (header.bits.gyro_bit) {
if (inv_validate_fifo_data(dptr))
st->sensor[SENSOR_GYRO].count++;
dptr += st->sensor[SENSOR_GYRO].sample_size;
}
/* temperature */
dptr += 1;
/* timestamp */
if (header.bits.timestamp_bit == 2)
dptr += 2;
}
return 0;
}
static int inv_process_42600_data(struct inv_mpu_state *st)
{
struct iio_dev *indio_dev = iio_priv_to_dev(st);
int total_bytes, tmp, res, fifo_count, pk_size, i;
u8 *dptr, *d;
u8 data[2];
bool done_flag;
res = inv_plat_read(st, REG_INT_STATUS2, 1, data);
if (res)
return res;
if (st->smd.on && (data[0] & BIT_INT_STATUS_SMD)) {
sysfs_notify(&indio_dev->dev.kobj, NULL, "poll_smd");
st->smd.on = false;
st->trigger_state = EVENT_TRIGGER;
res = set_inv_enable(indio_dev);
if (res)
return res;
st->wake_sensor_received = true;
}
if (st->gesture_only_on && (!st->batch.timeout)) {
pr_debug("ges cnt=%d, statu=%x\n",
st->gesture_int_count, data[0]);
if (data[0] & (BIT_INT_STATUS_WOM_XYZ)) {
if (!st->gesture_int_count) {
res = inv_plat_read(st,
REG_INT_SOURCE0, 1, data);
if (res)
return res;
data[0] |= BIT_INT_UI_DRDY_INT1_EN;
res = inv_plat_single_write(st,
REG_INT_SOURCE0, data[0]);
if (res)
return res;
data[0] = 0;
if (st->chip_config.gyro_enable ||
st->chip_config.accel_enable)
data[0] |= (BIT_FIFO_ACCEL_EN |
BIT_FIFO_GYRO_EN |
BIT_FIFO_WM_TH);
res = inv_plat_single_write(st,
REG_FIFO_CONFIG1, data[0]);
if (res)
return res;
/*
* First time wake up from WOM
* we don't need data in the FIFO
*/
res = inv_reset_fifo(st, true);
if (res)
return res;
if (st->chip_config.stationary_detect_enable) {
st->gesture_int_count =
STATIONARY_DELAY_THRESHOLD;
} else {
st->gesture_int_count =
WOM_DELAY_THRESHOLD;
}
return res;
}
if (st->chip_config.stationary_detect_enable) {
st->gesture_int_count =
STATIONARY_DELAY_THRESHOLD;
} else {
st->gesture_int_count =
WOM_DELAY_THRESHOLD;
}
} else {
if (!st->gesture_int_count) {
res = inv_plat_single_write(st,
REG_FIFO_CONFIG1, 0);
if (res)
return res;
res = inv_plat_read(st,
REG_INT_SOURCE0, 1, data);
if (res)
return res;
data[0] &= ~BIT_INT_UI_DRDY_INT1_EN;
res = inv_plat_single_write(st,
REG_INT_SOURCE0, data[0]);
if (res)
return res;
return res;
}
st->gesture_int_count--;
}
}
fifo_count = inv_get_last_run_time_non_dmp_record_mode(st);
pr_debug("fifc= %d\n", fifo_count);
if (!fifo_count) {
pr_debug("REG_FIFO_COUNT_H size is 0\n");
return 0;
}
pk_size = st->batch.pk_size;
if (!pk_size)
return -EINVAL;
if (fifo_count >= (HARDWARE_FIFO_SIZE / st->batch.pk_size)) {
pr_warn("fifo overflow pkt count=%d pkt sz=%d\n",
fifo_count, st->batch.pk_size);
return -EOVERFLOW;
}
fifo_count *= st->batch.pk_size;
st->fifo_count = fifo_count;
d = st->fifo_data_store;
dptr = d;
total_bytes = fifo_count;
while (total_bytes > 0) {
if (total_bytes < pk_size * MAX_FIFO_PACKET_READ)
tmp = total_bytes;
else
tmp = pk_size * MAX_FIFO_PACKET_READ;
res = inv_plat_read(st, REG_FIFO_DATA_REG, tmp, dptr);
if (res < 0) {
pr_err("read REG_FIFO_R_W is failed\n");
return res;
}
pr_debug("inside: %x, %x, %x, %x, %x, %x, %x, %x\n",
dptr[0], dptr[1], dptr[2], dptr[3],
dptr[4], dptr[5], dptr[6], dptr[7]);
pr_debug("insid2: %x, %x, %x, %x, %x, %x, %x, %x\n",
dptr[8], dptr[9], dptr[10], dptr[11],
dptr[12], dptr[13], dptr[14], dptr[15]);
dptr += tmp;
total_bytes -= tmp;
}
dptr = d;
pr_debug("dd: %x, %x, %x, %x, %x, %x, %x, %x\n",
d[0], d[1], d[2], d[3],
d[4], d[5], d[6], d[7]);
pr_debug("dd2: %x, %x, %x, %x, %x, %x, %x, %x\n",
d[8], d[9], d[10], d[11],
d[12], d[13], d[14], d[15]);
total_bytes = fifo_count;
inv_prescan_fifo_data(st, dptr, total_bytes);
st->header_count = 0;
for (i = 0; i < SENSOR_NUM_MAX; i++) {
if (st->sensor[i].on)
st->header_count = max(st->header_count,
st->sensor[i].count);
}
st->ts_algo.calib_counter++;
inv_bound_timestamp(st);
dptr = d;
done_flag = false;
while (!done_flag) {
pr_debug("total%d, pk=%d\n", total_bytes, pk_size);
if (total_bytes >= pk_size) {
res = inv_push_42600_data(st, dptr);
if (res)
return res;
total_bytes -= pk_size;
dptr += pk_size;
} else {
done_flag = true;
}
}
return 0;
}
int inv_get_42600_pedometer_steps(struct inv_mpu_state *st,
int *ped, int *update)
{
u8 r[2];
uint32_t cur_step_cnt;
int result;
*update = false;
result = inv_plat_read(st,
REG_APEX_DATA0, 2, &r[0]);
if (result)
return result;
cur_step_cnt = (uint32_t)((r[1] << 8) | r[0]);
if (st->apex_data.step_reset_last_val) {
st->apex_data.step_reset_last_val = false;
st->apex_data.step_cnt_last_val = cur_step_cnt;
}
if (cur_step_cnt !=
st->apex_data.step_cnt_last_val) {
if (cur_step_cnt < st->apex_data.step_cnt_last_val) {
/* overflow */
st->apex_data.step_cnt_total +=
cur_step_cnt + (0xFFFF - st->apex_data.step_cnt_last_val);
} else {
st->apex_data.step_cnt_total +=
cur_step_cnt - st->apex_data.step_cnt_last_val;
}
}
st->apex_data.step_cnt_last_val = cur_step_cnt;
*ped = st->apex_data.step_cnt_total;
*update = true;
return result;
}
static int inv_process_apex_gesture(struct inv_mpu_state *st)
{
struct iio_dev *indio_dev = iio_priv_to_dev(st);
u8 r[2], int_status3, apex_data4;
s16 s[3] = {0,};
u64 t;
uint32_t cur_step_cnt;
u8 tap_dir, tap_axis, tap_num;
int ped_update;
int result;
if (inv_get_apex_enabled(st)) {
t = get_time_ns();
result = inv_plat_read(st, REG_APEX_DATA4, 1, &r[0]);
if (result)
return result;
apex_data4 = r[0];
tap_dir = apex_data4 & 0x01;
tap_axis = (apex_data4 & 0x06) >> 1;
if (tap_axis > 2)
tap_axis = 2;
tap_num = (apex_data4 & 0x18) >> 3;
result = inv_plat_read(st, REG_INT_STATUS3, 1, &r[0]);
if (result)
return result;
int_status3 = r[0];
if (int_status3 & BIT_INT_STATUS_TILT_DET) {
sysfs_notify(&indio_dev->dev.kobj, NULL, "poll_tilt");
st->wake_sensor_received = true;
}
if (int_status3 & BIT_INT_STATUS_RAISE_DET) {
sysfs_notify(&indio_dev->dev.kobj, NULL, "poll_pick_up");
st->chip_config.pick_up_enable = 0;
set_inv_enable(indio_dev);
st->wake_sensor_received = true;
}
if (int_status3 & BIT_INT_STATUS_STEP_DET) {
if (st->step_detector_l_on) {
inv_push_8bytes_buffer(st,
STEP_DETECTOR_HDR, t, s);
}
if (st->step_detector_wake_l_on) {
inv_push_8bytes_buffer(st,
STEP_DETECTOR_WAKE_HDR, t, s);
st->wake_sensor_received = true;
}
if (st->ped.int_mode &&
(st->step_counter_l_on || st->step_counter_wake_l_on)) {
result = inv_get_42600_pedometer_steps(st,
&cur_step_cnt, &ped_update);
if (result)
return result;
if (ped_update)
inv_send_steps(st, st->apex_data.step_cnt_total, t);
if (st->step_counter_wake_l_on)
st->wake_sensor_received = true;
}
}
if (int_status3 & BIT_INT_STATUS_TAP_DET && tap_num == 2) {
s[tap_axis] = (tap_dir ? -1 : 1);
inv_push_8bytes_buffer(st, TAP_HDR, t, s);
st->wake_sensor_received = true;
}
}
return 0;
}
/*
* inv_read_fifo() - Transfer data from FIFO to ring buffer.
*/
irqreturn_t inv_read_fifo(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct inv_mpu_state *st = iio_priv(indio_dev);
int result;
result = wait_event_interruptible_timeout(st->wait_queue,
st->resume_state, msecs_to_jiffies(300));
if (result <= 0)
goto exit_handled;
mutex_lock(&indio_dev->mlock);
st->wake_sensor_received = false;
result = inv_process_apex_gesture(st);
if (result)
goto err_reset_fifo;
result = inv_process_42600_data(st);
if (result)
goto err_reset_fifo;
mutex_unlock(&indio_dev->mlock);
if (st->wake_sensor_received)
#ifdef CONFIG_HAS_WAKELOCK
wake_lock_timeout(&st->wake_lock, msecs_to_jiffies(200));
#else
__pm_wakeup_event(&st->wake_lock, 200); /* 200 msecs */
#endif
goto exit_handled;
err_reset_fifo:
if ((!st->chip_config.gyro_enable) &&
(!st->chip_config.accel_enable) &&
(!st->chip_config.slave_enable) &&
(!st->chip_config.pressure_enable) &&
(!inv_get_apex_enabled(st))) {
mutex_unlock(&indio_dev->mlock);
goto exit_handled;
}
pr_err("error to reset fifo\n");
inv_reset_fifo(st, true);
mutex_unlock(&indio_dev->mlock);
exit_handled:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
int inv_flush_batch_data(struct iio_dev *indio_dev, int data)
{
struct inv_mpu_state *st = iio_priv(indio_dev);
if (st->chip_config.gyro_enable ||
st->chip_config.accel_enable ||
st->chip_config.slave_enable ||
st->chip_config.pressure_enable ||
inv_get_apex_enabled(st)) {
st->wake_sensor_received = false;
inv_process_42600_data(st);
if (st->wake_sensor_received)
#ifdef CONFIG_HAS_WAKELOCK
wake_lock_timeout(&st->wake_lock, msecs_to_jiffies(200));
#else
__pm_wakeup_event(&st->wake_lock, 200); /* 200 msecs */
#endif
}
inv_push_marker_to_buffer(st, END_MARKER, data);
return 0;
}

613
drivers/iio/imu/inv_mpu/icm42600/inv_mpu_selftest_42600.c Normal file
View File

@@ -0,0 +1,613 @@
/*
* Copyright (C) 2017-2018 InvenSense, Inc.
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#define pr_fmt(fmt) "inv_mpu: " fmt
#include "../inv_mpu_iio.h"
static u8 reg_backup[128];
static u8 reg_backup_offset[9];
static u8 reg_backup_int6;
#define DEF_ST_ACCEL_RESULT_SHIFT 1
#define SELF_TEST_ODR 6 /* 1000Hz */
#define SELF_TEST_ODR_LP 7 /* 200Hz */
#define SELF_TEST_ACC_FS 3 /* +-2g */
#define SELF_TEST_GYR_FS 3 /* +-250dps */
#define SELF_TEST_ACC_BW_IND BIT_ACCEL_UI_LNM_BW_10_IIR
#define SELF_TEST_ACC_LPM_AVG BIT_ACCEL_UI_LPM_AVG_16
#define SELF_TEST_GYR_BW_IND BIT_GYRO_UI_LNM_BW_10_IIR
#define SELF_TEST_GYR_LPM_AVG BIT_GYRO_UI_LPM_AVG_16
#define SELF_TEST_PRECISION 1000
#define SELF_TEST_SAMPLE_NB 200
#define SELF_TEST_MIN_ACC_MG 225 /* mg */
#define SELF_TEST_MAX_ACC_MG 675 /* mg */
#define SELF_TEST_MIN_GYR_DPS 60 /* dps */
#define SELF_TEST_MAX_GYR_OFF_DPS 20 /* dps */
#define SELF_TEST_ACC_SHIFT_DELTA 500 /* = 0.5 */
#define SELF_TEST_GYR_SHIFT_DELTA 500 /* = 0.5 */
#define SELF_TEST_ACC_SCALE (32768 / (16 >> SELF_TEST_ACC_FS) / 1000)
#define SELF_TEST_GYR_SCALE (32768 / (2000 >> SELF_TEST_GYR_FS))
#define SELF_TEST_MIN_ACC \
(SELF_TEST_MIN_ACC_MG * SELF_TEST_ACC_SCALE * SELF_TEST_PRECISION)
#define SELF_TEST_MAX_ACC \
(SELF_TEST_MAX_ACC_MG * SELF_TEST_ACC_SCALE * SELF_TEST_PRECISION)
#define SELF_TEST_MIN_GYR \
(SELF_TEST_MIN_GYR_DPS * SELF_TEST_GYR_SCALE * SELF_TEST_PRECISION)
#define SELF_TEST_MAX_GYR_OFFSET \
(SELF_TEST_MAX_GYR_OFF_DPS * SELF_TEST_GYR_SCALE * SELF_TEST_PRECISION)
#define SELF_TEST_LN_MODE 0
#define SELF_TEST_LP_MODE 1
#define SELF_TEST_ACC_POWER_MODE SELF_TEST_LP_MODE /* low power mode is default */
#define SELF_TEST_GYRO_POWER_MODE SELF_TEST_LN_MODE /* low noise mode is default */
static const uint16_t SelfTestEquation[256] = {
2620, 2646, 2672, 2699, 2726, 2753, 2781, 2808,
2837, 2865, 2894, 2923, 2952, 2981, 3011, 3041,
3072, 3102, 3133, 3165, 3196, 3228, 3261, 3293,
3326, 3359, 3393, 3427, 3461, 3496, 3531, 3566,
3602, 3638, 3674, 3711, 3748, 3786, 3823, 3862,
3900, 3939, 3979, 4019, 4059, 4099, 4140, 4182,
4224, 4266, 4308, 4352, 4395, 4439, 4483, 4528,
4574, 4619, 4665, 4712, 4759, 4807, 4855, 4903,
4953, 5002, 5052, 5103, 5154, 5205, 5257, 5310,
5363, 5417, 5471, 5525, 5581, 5636, 5693, 5750,
5807, 5865, 5924, 5983, 6043, 6104, 6165, 6226,
6289, 6351, 6415, 6479, 6544, 6609, 6675, 6742,
6810, 6878, 6946, 7016, 7086, 7157, 7229, 7301,
7374, 7448, 7522, 7597, 7673, 7750, 7828, 7906,
7985, 8065, 8145, 8227, 8309, 8392, 8476, 8561,
8647, 8733, 8820, 8909, 8998, 9088, 9178, 9270,
9363, 9457, 9551, 9647, 9743, 9841, 9939, 10038,
10139, 10240, 10343, 10446, 10550, 10656, 10763, 10870,
10979, 11089, 11200, 11312, 11425, 11539, 11654, 11771,
11889, 12008, 12128, 12249, 12371, 12495, 12620, 12746,
12874, 13002, 13132, 13264, 13396, 13530, 13666, 13802,
13940, 14080, 14221, 14363, 14506, 14652, 14798, 14946,
15096, 15247, 15399, 15553, 15709, 15866, 16024, 16184,
16346, 16510, 16675, 16842, 17010, 17180, 17352, 17526,
17701, 17878, 18057, 18237, 18420, 18604, 18790, 18978,
19167, 19359, 19553, 19748, 19946, 20145, 20347, 20550,
20756, 20963, 21173, 21385, 21598, 21814, 22033, 22253,
22475, 22700, 22927, 23156, 23388, 23622, 23858, 24097,
24338, 24581, 24827, 25075, 25326, 25579, 25835, 26093,
26354, 26618, 26884, 27153, 27424, 27699, 27976, 28255,
28538, 28823, 29112, 29403, 29697, 29994, 30294, 30597,
30903, 31212, 31524, 31839, 32157, 32479, 32804
};
static int inv_save_setting(struct inv_mpu_state *st)
{
int result, i;
/* bank 0 */
for (i = 0; i <= REG_SELF_TEST_CONFIG; i++) {
if ((i == REG_MEM_R_W) || (i == REG_FIFO_DATA_REG)) {
reg_backup[i] = 0;
} else {
result = inv_plat_read(st, i, 1, &reg_backup[i]);
if (result)
return result;
}
}
/* bank 4 */
result = inv_plat_single_write(st, REG_REG_BANK_SEL, BIT_BANK_SEL_4);
if (result)
return result;
for (i = REG_GOS_USER0 ; i <= REG_GOS_USER8; i++) {
result = inv_plat_read(st, i, 1,
&reg_backup_offset[i - REG_GOS_USER0]);
if (result)
goto restore_bank;
}
result = inv_plat_read(st, REG_INT_SOURCE6, 1, &reg_backup_int6);
if (result)
goto restore_bank;
restore_bank:
result |= inv_plat_single_write(st, REG_REG_BANK_SEL, BIT_BANK_SEL_0);
return result;
}
static int inv_recover_setting(struct inv_mpu_state *st)
{
int result, i;
/* bank 0 */
for (i = 0; i <= REG_SELF_TEST_CONFIG; i++) {
if (i == REG_CHIP_CONFIG_REG ||
(i >= REG_TEMP_DATA0_UI && i <= REG_SIGNAL_PATH_RESET))
continue;
result = inv_plat_single_write(st, i, reg_backup[i]);
if (result)
return result;
}
/* bank 4 */
result = inv_plat_single_write(st, REG_REG_BANK_SEL, BIT_BANK_SEL_4);
if (result)
return result;
for (i = REG_GOS_USER0 ; i <= REG_GOS_USER8; i++) {
result = inv_plat_single_write(st, i,
reg_backup_offset[i - REG_GOS_USER0]);
if (result)
goto restore_bank;
}
result = inv_plat_single_write(st, REG_INT_SOURCE6, reg_backup_int6);
if (result)
goto restore_bank;
restore_bank:
result |= inv_plat_single_write(st, REG_REG_BANK_SEL, BIT_BANK_SEL_0);
return result;
}
static int inv_reset_offset_reg(struct inv_mpu_state *st)
{
int result = 0;
u8 databuf;
result = inv_plat_single_write(st, REG_REG_BANK_SEL, BIT_BANK_SEL_4);
if (result)
return result;
databuf = 0x00;
result = inv_plat_single_write(st, REG_GOS_USER0, databuf);
if (result)
return result;
result = inv_plat_single_write(st, REG_GOS_USER1, databuf);
if (result)
return result;
result = inv_plat_single_write(st, REG_GOS_USER2, databuf);
if (result)
return result;
result = inv_plat_single_write(st, REG_GOS_USER3, databuf);
if (result)
return result;
result = inv_plat_single_write(st, REG_GOS_USER4, databuf);
if (result)
return result;
result = inv_plat_single_write(st, REG_GOS_USER5, databuf);
if (result)
return result;
result = inv_plat_single_write(st, REG_GOS_USER6, databuf);
if (result)
return result;
result = inv_plat_single_write(st, REG_GOS_USER7, databuf);
if (result)
return result;
result = inv_plat_single_write(st, REG_GOS_USER8, databuf);
if (result)
return result;
result = inv_plat_single_write(st, REG_REG_BANK_SEL, BIT_BANK_SEL_0);
if (result)
return result;
return result;
}
static int inv_init_selftest(struct inv_mpu_state *st,
int sensor, int mode, int lp_mode)
{
int result = 0;
u8 databuf;
/* Disable Interrupt */
result = inv_plat_single_write(st, REG_INT_SOURCE0, 0);
if (result)
return result;
result = inv_plat_single_write(st, REG_INT_SOURCE1, 0);
if (result)
return result;
result = inv_plat_single_write(st, REG_REG_BANK_SEL, BIT_BANK_SEL_4);
if (result)
return result;
result = inv_plat_single_write(st, REG_INT_SOURCE6, 0);
if (result)
return result;
result = inv_plat_single_write(st, REG_REG_BANK_SEL, BIT_BANK_SEL_0);
if (result)
return result;
/* reset offset registers */
result = inv_reset_offset_reg(st);
if (result)
return result;
/* stop FIFO */
result = inv_plat_single_write(st, REG_FIFO_CONFIG_REG, 0);
if (result)
return result;
result = inv_plat_single_write(st, REG_FIFO_CONFIG1, 0);
if (result)
return result;
/* self-test mode set */
databuf = 0;
if (mode) {
if (sensor == SENSOR_ACCEL) {
databuf |= (BIT_TEST_AX_EN |
BIT_TEST_AY_EN |
BIT_TEST_AZ_EN);
databuf |= BIT_SELF_TEST_REGULATOR_EN;
} else if (sensor == SENSOR_GYRO) {
databuf |= (BIT_TEST_GX_EN |
BIT_TEST_GY_EN |
BIT_TEST_GZ_EN);
}
}
result = inv_plat_single_write(st, REG_SELF_TEST_CONFIG, databuf);
if (result)
return result;
/* set rate */
if (lp_mode) {
result = inv_plat_single_write(st, REG_GYRO_CONFIG0,
(SELF_TEST_GYR_FS << SHIFT_GYRO_FS_SEL) |
(SELF_TEST_ODR_LP << SHIFT_ODR_CONF));
if (result)
return result;
result = inv_plat_single_write(st, REG_ACCEL_CONFIG0,
(SELF_TEST_ACC_FS << SHIFT_ACCEL_FS_SEL) |
(SELF_TEST_ODR_LP << SHIFT_ODR_CONF));
if (result)
return result;
} else {
result = inv_plat_single_write(st, REG_GYRO_CONFIG0,
(SELF_TEST_GYR_FS << SHIFT_GYRO_FS_SEL) |
(SELF_TEST_ODR << SHIFT_ODR_CONF));
if (result)
return result;
result = inv_plat_single_write(st, REG_ACCEL_CONFIG0,
(SELF_TEST_ACC_FS << SHIFT_ACCEL_FS_SEL) |
(SELF_TEST_ODR << SHIFT_ODR_CONF));
if (result)
return result;
}
/* set filter */
databuf = 0;
if (sensor == SENSOR_ACCEL) {
if (lp_mode) {
result = inv_plat_single_write(st, REG_ACCEL_CONFIG1, 0x05);
if (result)
return result;
databuf |= SELF_TEST_ACC_LPM_AVG;
} else {
result = inv_plat_single_write(st, REG_ACCEL_CONFIG1, 0x10);
if (result)
return result;
databuf |= SELF_TEST_ACC_BW_IND;
}
} else if (sensor == SENSOR_GYRO) {
if (lp_mode) {
result = inv_plat_single_write(st, REG_GYRO_CONFIG1, 0x02);
if (result)
return result;
databuf |= SELF_TEST_GYR_LPM_AVG;
} else {
result = inv_plat_single_write(st, REG_GYRO_CONFIG1, 0x08);
if (result)
return result;
databuf |= SELF_TEST_GYR_BW_IND;
}
}
result = inv_plat_single_write(st, REG_GYRO_ACCEL_CONFIG0, databuf);
if (result)
return result;
/* turn on sensors */
databuf = 0;
if (sensor == SENSOR_ACCEL) {
if (lp_mode)
databuf |= BIT_ACCEL_MODE_LPM;
else
databuf |= BIT_ACCEL_MODE_LNM;
} else if (sensor == SENSOR_GYRO) {
if (lp_mode)
databuf |= BIT_GYRO_MODE_LPM;
else
databuf |= BIT_GYRO_MODE_LNM;
}
result = inv_plat_single_write(st, REG_PWR_MGMT_0, databuf);
if (result)
return result;
msleep(200);
return result;
}
static int inv_accel_calc_avg_with_samples(struct inv_mpu_state *st,
int avg[3], int count, int lp_mode)
{
int result = 0;
int i, nAxis;
u8 sensor_data[6];
s32 sum[3] = { 0, 0, 0 };
for (i = 0; i < count; i++) {
result = inv_plat_read(st, REG_ACCEL_DATA_X0_UI,
6, sensor_data);
if (result)
return result;
/* convert 8-bit to 16-bit */
for (nAxis = 0; nAxis < 3; nAxis++) {
sum[nAxis] +=
(s16) be16_to_cpup((__be16 *) (&sensor_data[nAxis*2]));
}
if (lp_mode) {
/* data register updated @200hz */
usleep_range(5000, 5001);
} else {
/* data register updated @1khz */
usleep_range(1000, 1001);
}
}
for (nAxis = 0; nAxis < 3; nAxis++)
avg[nAxis] = (int)(sum[nAxis] / count * SELF_TEST_PRECISION);
return result;
}
static int inv_gyro_calc_avg_with_samples(struct inv_mpu_state *st,
int avg[3], int count, int lp_mode)
{
int result = 0;
int i, nAxis;
u8 sensor_data[6];
s32 sum[3] = { 0, 0, 0 };
for (i = 0; i < count; i++) {
result = inv_plat_read(st, REG_GYRO_DATA_X0_UI, 6, sensor_data);
if (result)
return result;
/* convert 8-bit to 16-bit */
for (nAxis = 0; nAxis < 3; nAxis++) {
sum[nAxis] +=
(s16) be16_to_cpup((__be16 *) (&sensor_data[nAxis*2]));
}
if (lp_mode) {
/* data register updated @200hz */
usleep_range(5000, 5001);
} else {
/* data register updated @1khz */
usleep_range(1000, 1001);
}
}
for (nAxis = 0; nAxis < 3; nAxis++)
avg[nAxis] = (int)(sum[nAxis] / count * SELF_TEST_PRECISION);
return result;
}
static uint16_t inv_accel_do_selftest(struct inv_mpu_state *st,
int *accel_result, int lp_mode)
{
u16 ret = 0;
u8 reg_data = 0x0;
int lsb[3] = {0}, lsb_st[3] = {0}, lsb_lp[3] = {0};
int st_otp[3];
u8 otp_st_data_acc[3];
int pass = true;
int otp_value_zero = false;
int ratio;
int i;
int st_res;
ret = inv_init_selftest(st, SENSOR_ACCEL, false, false);
if (ret)
return ret;
ret = inv_accel_calc_avg_with_samples(st, lsb,
SELF_TEST_SAMPLE_NB, false);
if (ret)
return ret;
if (lp_mode) {
ret = inv_init_selftest(st, SENSOR_ACCEL, false, true);
if (ret)
return ret;
ret = inv_accel_calc_avg_with_samples(st, lsb_lp,
SELF_TEST_SAMPLE_NB, true);
if (ret)
return ret;
for (i = 0 ; i < 3 ; i++)
accel_result[i] = lsb_lp[i];
} else {
for (i = 0 ; i < 3 ; i++)
accel_result[i] = lsb[i];
}
ret = inv_init_selftest(st, SENSOR_ACCEL, true, false);
if (ret)
return ret;
/* wait 20ms for oscillations to stabilize */
msleep(20);
ret = inv_accel_calc_avg_with_samples(st, lsb_st,
SELF_TEST_SAMPLE_NB, false);
if (ret)
return ret;
/* set selftest off */
reg_data = 0x00;
ret = inv_plat_single_write(st, REG_SELF_TEST_CONFIG, reg_data);
if (ret)
return ret;
/* wait 20ms for oscillations to stabilize */
msleep(20);
/* read OTP */
ret = inv_plat_single_write(st, REG_REG_BANK_SEL, BIT_BANK_SEL_2);
if (ret)
return ret;
ret = inv_plat_read(st, REG_XA_ST_DATA, 3, otp_st_data_acc);
if (ret)
return ret;
ret = inv_plat_single_write(st, REG_REG_BANK_SEL, BIT_BANK_SEL_0);
if (ret)
return ret;
/* calculate ST results OTP based on the equation */
for (i = 0; i < 3; i++) {
if (otp_st_data_acc[i] != 0)
st_otp[i] = SelfTestEquation[otp_st_data_acc[i] - 1];
else
otp_value_zero = true;
}
if (!otp_value_zero) {
/* criteria a */
for (i = 0; i < 3; i++) {
st_res = lsb_st[i] - lsb[i];
ratio = abs(st_res / st_otp[i] - SELF_TEST_PRECISION);
if (ratio >= SELF_TEST_ACC_SHIFT_DELTA) {
pr_debug("error accel[%d] : st_res = %d, st_otp = %d\n",
i, st_res, st_otp[i]);
pass = false;
}
}
} else {
/* criteria b */
for (i = 0; i < 3; i++) {
st_res = abs(lsb_st[i] - lsb[i]);
if (st_res < SELF_TEST_MIN_ACC ||
st_res > SELF_TEST_MAX_ACC) {
pr_debug("error accel[%d] : st_res = %d, min = %d, max = %d\n",
i, st_res,
SELF_TEST_MIN_ACC,
SELF_TEST_MAX_ACC);
pass = false;
}
}
}
return !pass;
}
static uint16_t inv_gyro_do_selftest(struct inv_mpu_state *st,
int *gyro_result, int lp_mode)
{
u16 ret = 0;
u8 reg_data = 0x0;
int lsb[3] = {0}, lsb_st[3] = {0}, lsb_lp[3] = {0};
u8 otp_st_data_gyro[3];
int st_otp[3];
int pass = true;
int otp_value_zero = false;
int i;
int st_res, gyro_bias;
ret = inv_init_selftest(st, SENSOR_GYRO, false, false);
if (ret)
return ret;
ret = inv_gyro_calc_avg_with_samples(st, lsb,
SELF_TEST_SAMPLE_NB, false);
if (ret)
return ret;
if (lp_mode) {
ret = inv_init_selftest(st, SENSOR_GYRO, false, true);
if (ret)
return ret;
ret = inv_gyro_calc_avg_with_samples(st, lsb_lp,
SELF_TEST_SAMPLE_NB, true);
if (ret)
return ret;
for (i = 0 ; i < 3 ; i++)
gyro_result[i] = lsb_lp[i];
} else {
for (i = 0 ; i < 3 ; i++)
gyro_result[i] = lsb[i];
}
ret = inv_init_selftest(st, SENSOR_GYRO, true, false);
if (ret)
return ret;
/* wait 20ms for oscillations to stabilize */
msleep(20);
/* Read 200 Samples with selftest on */
ret = inv_gyro_calc_avg_with_samples(st, lsb_st,
SELF_TEST_SAMPLE_NB, false);
if (ret)
return ret;
/* set selftest off */
reg_data = 0x00;
ret = inv_plat_single_write(st, REG_SELF_TEST_CONFIG, reg_data);
if (ret)
return ret;
/* wait 20ms for oscillations to stabilize */
msleep(20);
/* read OTP */
ret = inv_plat_single_write(st, REG_REG_BANK_SEL, BIT_BANK_SEL_1);
if (ret)
return ret;
ret = inv_plat_read(st, REG_XG_ST_DATA, 3, otp_st_data_gyro);
if (ret)
return ret;
ret = inv_plat_single_write(st, REG_REG_BANK_SEL, BIT_BANK_SEL_0);
if (ret)
return ret;
/* calculate ST_OTP */
for (i = 0; i < 3; i++) {
if (otp_st_data_gyro[i] != 0)
st_otp[i] = SelfTestEquation[otp_st_data_gyro[i] - 1];
else
otp_value_zero = true;
}
if (!otp_value_zero) {
/* criteria a */
for (i = 0; i < 3; i++) {
st_res = lsb_st[i] - lsb[i];
if (st_res <= st_otp[i] * SELF_TEST_GYR_SHIFT_DELTA) {
pr_debug("error gyro[%d] : st_res = %d, st_otp = %d\n",
i, st_res, st_otp[i]);
pass = false;
}
}
} else {
/* criteria b */
for (i = 0; i < 3; i++) {
st_res = abs(lsb_st[i] - lsb[i]);
if (st_res < SELF_TEST_MIN_GYR) {
pr_debug("error gyro[%d] : st_res = %d, min = %d\n",
i, st_res, SELF_TEST_MIN_GYR);
pass = false;
}
}
}
if (pass) {
/* criteria c */
for (i = 0; i < 3; i++) {
gyro_bias = abs(lsb[i]);
if (gyro_bias > SELF_TEST_MAX_GYR_OFFSET) {
pr_debug("error gyro[%d] = %d, max = %d\n",
i, gyro_bias, SELF_TEST_MAX_GYR_OFFSET);
pass = false;
}
}
}
return !pass;
}
/*
* inv_hw_self_test() - main function to do hardware self test
*/
int inv_hw_self_test(struct inv_mpu_state *st)
{
int result;
int gyro_bias[3] = {0};
int accel_bias[3] = {0};
char accel_result, gyro_result;
int i;
accel_result = false;
gyro_result = false;
result = inv_save_setting(st);
if (result)
goto test_fail;
if (result)
goto test_fail;
result = inv_accel_do_selftest(st, accel_bias, SELF_TEST_ACC_POWER_MODE);
if (!result)
accel_result = true;
for (i = 0; i < 3; i++)
st->accel_st_bias[i] = accel_bias[i] / SELF_TEST_PRECISION;
result = inv_gyro_do_selftest(st, gyro_bias, SELF_TEST_GYRO_POWER_MODE);
if (!result)
gyro_result = true;
for (i = 0; i < 3; i++)
st->gyro_st_bias[i] = gyro_bias[i] / SELF_TEST_PRECISION;
test_fail:
inv_recover_setting(st);
return (accel_result << DEF_ST_ACCEL_RESULT_SHIFT) | gyro_result;
}

720
drivers/iio/imu/inv_mpu/icm42600/inv_mpu_setup_42600.c Normal file
View File

@@ -0,0 +1,720 @@
/*
* Copyright (C) 2017-2018 InvenSense, Inc.
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#define pr_fmt(fmt) "inv_mpu: " fmt
#include "../inv_mpu_iio.h"
static int inv_get_actual_duration(int rate)
{
int duration_ns;
if (rate > 500)
duration_ns = 1000000;
else if (rate > 200)
duration_ns = 2000000;
else if (rate > 100)
duration_ns = 5000000;
else if (rate > 50)
duration_ns = 10000000;
else if (rate > 25)
duration_ns = 20000000;
else if (rate > 12)
duration_ns = 40000000;
else if (rate > 6)
duration_ns = 80000000;
else if (rate > 3)
duration_ns = 160000000;
else
duration_ns = 320000000;
return duration_ns;
}
static int inv_calc_engine_dur(struct inv_mpu_state *st,
struct inv_engine_info *ei)
{
if (!ei->running_rate)
return -EINVAL;
ei->dur = ei->base_time / ei->orig_rate;
ei->dur *= ei->divider;
return 0;
}
int inv_turn_off_fifo(struct inv_mpu_state *st)
{
int res;
res = inv_plat_single_write(st,
REG_FIFO_CONFIG_REG, BIT_FIFO_MODE_BYPASS);
if (res)
return res;
return 0;
}
static int inv_turn_on_fifo(struct inv_mpu_state *st)
{
u8 int0_en, int1_en, fifo_en, smd;
u8 int6_en;
uint8_t burst_read[3];
int r;
r = inv_plat_single_write(st,
REG_FIFO_CONFIG_REG, BIT_FIFO_MODE_BYPASS);
if (r)
return r;
r = inv_plat_single_write(st, REG_FIFO_CONFIG1, 0);
if (r)
return r;
r = inv_plat_read(st, REG_FIFO_BYTE_COUNT1, 2, burst_read);
if (r)
return r;
r = inv_plat_read(st, REG_FIFO_DATA_REG, 3, burst_read);
if (r)
return r;
r = inv_plat_single_write(st,
REG_FIFO_CONFIG_REG, BIT_FIFO_MODE_STREAM);
if (r)
return r;
int0_en = 0;
int1_en = 0;
smd = 0;
if (inv_get_apex_enabled(st)) {
/* for saving power when apex gesture is enabled */
smd |= BIT_WOM_INT_MODE_AND |
BIT_WOM_MODE_PREV |
BIT_SMD_MODE_OLD;
}
if (st->gesture_only_on && (!st->batch.timeout)) {
if (st->chip_config.stationary_detect_enable)
st->gesture_int_count = STATIONARY_DELAY_THRESHOLD;
else
st->gesture_int_count = WOM_DELAY_THRESHOLD;
r = inv_set_accel_intel(st);
if (r)
return r;
int1_en |= BIT_INT_WOM_XYZ_INT1_EN;
smd = BIT_WOM_INT_MODE_AND |
BIT_WOM_MODE_PREV |
BIT_SMD_MODE_OLD;
}
if (st->smd.on) {
int1_en |= BIT_INT_SMD_INT1_EN;
smd |= BIT_WOM_INT_MODE_AND |
BIT_WOM_MODE_PREV |
BIT_SMD_MODE_LONG;
}
if (st->sensor[SENSOR_GYRO].on ||
st->sensor[SENSOR_ACCEL].on) {
if (st->batch.timeout) {
if (!st->batch.fifo_wm_th)
int0_en = BIT_INT_UI_DRDY_INT1_EN;
else
int0_en = BIT_INT_FIFO_THS_INT1_EN |
BIT_INT_FIFO_FULL_INT1_EN;
} else {
int0_en = BIT_INT_UI_DRDY_INT1_EN;
if (st->chip_config.eis_enable)
int0_en |= BIT_INT_UI_FSYNC_INT1_EN;
}
}
fifo_en = 0;
if (st->sensor[SENSOR_GYRO].on ||
st->sensor[SENSOR_ACCEL].on) {
fifo_en |= (BIT_FIFO_ACCEL_EN |
BIT_FIFO_GYRO_EN |
BIT_FIFO_WM_TH);
}
r = inv_plat_single_write(st, REG_FIFO_CONFIG1, fifo_en);
if (r)
return r;
r = inv_plat_single_write(st, REG_INT_SOURCE0, int0_en);
if (r)
return r;
r = inv_plat_single_write(st, REG_INT_SOURCE1, int1_en);
if (r)
return r;
int6_en = 0;
if (st->step_detector_l_on ||
st->step_detector_wake_l_on ||
st->step_counter_wake_l_on ||
(st->step_counter_l_on && st->ped.int_mode))
int6_en |= BIT_INT_STEP_DET_INT1_EN;
if (st->chip_config.tilt_enable)
int6_en |= BIT_INT_TILT_DET_INT1_EN;
if (st->chip_config.tap_enable)
int6_en |= BIT_INT_TAP_DET_INT1_EN;
if (st->chip_config.pick_up_enable)
int6_en |= BIT_INT_RAISE_DET_INT1_EN;
/* enable apex gesture interrupt */
if (int6_en) {
r = inv_plat_single_write(st,
REG_REG_BANK_SEL, BIT_BANK_SEL_4);
if (r)
return r;
r = inv_plat_single_write(st, REG_INT_SOURCE6, int6_en);
if (r)
return r;
r = inv_plat_single_write(st,
REG_REG_BANK_SEL, BIT_BANK_SEL_0);
}
r = inv_plat_single_write(st, REG_SMD_CONFIG, smd);
return r;
}
/*
* inv_reset_fifo() - Reset FIFO related registers.
*/
int inv_reset_fifo(struct inv_mpu_state *st, bool turn_off)
{
int r, i;
struct inv_timestamp_algo *ts_algo = &st->ts_algo;
int accel_rate, gyro_rate;
r = inv_turn_on_fifo(st);
if (r)
return r;
ts_algo->last_run_time = get_time_ns();
ts_algo->reset_ts = ts_algo->last_run_time;
accel_rate = 1000 / st->eng_info[ENGINE_ACCEL].divider;
gyro_rate = 1000 / st->eng_info[ENGINE_GYRO].divider;
if (accel_rate >= 500)
ts_algo->first_drop_samples[SENSOR_ACCEL] =
FIRST_DROP_SAMPLES_ACC_500HZ;
else if (accel_rate >= 200)
ts_algo->first_drop_samples[SENSOR_ACCEL] =
FIRST_DROP_SAMPLES_ACC_200HZ;
else
ts_algo->first_drop_samples[SENSOR_ACCEL] = 1;
if (gyro_rate >= 500)
ts_algo->first_drop_samples[SENSOR_GYRO] =
FIRST_DROP_SAMPLES_GYR_500HZ;
else if (gyro_rate >= 200)
ts_algo->first_drop_samples[SENSOR_GYRO] =
FIRST_DROP_SAMPLES_GYR_200HZ;
else
ts_algo->first_drop_samples[SENSOR_GYRO] = 1;
st->last_temp_comp_time = ts_algo->last_run_time;
st->left_over_size = 0;
for (i = 0; i < SENSOR_NUM_MAX; i++) {
st->sensor[i].calib_flag = 0;
st->sensor[i].sample_calib = 0;
st->sensor[i].time_calib = ts_algo->last_run_time;
}
ts_algo->calib_counter = 0;
return 0;
}
static int inv_turn_on_engine(struct inv_mpu_state *st)
{
u8 v, w;
int r;
unsigned int wait_ms;
int accel_rate, gyro_rate;
accel_rate = 1000 / st->eng_info[ENGINE_ACCEL].divider;
gyro_rate = 1000 / st->eng_info[ENGINE_GYRO].divider;
r = inv_plat_read(st, REG_PWR_MGMT_0, 1, &v);
if (r)
return r;
w = v & ~(BIT_GYRO_MODE_LNM|BIT_ACCEL_MODE_LNM);
if (st->chip_config.gyro_enable) {
/* gyro support low noise mode only */
w |= BIT_GYRO_MODE_LNM;
}
if (st->chip_config.accel_enable ||
inv_get_apex_enabled(st)) {
#ifdef SUPPORT_ACCEL_LPM
if (accel_rate > ACC_LPM_MAX_RATE)
w |= BIT_ACCEL_MODE_LNM;
else
w |= BIT_ACCEL_MODE_LPM;
#else
w |= BIT_ACCEL_MODE_LNM;
#endif
}
r = inv_plat_single_write(st, REG_PWR_MGMT_0, w);
if (r)
return r;
usleep_range(1000, 1001);
wait_ms = 0;
if (st->chip_config.gyro_enable && !(v & BIT_GYRO_MODE_LNM))
wait_ms = INV_ICM42600_GYRO_START_TIME;
if ((st->chip_config.accel_enable || inv_get_apex_enabled(st)) &&
!(v & BIT_ACCEL_MODE_LNM)) {
if (wait_ms < INV_ICM42600_ACCEL_START_TIME)
wait_ms = INV_ICM42600_ACCEL_START_TIME;
}
if (wait_ms)
msleep(wait_ms);
if (st->chip_config.has_compass) {
if (st->chip_config.compass_enable)
r = st->slave_compass->resume(st);
else
r = st->slave_compass->suspend(st);
if (r)
return r;
}
return 0;
}
static int inv_setup_dmp_rate(struct inv_mpu_state *st)
{
int i;
for (i = 0; i < SENSOR_NUM_MAX; i++) {
if (st->sensor[i].on) {
st->cntl |= st->sensor[i].output;
st->sensor[i].dur =
st->eng_info[st->sensor[i].engine_base].dur;
st->sensor[i].div = 1;
}
}
return 0;
}
/*
* inv_set_lpf() - set low pass filer based on fifo rate.
*/
static int inv_set_lpf(struct inv_mpu_state *st)
{
u8 ga_cfg0, g_cfg1, a_cfg1;
int accel_rate, gyro_rate;
int result = 0;
ga_cfg0 = 0;
a_cfg1 = BIT_ACC_UI_FILT_ODR_IND_3 | BIT_ACC_DEC2_M2_ORD_3;
g_cfg1 = BIT_TEMP_FILT_BW_BYPASS | BIT_GYR_UI_FILT_ORD_IND_3 |
BIT_GYR_DEC2_M2_ORD_3;
accel_rate = 1000 / st->eng_info[ENGINE_ACCEL].divider;
/* filter needs to be matched with HAL rate */
if (accel_rate < 25) {
if (st->sensor_l[SENSOR_L_ACCEL].on) {
accel_rate =
min(accel_rate,
st->sensor_l[SENSOR_L_ACCEL].rate);
}
if (st->sensor_l[SENSOR_L_ACCEL_WAKE].on) {
accel_rate =
min(accel_rate,
st->sensor_l[SENSOR_L_ACCEL_WAKE].rate);
}
}
gyro_rate = 1000 / st->eng_info[ENGINE_GYRO].divider;
#ifdef SUPPORT_ACCEL_LPM
if (accel_rate > ACC_LPM_MAX_RATE) {
ga_cfg0 |= BIT_ACCEL_UI_LNM_BW_2_FIR;
} else {
if (inv_get_apex_enabled(st) &&
!st->chip_config.accel_enable &&
!st->gesture_only_on) {
/* for saving power when apex gesture is enabled */
ga_cfg0 |= BIT_ACCEL_UI_LPM_AVG_1;
a_cfg1 |= BIT_ACC_AVG_FLT_RATE_8KHZ;
} else if (accel_rate <= 6) {
ga_cfg0 |= BIT_ACCEL_UI_LPM_AVG_128;
a_cfg1 |= BIT_ACC_AVG_FLT_RATE_1KHZ;
} else if (accel_rate <= 12) {
ga_cfg0 |= BIT_ACCEL_UI_LPM_AVG_64;
a_cfg1 |= BIT_ACC_AVG_FLT_RATE_1KHZ;
} else if (accel_rate <= 25) {
ga_cfg0 |= BIT_ACCEL_UI_LPM_AVG_32;
a_cfg1 |= BIT_ACC_AVG_FLT_RATE_1KHZ;
} else if (accel_rate <= 50) {
ga_cfg0 |= BIT_ACCEL_UI_LPM_AVG_128;
a_cfg1 |= BIT_ACC_AVG_FLT_RATE_8KHZ;
} else if (accel_rate <= 100) {
ga_cfg0 |= BIT_ACCEL_UI_LPM_AVG_64;
a_cfg1 |= BIT_ACC_AVG_FLT_RATE_8KHZ;
} else if (accel_rate <= 200) {
ga_cfg0 |= BIT_ACCEL_UI_LPM_AVG_16;
a_cfg1 |= BIT_ACC_AVG_FLT_RATE_8KHZ;
} else if (accel_rate <= 500) {
ga_cfg0 |= BIT_ACCEL_UI_LPM_AVG_4;
a_cfg1 |= BIT_ACC_AVG_FLT_RATE_8KHZ;
} else {
ga_cfg0 |= BIT_ACCEL_UI_LPM_AVG_1;
a_cfg1 |= BIT_ACC_AVG_FLT_RATE_8KHZ;
}
}
#else /* SUPPORT_ACCEL_LPM */
ga_cfg0 |= BIT_ACCEL_UI_LNM_BW_2_FIR;
#endif
ga_cfg0 |= BIT_GYRO_UI_LNM_BW_2_FIR;
result = inv_plat_single_write(st, REG_GYRO_CONFIG1, g_cfg1);
if (result)
return result;
result = inv_plat_single_write(st, REG_ACCEL_CONFIG1, a_cfg1);
if (result)
return result;
result = inv_plat_single_write(st, REG_GYRO_ACCEL_CONFIG0, ga_cfg0);
return result;
}
static int inv_set_div(struct inv_mpu_state *st, int a_d, int g_d)
{
u8 databuf;
int result;
result = inv_plat_read(st, REG_ACCEL_CONFIG0, 1, &databuf);
if (result)
return result;
databuf &= ~BIT_ACCEL_ODR;
if ((1000 / (a_d + 1)) > 500)
databuf |= BIT_ACCEL_ODR_1000;
else if ((1000 / (a_d + 1)) > 200)
databuf |= BIT_ACCEL_ODR_500;
else if ((1000 / (a_d + 1)) > 100)
databuf |= BIT_ACCEL_ODR_200;
else if ((1000 / (a_d + 1)) > 50)
databuf |= BIT_ACCEL_ODR_100;
else if ((1000 / (a_d + 1)) > 25)
databuf |= BIT_ACCEL_ODR_50;
else if ((1000 / (a_d + 1)) > 12)
databuf |= BIT_ACCEL_ODR_25;
else if ((1000 / (a_d + 1)) > 6)
databuf |= BIT_ACCEL_ODR_12;
else {
#ifdef SUPPORT_ACCEL_LPM
/* low power mode */
if ((1000 / (a_d + 1)) > 3)
databuf |= BIT_ACCEL_ODR_6;
else
databuf |= BIT_ACCEL_ODR_3;
#else
/* low noise mode */
databuf |= BIT_ACCEL_ODR_12;
#endif
}
result = inv_plat_single_write(st, REG_ACCEL_CONFIG0, databuf);
if (result)
return result;
result = inv_plat_read(st, REG_GYRO_CONFIG0, 1, &databuf);
if (result)
return result;
databuf &= ~BIT_GYRO_ODR;
if ((1000 / (g_d + 1)) > 500)
databuf |= BIT_GYRO_ODR_1000;
else if ((1000 / (g_d + 1)) > 200)
databuf |= BIT_GYRO_ODR_500;
else if ((1000 / (g_d + 1)) > 100)
databuf |= BIT_GYRO_ODR_200;
else if ((1000 / (g_d + 1)) > 50)
databuf |= BIT_GYRO_ODR_100;
else if ((1000 / (g_d + 1)) > 25)
databuf |= BIT_GYRO_ODR_50;
else if ((1000 / (g_d + 1)) > 12)
databuf |= BIT_GYRO_ODR_25;
else
databuf |= BIT_GYRO_ODR_12;
result = inv_plat_single_write(st, REG_GYRO_CONFIG0, databuf);
if (result)
return result;
return result;
}
static int inv_set_batch(struct inv_mpu_state *st)
{
int res = 0;
u32 w;
u32 running_rate;
if (st->sensor[SENSOR_ACCEL].on || st->sensor[SENSOR_GYRO].on)
st->batch.pk_size = 16;
else
st->batch.pk_size = 0;
if (st->sensor[SENSOR_GYRO].on && !st->sensor[SENSOR_ACCEL].on)
running_rate = st->eng_info[ENGINE_GYRO].running_rate;
else if (!st->sensor[SENSOR_GYRO].on && st->sensor[SENSOR_ACCEL].on)
running_rate = st->eng_info[ENGINE_ACCEL].running_rate;
else
running_rate = st->eng_info[ENGINE_GYRO].running_rate;
if (st->batch.timeout) {
w = st->batch.timeout * running_rate
* st->batch.pk_size / 1000;
if (w > MAX_BATCH_FIFO_SIZE)
w = MAX_BATCH_FIFO_SIZE;
} else {
w = 0;
}
st->batch.fifo_wm_th = w;
pr_debug("running= %d, pksize=%d, to=%d w=%d\n",
running_rate, st->batch.pk_size, st->batch.timeout, w);
res = inv_plat_single_write(st, REG_FIFO_CONFIG2, w & 0xff);
if (res)
return res;
res = inv_plat_single_write(st, REG_FIFO_CONFIG3, (w >> 8) & 0xff);
return res;
}
static int inv_set_rate(struct inv_mpu_state *st)
{
int g_d, a_d, result;
result = inv_setup_dmp_rate(st);
if (result)
return result;
g_d = st->eng_info[ENGINE_GYRO].divider - 1;
a_d = st->eng_info[ENGINE_ACCEL].divider - 1;
result = inv_set_div(st, a_d, g_d);
if (result)
return result;
result = inv_set_lpf(st);
if (result)
return result;
inv_set_batch(st);
return result;
}
static int inv_enable_apex_gestures(struct inv_mpu_state *st)
{
int result;
u8 w, r;
unsigned int wait_ms = 0;
result = inv_plat_read(st, REG_APEX_CONFIG0, 1, &r);
if (result)
return result;
#ifdef NOT_SET_DMP_POWER_SAVE
w = BIT_DMP_ODR_50HZ;
#else
w = BIT_DMP_POWER_SAVE_EN | BIT_DMP_ODR_50HZ;
#endif
if (st->step_detector_l_on ||
st->step_detector_wake_l_on ||
st->step_counter_l_on ||
st->step_counter_wake_l_on)
w |= BIT_PEDO_ENABLE;
if (st->chip_config.tilt_enable)
w |= BIT_TILT_ENABLE;
if (st->chip_config.tap_enable)
w |= BIT_TAP_ENABLE;
if (st->chip_config.pick_up_enable)
w |= BIT_RAISE_ENABLE;
if (r != w) {
if (!(r & BIT_PEDO_ENABLE) && (w & BIT_PEDO_ENABLE))
wait_ms = 50;
if (!(r & (BIT_PEDO_ENABLE | BIT_TILT_ENABLE | BIT_RAISE_ENABLE)) &&
(w & (BIT_PEDO_ENABLE | BIT_TILT_ENABLE | BIT_RAISE_ENABLE))) {
result = inv_plat_single_write(st, REG_SIGNAL_PATH_RESET, BIT_DMP_INIT_EN);
if (result)
return result;
msleep(50);
result = inv_plat_single_write(st, REG_APEX_CONFIG0, w);
if (result)
return result;
if (wait_ms)
msleep(wait_ms);
} else if (!(w & (BIT_PEDO_ENABLE | BIT_TILT_ENABLE | BIT_RAISE_ENABLE))) {
result = inv_plat_single_write(st, REG_APEX_CONFIG0, w);
if (result)
return result;
result = inv_plat_single_write(st, REG_SIGNAL_PATH_RESET,
BIT_DMP_MEM_RESET_EN);
if (result)
return result;
usleep_range(1000, 1001);
} else {
result = inv_plat_single_write(st, REG_APEX_CONFIG0, w);
if (result)
return result;
if (wait_ms)
msleep(wait_ms);
}
if (!(r & BIT_PEDO_ENABLE) && (w & BIT_PEDO_ENABLE))
st->apex_data.step_reset_last_val = true;
else
st->apex_data.step_reset_last_val = false;
}
return result;
}
static int inv_determine_engine(struct inv_mpu_state *st)
{
int i;
bool a_en, g_en;
int accel_rate, gyro_rate;
a_en = false;
g_en = false;
gyro_rate = MPU_INIT_SENSOR_RATE_LNM;
#ifdef SUPPORT_ACCEL_LPM
accel_rate = MPU_INIT_SENSOR_RATE_LPM;
#else
accel_rate = MPU_INIT_SENSOR_RATE_LNM;
#endif
/*
* loop the streaming sensors to see which engine needs to be turned on
*/
for (i = 0; i < SENSOR_NUM_MAX; i++) {
if (st->sensor[i].on) {
a_en |= st->sensor[i].a_en;
g_en |= st->sensor[i].g_en;
}
}
if (st->chip_config.eis_enable) {
g_en = true;
st->eis.frame_count = 0;
st->eis.fsync_delay = 0;
st->eis.gyro_counter = 0;
st->eis.voting_count = 0;
st->eis.voting_count_sub = 0;
gyro_rate = BASE_SAMPLE_RATE;
} else {
st->eis.eis_triggered = false;
st->eis.prev_state = false;
}
accel_rate = st->sensor[SENSOR_ACCEL].rate;
gyro_rate = max(gyro_rate, st->sensor[SENSOR_GYRO].rate);
if (g_en)
st->ts_algo.clock_base = ENGINE_GYRO;
else
st->ts_algo.clock_base = ENGINE_ACCEL;
st->eng_info[ENGINE_GYRO].running_rate = gyro_rate;
st->eng_info[ENGINE_ACCEL].running_rate = accel_rate;
/* engine divider for pressure and compass is set later */
if (st->chip_config.eis_enable) {
st->eng_info[ENGINE_GYRO].divider = 1;
st->eng_info[ENGINE_ACCEL].divider = 1;
/* need to update rate and div for 1khz mode */
for (i = 0 ; i < SENSOR_L_NUM_MAX ; i++) {
if (st->sensor_l[i].on) {
st->sensor_l[i].counter = 0;
if (st->sensor_l[i].rate)
st->sensor_l[i].div =
((BASE_SAMPLE_RATE /
st->eng_info[ENGINE_GYRO].divider) /
st->sensor_l[i].rate);
else
st->sensor_l[i].div = 0xffff;
}
}
} else {
st->eng_info[ENGINE_GYRO].divider =
inv_get_actual_duration(st->eng_info[ENGINE_GYRO].running_rate) / 1000000;
st->eng_info[ENGINE_ACCEL].divider =
inv_get_actual_duration(st->eng_info[ENGINE_ACCEL].running_rate) / 1000000;
}
for (i = 0 ; i < SENSOR_L_NUM_MAX ; i++)
st->sensor_l[i].counter = 0;
inv_calc_engine_dur(st, &st->eng_info[ENGINE_GYRO]);
inv_calc_engine_dur(st, &st->eng_info[ENGINE_ACCEL]);
pr_debug("gen: %d aen: %d grate: %d arate: %d\n",
g_en, a_en, gyro_rate, accel_rate);
st->chip_config.gyro_enable = g_en;
st->chip_config.accel_enable = a_en;
return 0;
}
/*
* set_inv_enable() - enable function.
*/
int set_inv_enable(struct iio_dev *indio_dev)
{
int result;
struct inv_mpu_state *st = iio_priv(indio_dev);
u8 w;
inv_stop_interrupt(st);
inv_turn_off_fifo(st);
inv_determine_engine(st);
result = inv_set_rate(st);
if (result) {
pr_err("inv_set_rate error\n");
return result;
}
/* enable chip timestamp */
w = BIT_EN_DREG_FIFO_D2A |
BIT_TMST_TO_REGS_EN |
BIT_TMST_EN;
result = inv_plat_single_write(st, REG_TMST_CONFIG, w);
if (result)
return result;
w = BIT_GYRO_AFSR_MODE_HFS |
BIT_ACCEL_AFSR_MODE_HFS |
BIT_CLK_SEL_PLL;
if (st->chip_config.accel_enable && st->chip_config.gyro_enable)
w |= BIT_ACCEL_LP_CLK_SEL;
result = inv_plat_single_write(st, REG_INTF_CONFIG1, w);
if (result)
return result;
if (w & BIT_ACCEL_LP_CLK_SEL)
msleep(st->eng_info[ENGINE_ACCEL].divider);
result = inv_turn_on_engine(st);
if (result) {
pr_err("inv_turn_on_engine error\n");
return result;
}
result = inv_enable_apex_gestures(st);
if (result) {
pr_err("inv_enable_apex_gestures error\n");
return result;
}
result = inv_reset_fifo(st, false);
if (result)
return result;
if ((!st->chip_config.gyro_enable) &&
(!st->chip_config.accel_enable) &&
(!inv_get_apex_enabled(st))) {
inv_set_power(st, false);
return 0;
}
return result;
}
/* dummy function for 20608D */
int inv_enable_pedometer_interrupt(struct inv_mpu_state *st, bool en)
{
return 0;
}
int inv_dmp_read(struct inv_mpu_state *st, int off, int size, u8 *buf)
{
return 0;
}
int inv_firmware_load(struct inv_mpu_state *st)
{
return 0;
}

1192
drivers/iio/imu/inv_mpu/inv_mpu_common.c Normal file
View File

File diff suppressed because it is too large Load Diff

343
drivers/iio/imu/inv_mpu/inv_mpu_dts.c Normal file
View File

@@ -0,0 +1,343 @@
/*
* Copyright (C) 2012-2017 InvenSense, Inc.
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#include <linux/device.h>
#include <linux/err.h>
#include <linux/of_gpio.h>
#include <linux/of.h>
#include <linux/of_device.h>
#include <linux/regulator/consumer.h>
#include <linux/export.h>
#include <linux/iio/imu/mpu.h>
#include "inv_mpu_dts.h"
#include "inv_mpu_iio.h"
#ifdef CONFIG_OF
static int inv_mpu_power_on(struct mpu_platform_data *pdata)
{
int err;
if (!IS_ERR(pdata->vdd_ana)) {
err = regulator_enable(pdata->vdd_ana);
if (err)
return err;
}
if (!IS_ERR(pdata->vdd_i2c)) {
err = regulator_enable(pdata->vdd_i2c);
if (err)
goto error_disable_vdd_ana;
}
return 0;
error_disable_vdd_ana:
regulator_disable(pdata->vdd_ana);
return err;
}
static int inv_mpu_power_off(struct mpu_platform_data *pdata)
{
if (!IS_ERR(pdata->vdd_ana))
regulator_disable(pdata->vdd_ana);
if (!IS_ERR(pdata->vdd_i2c))
regulator_disable(pdata->vdd_i2c);
return 0;
}
static int inv_parse_orientation_matrix(struct device *dev, s8 *orient)
{
int rc, i;
struct device_node *np = dev->of_node;
u32 temp_val, temp_val2;
for (i = 0; i < 9; i++)
orient[i] = 0;
/* parsing axis x orientation matrix */
rc = of_property_read_u32(np, "axis_map_x", &temp_val);
if (rc) {
dev_err(dev, "Unable to read axis_map_x\n");
return rc;
}
rc = of_property_read_u32(np, "negate_x", &temp_val2);
if (rc) {
dev_err(dev, "Unable to read negate_x\n");
return rc;
}
if (temp_val2)
orient[temp_val] = -1;
else
orient[temp_val] = 1;
/* parsing axis y orientation matrix */
rc = of_property_read_u32(np, "axis_map_y", &temp_val);
if (rc) {
dev_err(dev, "Unable to read axis_map_y\n");
return rc;
}
rc = of_property_read_u32(np, "negate_y", &temp_val2);
if (rc) {
dev_err(dev, "Unable to read negate_y\n");
return rc;
}
if (temp_val2)
orient[3 + temp_val] = -1;
else
orient[3 + temp_val] = 1;
/* parsing axis z orientation matrix */
rc = of_property_read_u32(np, "axis_map_z", &temp_val);
if (rc) {
dev_err(dev, "Unable to read axis_map_z\n");
return rc;
}
rc = of_property_read_u32(np, "negate_z", &temp_val2);
if (rc) {
dev_err(dev, "Unable to read negate_z\n");
return rc;
}
if (temp_val2)
orient[6 + temp_val] = -1;
else
orient[6 + temp_val] = 1;
return 0;
}
static int inv_parse_secondary_orientation_matrix(struct device *dev,
s8 *orient)
{
int rc, i;
struct device_node *np = dev->of_node;
u32 temp_val, temp_val2;
for (i = 0; i < 9; i++)
orient[i] = 0;
/* parsing axis x orientation matrix */
rc = of_property_read_u32(np, "inven,secondary_axis_map_x", &temp_val);
if (rc) {
dev_err(dev, "Unable to read secondary axis_map_x\n");
return rc;
}
rc = of_property_read_u32(np, "inven,secondary_negate_x", &temp_val2);
if (rc) {
dev_err(dev, "Unable to read secondary negate_x\n");
return rc;
}
if (temp_val2)
orient[temp_val] = -1;
else
orient[temp_val] = 1;
/* parsing axis y orientation matrix */
rc = of_property_read_u32(np, "inven,secondary_axis_map_y", &temp_val);
if (rc) {
dev_err(dev, "Unable to read secondary axis_map_y\n");
return rc;
}
rc = of_property_read_u32(np, "inven,secondary_negate_y", &temp_val2);
if (rc) {
dev_err(dev, "Unable to read secondary negate_y\n");
return rc;
}
if (temp_val2)
orient[3 + temp_val] = -1;
else
orient[3 + temp_val] = 1;
/* parsing axis z orientation matrix */
rc = of_property_read_u32(np, "inven,secondary_axis_map_z", &temp_val);
if (rc) {
dev_err(dev, "Unable to read secondary axis_map_z\n");
return rc;
}
rc = of_property_read_u32(np, "inven,secondary_negate_z", &temp_val2);
if (rc) {
dev_err(dev, "Unable to read secondary negate_z\n");
return rc;
}
if (temp_val2)
orient[6 + temp_val] = -1;
else
orient[6 + temp_val] = 1;
return 0;
}
static int inv_parse_secondary(struct device *dev,
struct mpu_platform_data *pdata)
{
int rc;
struct device_node *np = dev->of_node;
u32 temp_val;
const char *name;
if (of_property_read_string(np, "inven,secondary_type", &name)) {
dev_err(dev, "Missing secondary type.\n");
return -EINVAL;
}
if (!strcmp(name, "compass")) {
pdata->sec_slave_type = SECONDARY_SLAVE_TYPE_COMPASS;
} else if (!strcmp(name, "none")) {
pdata->sec_slave_type = SECONDARY_SLAVE_TYPE_NONE;
return 0;
} else {
return -EINVAL;
}
if (of_property_read_string(np, "inven,secondary_name", &name)) {
dev_err(dev, "Missing secondary name.\n");
return -EINVAL;
}
if (!strcmp(name, "ak8963"))
pdata->sec_slave_id = COMPASS_ID_AK8963;
else if (!strcmp(name, "ak8975"))
pdata->sec_slave_id = COMPASS_ID_AK8975;
else if (!strcmp(name, "ak8972"))
pdata->sec_slave_id = COMPASS_ID_AK8972;
else if (!strcmp(name, "ak09911"))
pdata->sec_slave_id = COMPASS_ID_AK09911;
else if (!strcmp(name, "ak09912"))
pdata->sec_slave_id = COMPASS_ID_AK09912;
else if (!strcmp(name, "ak09916"))
pdata->sec_slave_id = COMPASS_ID_AK09916;
else
return -EINVAL;
rc = of_property_read_u32(np, "inven,secondary_reg", &temp_val);
if (rc) {
dev_err(dev, "Unable to read secondary register\n");
return rc;
}
pdata->secondary_i2c_addr = temp_val;
rc = inv_parse_secondary_orientation_matrix(dev,
pdata->
secondary_orientation);
return rc;
}
static int inv_parse_aux(struct device *dev, struct mpu_platform_data *pdata)
{
int rc;
struct device_node *np = dev->of_node;
u32 temp_val;
const char *name;
if (of_property_read_string(np, "inven,aux_type", &name)) {
dev_err(dev, "Missing aux type.\n");
return -EINVAL;
}
if (!strcmp(name, "pressure")) {
pdata->aux_slave_type = SECONDARY_SLAVE_TYPE_PRESSURE;
} else if (!strcmp(name, "none")) {
pdata->aux_slave_type = SECONDARY_SLAVE_TYPE_NONE;
return 0;
} else {
return -EINVAL;
}
if (of_property_read_string(np, "inven,aux_name", &name)) {
dev_err(dev, "Missing aux name.\n");
return -EINVAL;
}
if (!strcmp(name, "bmp280"))
pdata->aux_slave_id = PRESSURE_ID_BMP280;
else
return -EINVAL;
rc = of_property_read_u32(np, "inven,aux_reg", &temp_val);
if (rc) {
dev_err(dev, "Unable to read aux register\n");
return rc;
}
pdata->aux_i2c_addr = temp_val;
return 0;
}
static int inv_parse_readonly_secondary(struct device *dev,
struct mpu_platform_data *pdata)
{
int rc;
struct device_node *np = dev->of_node;
u32 temp_val;
const char *name;
if (of_property_read_string(np, "inven,read_only_slave_type", &name)) {
dev_err(dev, "Missing read only slave type type.\n");
return -EINVAL;
}
if (!strcmp(name, "als")) {
pdata->read_only_slave_type = SECONDARY_SLAVE_TYPE_ALS;
} else if (!strcmp(name, "none")) {
pdata->read_only_slave_type = SECONDARY_SLAVE_TYPE_NONE;
return 0;
} else {
return -EINVAL;
}
if (of_property_read_string(np, "inven,read_only_slave_name", &name)) {
dev_err(dev, "Missing read only slave type name.\n");
return -EINVAL;
}
if (!strcmp(name, "apds9930"))
pdata->read_only_slave_id = ALS_ID_APDS_9930;
else
return -EINVAL;
rc = of_property_read_u32(np, "inven,read_only_slave_reg", &temp_val);
if (rc) {
dev_err(dev, "Unable to read read only slave reg register\n");
return rc;
}
pdata->read_only_i2c_addr = temp_val;
return 0;
}
int invensense_mpu_parse_dt(struct device *dev, struct mpu_platform_data *pdata)
{
int rc;
rc = inv_parse_orientation_matrix(dev, pdata->orientation);
if (rc)
return rc;
rc = inv_parse_secondary(dev, pdata);
if (rc)
return rc;
inv_parse_aux(dev, pdata);
inv_parse_readonly_secondary(dev, pdata);
pdata->vdd_ana = regulator_get(dev, "inven,vdd_ana");
if (IS_ERR(pdata->vdd_ana)) {
rc = PTR_ERR(pdata->vdd_ana);
dev_warn(dev, "regulator get failed vdd_ana-supply rc=%d\n", rc);
}
pdata->vdd_i2c = regulator_get(dev, "inven,vcc_i2c");
if (IS_ERR(pdata->vdd_i2c)) {
rc = PTR_ERR(pdata->vdd_i2c);
dev_warn(dev, "regulator get failed vcc-i2c-supply rc=%d\n", rc);
}
pdata->power_on = inv_mpu_power_on;
pdata->power_off = inv_mpu_power_off;
dev_dbg(dev, "parse dt complete\n");
return 0;
}
EXPORT_SYMBOL_GPL(invensense_mpu_parse_dt);
#endif /* CONFIG_OF */

25
drivers/iio/imu/inv_mpu/inv_mpu_dts.h Normal file
View File

@@ -0,0 +1,25 @@
/*
* Copyright (C) 2012-2017 InvenSense, Inc.
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#ifndef _INV_MPU_DTS_H_
#define _INV_MPU_DTS_H_
#include <linux/kernel.h>
#include <linux/iio/imu/mpu.h>
#ifdef CONFIG_OF
int invensense_mpu_parse_dt(struct device *dev,
struct mpu_platform_data *pdata);
#endif
#endif /* #ifndef _INV_MPU_DTS_H_ */

545
drivers/iio/imu/inv_mpu/inv_mpu_i2c.c Normal file
View File

@@ -0,0 +1,545 @@
/*
* Copyright (C) 2012-2018 InvenSense, Inc.
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#define pr_fmt(fmt) "inv_mpu: " fmt
#include <linux/module.h>
#include <linux/init.h>
#include <linux/slab.h>
#include <linux/i2c.h>
#include <linux/err.h>
#include <linux/delay.h>
#include <linux/sysfs.h>
#include <linux/jiffies.h>
#include <linux/irq.h>
#include <linux/interrupt.h>
#include <linux/kfifo.h>
#include <linux/poll.h>
#include <linux/miscdevice.h>
#include <linux/spinlock.h>
#include <linux/of_device.h>
#include "inv_mpu_iio.h"
#include "inv_mpu_dts.h"
#define CONFIG_DYNAMIC_DEBUG_I2C 0
/**
* inv_i2c_read_base() - Read one or more bytes from the device registers.
* @st: Device driver instance.
* @i2c_addr: i2c address of device.
* @reg: First device register to be read from.
* @length: Number of bytes to read.
* @data: Data read from device.
* NOTE:This is not re-implementation of i2c_smbus_read because i2c
* address could be specified in this case. We could have two different
* i2c address due to secondary i2c interface.
*/
int inv_i2c_read_base(struct inv_mpu_state *st, u16 i2c_addr,
u8 reg, u16 length, u8 *data)
{
struct i2c_msg msgs[2];
int res;
if (!data)
return -EINVAL;
msgs[0].addr = i2c_addr;
msgs[0].flags = 0; /* write */
msgs[0].buf = &reg;
msgs[0].len = 1;
msgs[1].addr = i2c_addr;
msgs[1].flags = I2C_M_RD;
msgs[1].buf = data;
msgs[1].len = length;
res = i2c_transfer(st->sl_handle, msgs, 2);
if (res < 2) {
if (res >= 0)
res = -EIO;
} else
res = 0;
INV_I2C_INC_MPUWRITE(3);
INV_I2C_INC_MPUREAD(length);
return res;
}
/**
* inv_i2c_single_write_base() - Write a byte to a device register.
* @st: Device driver instance.
* @i2c_addr: I2C address of the device.
* @reg: Device register to be written to.
* @data: Byte to write to device.
* NOTE:This is not re-implementation of i2c_smbus_write because i2c
* address could be specified in this case. We could have two different
* i2c address due to secondary i2c interface.
*/
int inv_i2c_single_write_base(struct inv_mpu_state *st,
u16 i2c_addr, u8 reg, u8 data)
{
u8 tmp[2];
struct i2c_msg msg;
int res;
tmp[0] = reg;
tmp[1] = data;
msg.addr = i2c_addr;
msg.flags = 0; /* write */
msg.buf = tmp;
msg.len = 2;
INV_I2C_INC_MPUWRITE(3);
res = i2c_transfer(st->sl_handle, &msg, 1);
if (res < 1) {
if (res == 0)
res = -EIO;
return res;
} else
return 0;
}
static int inv_i2c_single_write(struct inv_mpu_state *st, u8 reg, u8 data)
{
return inv_i2c_single_write_base(st, st->i2c_addr, reg, data);
}
static int inv_i2c_read(struct inv_mpu_state *st, u8 reg, int len, u8 *data)
{
return inv_i2c_read_base(st, st->i2c_addr, reg, len, data);
}
#if !defined(CONFIG_INV_MPU_IIO_ICM42600)
static int _memory_write(struct inv_mpu_state *st, u8 mpu_addr, u16 mem_addr,
u32 len, u8 const *data)
{
u8 bank[2];
u8 addr[2];
u8 buf[513];
struct i2c_msg msgs[3];
int res;
if (!data || !st)
return -EINVAL;
if (len >= (sizeof(buf) - 1))
return -ENOMEM;
bank[0] = REG_MEM_BANK_SEL;
bank[1] = mem_addr >> 8;
addr[0] = REG_MEM_START_ADDR;
addr[1] = mem_addr & 0xFF;
buf[0] = REG_MEM_R_W;
memcpy(buf + 1, data, len);
/* write message */
msgs[0].addr = mpu_addr;
msgs[0].flags = 0;
msgs[0].buf = bank;
msgs[0].len = sizeof(bank);
msgs[1].addr = mpu_addr;
msgs[1].flags = 0;
msgs[1].buf = addr;
msgs[1].len = sizeof(addr);
msgs[2].addr = mpu_addr;
msgs[2].flags = 0;
msgs[2].buf = (u8 *) buf;
msgs[2].len = len + 1;
INV_I2C_INC_MPUWRITE(3 + 3 + (2 + len));
#if CONFIG_DYNAMIC_DEBUG_I2C
{
char *write = 0;
pr_debug("%s WM%02X%02X%02X%s%s - %d\n", st->hw->name,
mpu_addr, bank[1], addr[1],
wr_pr_debug_begin(data, len, write),
wr_pr_debug_end(write), len);
}
#endif
res = i2c_transfer(st->sl_handle, msgs, 3);
if (res != 3) {
if (res >= 0)
res = -EIO;
return res;
} else {
return 0;
}
}
static int inv_i2c_mem_write(struct inv_mpu_state *st, u8 mpu_addr, u16 mem_addr,
u32 len, u8 const *data)
{
int r, i, j;
#define DMP_MEM_CMP_SIZE 16
u8 w[DMP_MEM_CMP_SIZE];
bool retry;
j = 0;
retry = true;
while ((j < 3) && retry) {
retry = false;
r = _memory_write(st, mpu_addr, mem_addr, len, data);
if (len < DMP_MEM_CMP_SIZE) {
r = mem_r(mem_addr, len, w);
for (i = 0; i < len; i++) {
if (data[i] != w[i]) {
pr_debug
("error write=%x, len=%d,data=%x, w=%x, i=%d\n",
mem_addr, len, data[i], w[i], i);
retry = true;
}
}
}
j++;
}
return r;
}
static int inv_i2c_mem_read(struct inv_mpu_state *st, u8 mpu_addr, u16 mem_addr,
u32 len, u8 *data)
{
u8 bank[2];
u8 addr[2];
u8 buf;
struct i2c_msg msgs[4];
int res;
if (!data || !st)
return -EINVAL;
bank[0] = REG_MEM_BANK_SEL;
bank[1] = mem_addr >> 8;
addr[0] = REG_MEM_START_ADDR;
addr[1] = mem_addr & 0xFF;
buf = REG_MEM_R_W;
/* write message */
msgs[0].addr = mpu_addr;
msgs[0].flags = 0;
msgs[0].buf = bank;
msgs[0].len = sizeof(bank);
msgs[1].addr = mpu_addr;
msgs[1].flags = 0;
msgs[1].buf = addr;
msgs[1].len = sizeof(addr);
msgs[2].addr = mpu_addr;
msgs[2].flags = 0;
msgs[2].buf = &buf;
msgs[2].len = 1;
msgs[3].addr = mpu_addr;
msgs[3].flags = I2C_M_RD;
msgs[3].buf = data;
msgs[3].len = len;
res = i2c_transfer(st->sl_handle, msgs, 4);
if (res != 4) {
if (res >= 0)
res = -EIO;
} else
res = 0;
INV_I2C_INC_MPUWRITE(3 + 3 + 3);
INV_I2C_INC_MPUREAD(len);
#if CONFIG_DYNAMIC_DEBUG_I2C
{
char *read = 0;
pr_debug("%s RM%02X%02X%02X%02X - %s%s\n", st->hw->name,
mpu_addr, bank[1], addr[1], len,
wr_pr_debug_begin(data, len, read),
wr_pr_debug_end(read));
}
#endif
return res;
}
#endif
/*
* inv_mpu_probe() - probe function.
*/
static int inv_mpu_probe(struct i2c_client *client,
const struct i2c_device_id *id)
{
struct inv_mpu_state *st;
struct iio_dev *indio_dev;
int result;
if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
result = -ENOSYS;
pr_err("I2c function error\n");
goto out_no_free;
}
indio_dev = iio_device_alloc(sizeof(*st));
if (indio_dev == NULL) {
pr_err("memory allocation failed\n");
result = -ENOMEM;
goto out_no_free;
}
st = iio_priv(indio_dev);
st->client = client;
st->sl_handle = client->adapter;
st->i2c_addr = client->addr;
st->write = inv_i2c_single_write;
st->read = inv_i2c_read;
#if !defined(CONFIG_INV_MPU_IIO_ICM42600)
st->mem_write = inv_i2c_mem_write;
st->mem_read = inv_i2c_mem_read;
#endif
st->dev = &client->dev;
st->irq = client->irq;
st->bus_type = BUS_I2C;
i2c_set_clientdata(client, indio_dev);
indio_dev->dev.parent = &client->dev;
indio_dev->name = id->name;
#ifdef CONFIG_OF
result = invensense_mpu_parse_dt(st->dev, &st->plat_data);
if (result)
goto out_free;
#else
if (dev_get_platdata(st->dev) == NULL) {
result = -ENODEV;
goto out_free;
}
st->plat_data = *(struct mpu_platform_data *)dev_get_platdata(st->dev);
#endif
/* Power on device */
if (st->plat_data.power_on) {
result = st->plat_data.power_on(&st->plat_data);
if (result < 0) {
dev_err(st->dev, "power_on failed: %d\n", result);
goto out_free;
}
pr_info("%s: power on here.\n", __func__);
}
pr_info("%s: power on.\n", __func__);
msleep(100);
/* power is turned on inside check chip type */
result = inv_check_chip_type(indio_dev, id->name);
if (result)
goto out_free;
result = inv_mpu_configure_ring(indio_dev);
if (result) {
pr_err("configure ring buffer fail\n");
goto out_free;
}
result = iio_device_register(indio_dev);
if (result) {
pr_err("IIO device register fail\n");
goto out_unreg_ring;
}
result = inv_create_dmp_sysfs(indio_dev);
if (result) {
pr_err("create dmp sysfs failed\n");
goto out_unreg_iio;
}
init_waitqueue_head(&st->wait_queue);
st->resume_state = true;
#ifdef CONFIG_HAS_WAKELOCK
wake_lock_init(&st->wake_lock, WAKE_LOCK_SUSPEND, "inv_mpu");
#else
wakeup_source_init(&st->wake_lock, "inv_mpu");
#endif
dev_info(st->dev, "%s ma-kernel-%s is ready to go!\n",
indio_dev->name, INVENSENSE_DRIVER_VERSION);
#ifdef SENSOR_DATA_FROM_REGISTERS
pr_info("Data read from registers\n");
#else
pr_info("Data read from FIFO\n");
#endif
#ifdef TIMER_BASED_BATCHING
pr_info("Timer based batching\n");
#endif
return 0;
out_unreg_iio:
iio_device_unregister(indio_dev);
out_unreg_ring:
inv_mpu_unconfigure_ring(indio_dev);
out_free:
iio_device_free(indio_dev);
out_no_free:
dev_err(&client->dev, "%s failed %d\n", __func__, result);
return result;
}
static void inv_mpu_shutdown(struct i2c_client *client)
{
struct iio_dev *indio_dev = i2c_get_clientdata(client);
struct inv_mpu_state *st = iio_priv(indio_dev);
int result;
mutex_lock(&indio_dev->mlock);
inv_switch_power_in_lp(st, true);
dev_dbg(st->dev, "Shutting down %s...\n", st->hw->name);
/* reset to make sure previous state are not there */
#if defined(CONFIG_INV_MPU_IIO_ICM42600)
result = inv_plat_single_write(st, REG_CHIP_CONFIG_REG, BIT_SOFT_RESET);
#else
result = inv_plat_single_write(st, REG_PWR_MGMT_1, BIT_H_RESET);
#endif
if (result)
dev_err(st->dev, "Failed to reset %s\n",
st->hw->name);
msleep(POWER_UP_TIME);
/* turn off power to ensure gyro engine is off */
result = inv_set_power(st, false);
if (result)
dev_err(st->dev, "Failed to turn off %s\n",
st->hw->name);
inv_switch_power_in_lp(st, false);
mutex_unlock(&indio_dev->mlock);
}
/*
* inv_mpu_remove() - remove function.
*/
static int inv_mpu_remove(struct i2c_client *client)
{
struct iio_dev *indio_dev = i2c_get_clientdata(client);
struct inv_mpu_state *st = iio_priv(indio_dev);
if (st->aux_dev)
i2c_unregister_device(st->aux_dev);
iio_device_unregister(indio_dev);
inv_mpu_unconfigure_ring(indio_dev);
iio_device_free(indio_dev);
dev_info(st->dev, "inv-mpu-iio module removed.\n");
return 0;
}
#ifdef CONFIG_PM_SLEEP
static int inv_mpu_i2c_suspend(struct device *dev)
{
struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
return inv_mpu_suspend(indio_dev);
}
static void inv_mpu_i2c_complete(struct device *dev)
{
struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
inv_mpu_complete(indio_dev);
}
#endif
static const struct dev_pm_ops inv_mpu_i2c_pmops = {
#ifdef CONFIG_PM_SLEEP
.suspend = inv_mpu_i2c_suspend,
.complete = inv_mpu_i2c_complete,
#endif
};
/* device id table is used to identify what device can be
* supported by this driver
*/
static const struct i2c_device_id inv_mpu_id[] = {
#ifdef CONFIG_INV_MPU_IIO_ICM20648
{"icm20645", ICM20645},
{"icm10340", ICM10340},
{"icm20648", ICM20648},
#else
{"icm20608d", ICM20608D},
{"icm20789", ICM20789},
{"icm20690", ICM20690},
{"icm20602", ICM20602},
{"iam20680", IAM20680},
{"icm42600", ICM42600},
#endif
{}
};
MODULE_DEVICE_TABLE(i2c, inv_mpu_id);
static const struct of_device_id inv_mpu_of_match[] = {
#ifdef CONFIG_INV_MPU_IIO_ICM20648
{
.compatible = "invensense,icm20645",
.data = (void *)ICM20645,
}, {
.compatible = "invensense,icm10340",
.data = (void *)ICM10340,
}, {
.compatible = "invensense,icm20648",
.data = (void *)ICM20648,
},
#else
{
.compatible = "invensense,icm20608d",
.data = (void *)ICM20608D,
}, {
.compatible = "invensense,icm20789",
.data = (void *)ICM20789,
}, {
.compatible = "invensense,icm20690",
.data = (void *)ICM20690,
}, {
.compatible = "invensense,icm20602",
.data = (void *)ICM20602,
}, {
.compatible = "invensense,iam20680",
.data = (void *)IAM20680,
}, {
.compatible = "invensense,icm42600",
.data = (void *)ICM42600,
},
#endif
{ }
};
MODULE_DEVICE_TABLE(of, inv_mpu_of_match);
static struct i2c_driver inv_mpu_driver = {
.probe = inv_mpu_probe,
.remove = inv_mpu_remove,
.shutdown = inv_mpu_shutdown,
.id_table = inv_mpu_id,
.driver = {
.owner = THIS_MODULE,
.of_match_table = inv_mpu_of_match,
.name = "inv-mpu-iio-i2c",
.pm = &inv_mpu_i2c_pmops,
},
};
module_i2c_driver(inv_mpu_driver);
MODULE_AUTHOR("Invensense Corporation");
MODULE_DESCRIPTION("Invensense I2C device driver");
MODULE_LICENSE("GPL");

1187
drivers/iio/imu/inv_mpu/inv_mpu_iio.h Normal file
View File

File diff suppressed because it is too large Load Diff

629
drivers/iio/imu/inv_mpu/inv_mpu_ring.c Normal file
View File

@@ -0,0 +1,629 @@
/*
* Copyright (C) 2012-2018 InvenSense, Inc.
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#define pr_fmt(fmt) "inv_mpu: " fmt
#include <linux/export.h>
#include <linux/kernel.h>
#include <linux/device.h>
#include <linux/slab.h>
#include <linux/err.h>
#include <linux/delay.h>
#include <linux/sysfs.h>
#include <linux/jiffies.h>
#include <linux/irq.h>
#include <linux/interrupt.h>
#include <linux/kfifo.h>
#include <linux/poll.h>
#include <linux/math64.h>
#include <linux/iio/iio.h>
#include <linux/iio/buffer.h>
#include <linux/iio/trigger.h>
#include <linux/iio/triggered_buffer.h>
#include "inv_mpu_iio.h"
static void inv_push_timestamp(struct iio_dev *indio_dev, u64 t)
{
u8 buf[IIO_BUFFER_BYTES];
struct inv_mpu_state *st;
st = iio_priv(indio_dev);
if (st->poke_mode_on)
memcpy(buf, &st->poke_ts, sizeof(t));
else
memcpy(buf, &t, sizeof(t));
iio_push_to_buffers(indio_dev, buf);
}
int inv_push_marker_to_buffer(struct inv_mpu_state *st, u16 hdr, int data)
{
struct iio_dev *indio_dev = iio_priv_to_dev(st);
u8 buf[IIO_BUFFER_BYTES];
memcpy(buf, &hdr, sizeof(hdr));
memcpy(&buf[4], &data, sizeof(data));
iio_push_to_buffers(indio_dev, buf);
return 0;
}
static int inv_calc_precision(struct inv_mpu_state *st)
{
int diff;
int init;
if (st->eis.voting_state != 8)
return 0;
diff = abs(st->eis.fsync_delay_s[1] - st->eis.fsync_delay_s[0]);
init = 0;
if (diff)
init = st->sensor[SENSOR_GYRO].dur / diff;
if (abs(init - NSEC_PER_USEC) < (NSEC_PER_USEC >> 3))
st->eis.count_precision = init;
else
st->eis.voting_state = 0;
pr_debug("dur= %d prc= %d\n", st->sensor[SENSOR_GYRO].dur,
st->eis.count_precision);
return 0;
}
static s64 calc_frame_ave(struct inv_mpu_state *st, int delay)
{
s64 ts;
ts = st->eis.current_timestamp - delay;
#if defined(CONFIG_INV_MPU_IIO_ICM20648) | defined(CONFIG_INV_MPU_IIO_ICM20690)
ts -= st->ts_algo.gyro_ts_shift;
#endif
pr_debug("shift= %d ts = %lld\n", st->ts_algo.gyro_ts_shift, ts);
return ts;
}
static void inv_push_eis_ring(struct inv_mpu_state *st, int *q, bool sync,
s64 t)
{
struct iio_dev *indio_dev = iio_priv_to_dev(st);
struct inv_eis *eis = &st->eis;
u8 buf[IIO_BUFFER_BYTES];
int tmp, ii;
buf[0] = (EIS_GYRO_HDR & 0xff);
buf[1] = (EIS_GYRO_HDR >> 8);
memcpy(buf + 4, &q[0], sizeof(q[0]));
iio_push_to_buffers(indio_dev, buf);
for (ii = 0; ii < 2; ii++)
memcpy(buf + 4 * ii, &q[ii + 1], sizeof(q[ii]));
iio_push_to_buffers(indio_dev, buf);
tmp = eis->frame_count;
if (sync)
tmp |= 0x80000000;
memcpy(buf, &tmp, sizeof(tmp));
iio_push_to_buffers(indio_dev, buf);
inv_push_timestamp(indio_dev, t);
}
static int inv_do_interpolation_gyro(struct inv_mpu_state *st, int *prev,
s64 prev_t, int *curr, s64 curr_t, s64 t, bool trigger)
{
int i;
int out[3];
#if defined(CONFIG_INV_MPU_IIO_ICM20648) | defined(CONFIG_INV_MPU_IIO_ICM20690)
prev_t -= st->ts_algo.gyro_ts_shift;
prev_t += MPU_4X_TS_GYRO_SHIFT;
curr_t -= st->ts_algo.gyro_ts_shift;
curr_t += MPU_4X_TS_GYRO_SHIFT;
#endif
if ((t > prev_t) && (t < curr_t)) {
for (i = 0; i < 3; i++)
out[i] = (int)div_s64((s64)(curr[i] - prev[i]) *
(s64)(t - prev_t), curr_t - prev_t) + prev[i];
} else if (t < prev_t) {
for (i = 0; i < 3; i++)
out[i] = prev[i];
} else {
for (i = 0; i < 3; i++)
out[i] = curr[i];
}
pr_debug("prev= %lld t = %lld curr= %lld\n", prev_t, t, curr_t);
pr_debug("prev = %d, %d, %d\n", prev[0], prev[1], prev[2]);
pr_debug("curr = %d, %d, %d\n", curr[0], curr[1], curr[2]);
pr_debug("out = %d, %d, %d\n", out[0], out[1], out[2]);
inv_push_eis_ring(st, out, trigger, t);
return 0;
}
#if defined(CONFIG_INV_MPU_IIO_ICM20648) | defined(CONFIG_INV_MPU_IIO_ICM20690)
static void inv_handle_triggered_eis(struct inv_mpu_state *st)
{
struct inv_eis *eis = &st->eis;
int delay;
if (st->eis.eis_frame) {
inv_calc_precision(st);
delay = ((int)st->eis.fsync_delay) * st->eis.count_precision;
eis->fsync_timestamp = calc_frame_ave(st, delay);
inv_do_interpolation_gyro(st,
st->eis.prev_gyro, st->eis.prev_timestamp,
st->eis.current_gyro, st->eis.current_timestamp,
eis->fsync_timestamp, true);
pr_debug("fsync=%lld, curr=%lld, delay=%d\n",
eis->fsync_timestamp, eis->current_timestamp, delay);
inv_push_eis_ring(st, st->eis.current_gyro, false,
st->eis.current_timestamp - st->ts_algo.gyro_ts_shift
+ MPU_4X_TS_GYRO_SHIFT);
eis->last_fsync_timestamp = eis->fsync_timestamp;
} else {
pr_debug("cur= %lld\n", st->eis.current_timestamp);
inv_push_eis_ring(st, st->eis.current_gyro, false,
st->eis.current_timestamp - st->ts_algo.gyro_ts_shift
+ MPU_4X_TS_GYRO_SHIFT);
}
}
#else
static void inv_handle_triggered_eis(struct inv_mpu_state *st)
{
struct inv_eis *eis = &st->eis;
int delay;
if ((st->eis.eis_frame && (st->eis.fsync_delay != 5)) ||
(st->eis.eis_frame && (st->eis.fsync_delay == 5) &&
(!st->eis.current_sync))
) {
inv_calc_precision(st);
delay = ((int)st->eis.fsync_delay) * st->eis.count_precision;
eis->fsync_timestamp = calc_frame_ave(st, delay);
inv_do_interpolation_gyro(st,
st->eis.prev_gyro, st->eis.prev_timestamp,
st->eis.current_gyro, st->eis.current_timestamp,
eis->fsync_timestamp, true);
pr_debug("fsync=%lld, curr=%lld, delay=%d\n",
eis->fsync_timestamp, eis->current_timestamp, delay);
inv_push_eis_ring(st, st->eis.current_gyro, false,
st->eis.current_timestamp);
eis->last_fsync_timestamp = eis->fsync_timestamp;
st->eis.eis_frame = false;
} else {
st->eis.current_sync = false;
pr_debug("cur= %lld\n", st->eis.current_timestamp);
inv_push_eis_ring(st, st->eis.current_gyro, false,
st->eis.current_timestamp);
}
}
#endif
static void inv_push_eis_buffer(struct inv_mpu_state *st, u64 t, int *q)
{
int ii;
if (st->eis.eis_triggered) {
for (ii = 0; ii < 3; ii++)
st->eis.prev_gyro[ii] = st->eis.current_gyro[ii];
st->eis.prev_timestamp = st->eis.current_timestamp;
for (ii = 0; ii < 3; ii++)
st->eis.current_gyro[ii] = q[ii];
st->eis.current_timestamp = t;
inv_handle_triggered_eis(st);
} else {
for (ii = 0; ii < 3; ii++)
st->eis.current_gyro[ii] = q[ii];
st->eis.current_timestamp = t;
}
}
static int inv_push_16bytes_final(struct inv_mpu_state *st, int j,
s32 *q, u64 t, s16 accur)
{
struct iio_dev *indio_dev = iio_priv_to_dev(st);
u8 buf[IIO_BUFFER_BYTES];
int ii;
memcpy(buf, &st->sensor_l[j].header, sizeof(st->sensor_l[j].header));
memcpy(buf + 2, &accur, sizeof(accur));
memcpy(buf + 4, &q[0], sizeof(q[0]));
iio_push_to_buffers(indio_dev, buf);
for (ii = 0; ii < 2; ii++)
memcpy(buf + 4 * ii, &q[ii + 1], sizeof(q[ii]));
iio_push_to_buffers(indio_dev, buf);
inv_push_timestamp(indio_dev, t);
st->sensor_l[j].counter = 0;
if (st->sensor_l[j].wake_on)
st->wake_sensor_received = true;
return 0;
}
int inv_push_16bytes_buffer(struct inv_mpu_state *st, u16 sensor,
u64 t, int *q, s16 accur)
{
int j;
for (j = 0; j < SENSOR_L_NUM_MAX; j++) {
if (st->sensor_l[j].on && (st->sensor_l[j].base == sensor)) {
st->sensor_l[j].counter++;
if ((st->sensor_l[j].div != 0xffff) &&
(st->sensor_l[j].counter >=
st->sensor_l[j].div)) {
pr_debug(
"Sensor_l = %d sensor = %d header [%04X] div [%d] ts [%lld] %d %d %d\n",
j, sensor,
st->sensor_l[j].header,
st->sensor_l[j].div,
t, q[0], q[1], q[2]);
inv_push_16bytes_final(st, j, q, t, accur);
}
}
}
return 0;
}
void inv_convert_and_push_16bytes(struct inv_mpu_state *st, u16 hdr,
u8 *d, u64 t, s8 *m)
{
int i, j;
s32 in[3], out[3];
for (i = 0; i < 3; i++)
in[i] = be32_to_int(d + i * 4);
/* multiply with orientation matrix can be optimized like this */
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
if (m[i * 3 + j])
out[i] = in[j] * m[i * 3 + j];
inv_push_16bytes_buffer(st, hdr, t, out, 0);
}
void inv_convert_and_push_8bytes(struct inv_mpu_state *st, u16 hdr,
u8 *d, u64 t, s8 *m)
{
int i, j;
s16 in[3], out[3];
for (i = 0; i < 3; i++)
in[i] = be16_to_cpup((__be16 *) (d + i * 2));
/* multiply with orientation matrix can be optimized like this */
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
if (m[i * 3 + j])
out[i] = in[j] * m[i * 3 + j];
inv_push_8bytes_buffer(st, hdr, t, out);
}
int inv_push_special_8bytes_buffer(struct inv_mpu_state *st,
u16 hdr, u64 t, s16 *d)
{
struct iio_dev *indio_dev = iio_priv_to_dev(st);
u8 buf[IIO_BUFFER_BYTES];
int j;
memcpy(buf, &hdr, sizeof(hdr));
memcpy(&buf[2], &d[0], sizeof(d[0]));
for (j = 0; j < 2; j++)
memcpy(&buf[4 + j * 2], &d[j + 1], sizeof(d[j]));
iio_push_to_buffers(indio_dev, buf);
inv_push_timestamp(indio_dev, t);
return 0;
}
static int inv_s16_gyro_push(struct inv_mpu_state *st, int i, s16 *raw, u64 t)
{
if (st->sensor_l[i].on) {
st->sensor_l[i].counter++;
if ((st->sensor_l[i].div != 0xffff) &&
(st->sensor_l[i].counter >= st->sensor_l[i].div)) {
inv_push_special_8bytes_buffer(st,
st->sensor_l[i].header, t, raw);
st->sensor_l[i].counter = 0;
if (st->sensor_l[i].wake_on)
st->wake_sensor_received = true;
}
}
return 0;
}
static int inv_s32_gyro_push(struct inv_mpu_state *st, int i, s32 *calib, u64 t)
{
if (st->sensor_l[i].on) {
st->sensor_l[i].counter++;
if ((st->sensor_l[i].div != 0xffff) &&
(st->sensor_l[i].counter >= st->sensor_l[i].div)) {
inv_push_16bytes_final(st, i, calib, t, 0);
st->sensor_l[i].counter = 0;
if (st->sensor_l[i].wake_on)
st->wake_sensor_received = true;
}
}
return 0;
}
int inv_push_gyro_data(struct inv_mpu_state *st, s16 *raw, s32 *calib, u64 t)
{
int gyro_data[] = {SENSOR_L_GYRO, SENSOR_L_GYRO_WAKE};
int calib_data[] = {SENSOR_L_GYRO_CAL, SENSOR_L_GYRO_CAL_WAKE};
int i;
if (st->sensor_l[SENSOR_L_EIS_GYRO].on)
inv_push_eis_buffer(st, t, calib);
for (i = 0; i < 2; i++)
inv_s16_gyro_push(st, gyro_data[i], raw, t);
for (i = 0; i < 2; i++)
inv_s32_gyro_push(st, calib_data[i], calib, t);
return 0;
}
int inv_push_8bytes_buffer(struct inv_mpu_state *st, u16 sensor, u64 t, s16 *d)
{
struct iio_dev *indio_dev = iio_priv_to_dev(st);
u8 buf[IIO_BUFFER_BYTES];
int ii, j;
if ((sensor == STEP_DETECTOR_HDR) ||
(sensor == STEP_DETECTOR_WAKE_HDR) ||
(sensor == TAP_HDR)) {
memcpy(buf, &sensor, sizeof(sensor));
memcpy(&buf[2], &d[0], sizeof(d[0]));
for (j = 0; j < 2; j++)
memcpy(&buf[4 + j * 2], &d[j + 1], sizeof(d[j]));
iio_push_to_buffers(indio_dev, buf);
inv_push_timestamp(indio_dev, t);
if (sensor == STEP_DETECTOR_WAKE_HDR)
st->wake_sensor_received = true;
return 0;
}
for (ii = 0; ii < SENSOR_L_NUM_MAX; ii++) {
if (st->sensor_l[ii].on &&
(st->sensor_l[ii].base == sensor) &&
(st->sensor_l[ii].div != 0xffff)) {
st->sensor_l[ii].counter++;
if (st->sensor_l[ii].counter >= st->sensor_l[ii].div) {
pr_debug(
"Sensor_l = %d sensor = %d header [%04X] div [%d] ts [%lld] %d %d %d\n",
ii, sensor, st->sensor_l[ii].header,
st->sensor_l[ii].div, t, d[0], d[1], d[2]);
memcpy(buf, &st->sensor_l[ii].header,
sizeof(st->sensor_l[ii].header));
memcpy(&buf[2], &d[0], sizeof(d[0]));
for (j = 0; j < 2; j++)
memcpy(&buf[4 + j * 2], &d[j + 1],
sizeof(d[j]));
iio_push_to_buffers(indio_dev, buf);
inv_push_timestamp(indio_dev, t);
st->sensor_l[ii].counter = 0;
if (st->sensor_l[ii].wake_on)
st->wake_sensor_received = true;
}
}
}
return 0;
}
#ifdef CONFIG_INV_MPU_IIO_ICM20648
/* Implemented activity to string function for BAC test */
#define TILT_DETECTED 0x1000
#define NONE 0x00
#define DRIVE 0x01
#define WALK 0x02
#define RUN 0x04
#define BIKE 0x08
#define TILT 0x10
#define STILL 0x20
#define DRIVE_WALK (DRIVE | WALK)
#define DRIVE_RUN (DRIVE | RUN)
char *act_string(s16 data)
{
data &= (~TILT);
switch (data) {
case NONE:
return "None";
case DRIVE:
return "Drive";
case WALK:
return "Walk";
case RUN:
return "Run";
case BIKE:
return "Bike";
case STILL:
return "Still";
case DRIVE_WALK:
return "drive and walk";
case DRIVE_RUN:
return "drive and run";
default:
return "Unknown";
}
return "Unknown";
}
char *inv_tilt_check(s16 data)
{
if (data & TILT)
return "Tilt";
else
return "None";
}
int inv_push_8bytes_kf(struct inv_mpu_state *st, u16 hdr, u64 t, s16 *d)
{
struct iio_dev *indio_dev = iio_priv_to_dev(st);
u8 buf[IIO_BUFFER_BYTES];
int i;
if (st->chip_config.activity_on) {
memcpy(buf, &hdr, sizeof(hdr));
for (i = 0; i < 3; i++)
memcpy(&buf[2 + i * 2], &d[i], sizeof(d[i]));
kfifo_in(&st->kf, buf, IIO_BUFFER_BYTES);
memcpy(buf, &t, sizeof(t));
kfifo_in(&st->kf, buf, IIO_BUFFER_BYTES);
st->activity_size += IIO_BUFFER_BYTES * 2;
}
if (st->chip_config.tilt_enable) {
pr_debug("d[0] = %04X, [%X : %s] to [%X : %s]",
d[0], d[0] & 0x00FF,
inv_tilt_check(d[0] & 0x00FF),
(d[0] & 0xFF00) >> 8, inv_tilt_check((d[0] & 0xFF00) >> 8));
if (!((d[0] & 0x00FF) & TILT) &&
(((d[0] & 0xFF00) >> 8) & TILT)) {
/* Not Tilt to Tilt */
sysfs_notify(&indio_dev->dev.kobj, NULL, "poll_tilt");
st->wake_sensor_received = true; /* Tilt is wake-up */
}
}
pr_debug("d[0] = %04X, [%X : %s] to [%X : %s]", d[0], d[0] & 0x00FF,
act_string(d[0] & 0x00FF),
(d[0] & 0xFF00) >> 8, act_string((d[0] & 0xFF00) >> 8));
read_be32_from_mem(st, &st->bac_drive_conf, BAC_DRIVE_CONFIDENCE);
read_be32_from_mem(st, &st->bac_walk_conf, BAC_WALK_CONFIDENCE);
read_be32_from_mem(st, &st->bac_smd_conf, BAC_SMD_CONFIDENCE);
read_be32_from_mem(st, &st->bac_bike_conf, BAC_BIKE_CONFIDENCE);
read_be32_from_mem(st, &st->bac_still_conf, BAC_STILL_CONFIDENCE);
read_be32_from_mem(st, &st->bac_run_conf, BAC_RUN_CONFIDENCE);
return 0;
}
#endif
int inv_send_steps(struct inv_mpu_state *st, int step, u64 ts)
{
s16 s[3];
s[0] = 0;
s[1] = (s16) (step & 0xffff);
s[2] = (s16) ((step >> 16) & 0xffff);
if (st->step_counter_l_on)
inv_push_special_8bytes_buffer(st, STEP_COUNTER_HDR, ts, s);
if (st->step_counter_wake_l_on) {
inv_push_special_8bytes_buffer(st, STEP_COUNTER_WAKE_HDR,
ts, s);
st->wake_sensor_received = true;
}
return 0;
}
void inv_push_step_indicator(struct inv_mpu_state *st, u64 t)
{
s16 sen[3];
#define STEP_INDICATOR_HEADER 0x0001
sen[0] = 0;
sen[1] = 0;
sen[2] = 0;
inv_push_8bytes_buffer(st, STEP_INDICATOR_HEADER, t, sen);
}
#ifdef TIMER_BASED_BATCHING
static enum hrtimer_restart inv_batch_timer_handler(struct hrtimer *timer)
{
struct inv_mpu_state *st =
container_of(timer, struct inv_mpu_state, hr_batch_timer);
if (st->chip_config.gyro_enable || st->chip_config.accel_enable) {
hrtimer_forward_now(&st->hr_batch_timer,
ns_to_ktime(st->batch_timeout));
schedule_work(&st->batch_work);
return HRTIMER_RESTART;
}
st->is_batch_timer_running = 0;
return HRTIMER_NORESTART;
}
#endif
static int inv_mpu_set_trigger(struct iio_trigger *trig, bool state)
{
/* unused for the moment */
return 0;
}
static const struct iio_trigger_ops inv_mpu_trigger_ops = {
.set_trigger_state = &inv_mpu_set_trigger,
};
int inv_mpu_configure_ring(struct iio_dev *indio_dev)
{
int ret;
struct inv_mpu_state *st = iio_priv(indio_dev);
#ifdef TIMER_BASED_BATCHING
/* configure hrtimer */
hrtimer_init(&st->hr_batch_timer, CLOCK_BOOTTIME, HRTIMER_MODE_REL);
st->hr_batch_timer.function = inv_batch_timer_handler;
INIT_WORK(&st->batch_work, inv_batch_work);
#endif
ret = iio_triggered_buffer_setup(indio_dev, NULL, inv_read_fifo, NULL);
if (ret) {
dev_err(st->dev, "iio triggered buffer failed %d\n", ret);
return ret;
}
st->trig = iio_trigger_alloc("%s-dev%d", indio_dev->name,
indio_dev->id);
if (st->trig == NULL) {
ret = -ENOMEM;
dev_err(st->dev, "iio trigger alloc error\n");
goto error_free_buffer;
}
st->trig->dev.parent = st->dev;
st->trig->ops = &inv_mpu_trigger_ops;
iio_trigger_set_drvdata(st->trig, indio_dev);
ret = request_irq(st->irq, &iio_trigger_generic_data_rdy_poll,
IRQF_TRIGGER_RISING, "inv_mpu", st->trig);
if (ret) {
dev_err(st->dev, "irq request error %d\n", ret);
goto error_free_trigger;
}
ret = iio_trigger_register(st->trig);
if (ret) {
dev_err(st->dev, "iio trigger register error %d\n", ret);
goto error_free_irq;
}
iio_trigger_get(st->trig);
indio_dev->trig = st->trig;
return 0;
error_free_irq:
free_irq(st->irq, st->trig);
error_free_trigger:
iio_trigger_free(st->trig);
error_free_buffer:
iio_triggered_buffer_cleanup(indio_dev);
return ret;
}
EXPORT_SYMBOL_GPL(inv_mpu_configure_ring);
void inv_mpu_unconfigure_ring(struct iio_dev *indio_dev)
{
struct inv_mpu_state *st = iio_priv(indio_dev);
iio_trigger_unregister(st->trig);
free_irq(st->irq, st->trig);
iio_trigger_free(st->trig);
iio_triggered_buffer_cleanup(indio_dev);
};
EXPORT_SYMBOL_GPL(inv_mpu_unconfigure_ring);

399
drivers/iio/imu/inv_mpu/inv_mpu_spi.c Normal file
View File

@@ -0,0 +1,399 @@
/*
* Copyright (C) 2012-2018 InvenSense, Inc.
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#define pr_fmt(fmt) "inv_mpu: " fmt
#include <linux/module.h>
#include <linux/init.h>
#include <linux/slab.h>
#include <linux/spi/spi.h>
#include <linux/err.h>
#include <linux/delay.h>
#include <linux/sysfs.h>
#include <linux/jiffies.h>
#include <linux/irq.h>
#include <linux/interrupt.h>
#include <linux/kfifo.h>
#include <linux/poll.h>
#include <linux/miscdevice.h>
#include <linux/spinlock.h>
#include <linux/of_device.h>
#include "inv_mpu_iio.h"
#include "inv_mpu_dts.h"
#define INV_SPI_READ 0x80
static int inv_spi_single_write(struct inv_mpu_state *st, u8 reg, u8 data)
{
struct spi_message msg;
int res;
u8 d[2];
struct spi_transfer xfers = {
.tx_buf = d,
.bits_per_word = 8,
.len = 2,
};
pr_debug("reg_write: reg=0x%x data=0x%x\n", reg, data);
d[0] = reg;
d[1] = data;
spi_message_init(&msg);
spi_message_add_tail(&xfers, &msg);
res = spi_sync(to_spi_device(st->dev), &msg);
return res;
}
static int inv_spi_read(struct inv_mpu_state *st, u8 reg, int len, u8 *data)
{
struct spi_message msg;
int res;
u8 d[1];
struct spi_transfer xfers[] = {
{
.tx_buf = d,
.bits_per_word = 8,
.len = 1,
},
{
.rx_buf = data,
.bits_per_word = 8,
.len = len,
}
};
if (!data)
return -EINVAL;
d[0] = (reg | INV_SPI_READ);
spi_message_init(&msg);
spi_message_add_tail(&xfers[0], &msg);
spi_message_add_tail(&xfers[1], &msg);
res = spi_sync(to_spi_device(st->dev), &msg);
if (len ==1)
pr_debug("reg_read: reg=0x%x length=%d data=0x%x\n",
reg, len, data[0]);
else
pr_debug("reg_read: reg=0x%x length=%d d0=0x%x d1=0x%x\n",
reg, len, data[0], data[1]);
return res;
}
#if !defined(CONFIG_INV_MPU_IIO_ICM42600)
static int inv_spi_mem_write(struct inv_mpu_state *st, u8 mpu_addr, u16 mem_addr,
u32 len, u8 const *data)
{
struct spi_message msg;
u8 buf[258];
int res;
struct spi_transfer xfers = {
.tx_buf = buf,
.bits_per_word = 8,
.len = len + 1,
};
if (!data || !st)
return -EINVAL;
if (len > (sizeof(buf) - 1))
return -ENOMEM;
inv_plat_single_write(st, REG_MEM_BANK_SEL, mem_addr >> 8);
inv_plat_single_write(st, REG_MEM_START_ADDR, mem_addr & 0xFF);
buf[0] = REG_MEM_R_W;
memcpy(buf + 1, data, len);
spi_message_init(&msg);
spi_message_add_tail(&xfers, &msg);
res = spi_sync(to_spi_device(st->dev), &msg);
return res;
}
static int inv_spi_mem_read(struct inv_mpu_state *st, u8 mpu_addr, u16 mem_addr,
u32 len, u8 *data)
{
int res;
if (!data || !st)
return -EINVAL;
if (len > 256)
return -EINVAL;
res = inv_plat_single_write(st, REG_MEM_BANK_SEL, mem_addr >> 8);
res = inv_plat_single_write(st, REG_MEM_START_ADDR, mem_addr & 0xFF);
res = inv_plat_read(st, REG_MEM_R_W, len, data);
return res;
}
#endif
/*
* inv_mpu_probe() - probe function.
*/
static int inv_mpu_probe(struct spi_device *spi)
{
const struct spi_device_id *id = spi_get_device_id(spi);
struct inv_mpu_state *st;
struct iio_dev *indio_dev;
int result;
indio_dev = iio_device_alloc(sizeof(*st));
if (indio_dev == NULL) {
pr_err("memory allocation failed\n");
result = -ENOMEM;
goto out_no_free;
}
st = iio_priv(indio_dev);
st->write = inv_spi_single_write;
st->read = inv_spi_read;
#if !defined(CONFIG_INV_MPU_IIO_ICM42600)
st->mem_write = inv_spi_mem_write;
st->mem_read = inv_spi_mem_read;
#endif
st->dev = &spi->dev;
st->irq = spi->irq;
#if defined(CONFIG_INV_MPU_IIO_ICM42600)
st->i2c_dis = BIT_UI_SIFS_DISABLE_I2C;
#elif !defined(CONFIG_INV_MPU_IIO_ICM20602) \
&& !defined(CONFIG_INV_MPU_IIO_IAM20680)
st->i2c_dis = BIT_I2C_IF_DIS;
#endif
st->bus_type = BUS_SPI;
spi_set_drvdata(spi, indio_dev);
indio_dev->dev.parent = &spi->dev;
indio_dev->name = id->name;
#ifdef CONFIG_OF
result = invensense_mpu_parse_dt(st->dev, &st->plat_data);
if (result)
goto out_free;
#else
if (dev_get_platdata(st->dev) == NULL) {
result = -ENODEV;
goto out_free;
}
st->plat_data = *(struct mpu_platform_data *)dev_get_platdata(st->dev);
#endif
/* Power on device */
if (st->plat_data.power_on) {
result = st->plat_data.power_on(&st->plat_data);
if (result < 0) {
dev_err(st->dev, "power_on failed: %d\n", result);
goto out_free;
}
pr_info("%s: power on here.\n", __func__);
}
pr_info("%s: power on.\n", __func__);
msleep(100);
/* power is turned on inside check chip type */
result = inv_check_chip_type(indio_dev, id->name);
if (result)
goto out_free;
result = inv_mpu_configure_ring(indio_dev);
if (result) {
pr_err("configure ring buffer fail\n");
goto out_free;
}
result = iio_device_register(indio_dev);
if (result) {
pr_err("IIO device register fail\n");
goto out_unreg_ring;
}
result = inv_create_dmp_sysfs(indio_dev);
if (result) {
pr_err("create dmp sysfs failed\n");
goto out_unreg_iio;
}
init_waitqueue_head(&st->wait_queue);
st->resume_state = true;
#ifdef CONFIG_HAS_WAKELOCK
wake_lock_init(&st->wake_lock, WAKE_LOCK_SUSPEND, "inv_mpu");
#else
wakeup_source_init(&st->wake_lock, "inv_mpu");
#endif
dev_info(st->dev, "%s ma-kernel-%s is ready to go!\n",
indio_dev->name, INVENSENSE_DRIVER_VERSION);
#ifdef SENSOR_DATA_FROM_REGISTERS
pr_info("Data read from registers\n");
#else
pr_info("Data read from FIFO\n");
#endif
#ifdef TIMER_BASED_BATCHING
pr_info("Timer based batching\n");
#endif
return 0;
out_unreg_iio:
iio_device_unregister(indio_dev);
out_unreg_ring:
inv_mpu_unconfigure_ring(indio_dev);
out_free:
iio_device_free(indio_dev);
out_no_free:
dev_err(&spi->dev, "%s failed %d\n", __func__, result);
return result;
}
static void inv_mpu_shutdown(struct spi_device *spi)
{
struct iio_dev *indio_dev = spi_get_drvdata(spi);
struct inv_mpu_state *st = iio_priv(indio_dev);
int result;
mutex_lock(&indio_dev->mlock);
inv_switch_power_in_lp(st, true);
dev_dbg(st->dev, "Shutting down %s...\n", st->hw->name);
/* reset to make sure previous state are not there */
#if defined(CONFIG_INV_MPU_IIO_ICM42600)
result = inv_plat_single_write(st, REG_CHIP_CONFIG_REG, BIT_SOFT_RESET);
#else
result = inv_plat_single_write(st, REG_PWR_MGMT_1, BIT_H_RESET);
#endif
if (result)
dev_err(st->dev, "Failed to reset %s\n",
st->hw->name);
msleep(POWER_UP_TIME);
/* turn off power to ensure gyro engine is off */
result = inv_set_power(st, false);
if (result)
dev_err(st->dev, "Failed to turn off %s\n",
st->hw->name);
inv_switch_power_in_lp(st, false);
mutex_unlock(&indio_dev->mlock);
}
/*
* inv_mpu_remove() - remove function.
*/
static int inv_mpu_remove(struct spi_device *spi)
{
struct iio_dev *indio_dev = spi_get_drvdata(spi);
struct inv_mpu_state *st = iio_priv(indio_dev);
iio_device_unregister(indio_dev);
inv_mpu_unconfigure_ring(indio_dev);
iio_device_free(indio_dev);
dev_info(st->dev, "inv-mpu-iio module removed.\n");
return 0;
}
#ifdef CONFIG_PM_SLEEP
static int inv_mpu_spi_suspend(struct device *dev)
{
struct iio_dev *indio_dev = spi_get_drvdata(to_spi_device(dev));
return inv_mpu_suspend(indio_dev);
}
static void inv_mpu_spi_complete(struct device *dev)
{
struct iio_dev *indio_dev = spi_get_drvdata(to_spi_device(dev));
inv_mpu_complete(indio_dev);
}
#endif
static const struct dev_pm_ops inv_mpu_spi_pmops = {
#ifdef CONFIG_PM_SLEEP
.suspend = inv_mpu_spi_suspend,
.complete = inv_mpu_spi_complete,
#endif
};
/* device id table is used to identify what device can be
* supported by this driver
*/
static const struct spi_device_id inv_mpu_id[] = {
#ifdef CONFIG_INV_MPU_IIO_ICM20648
{"icm20645", ICM20645},
{"icm10340", ICM10340},
{"icm20648", ICM20648},
#else
{"icm20608d", ICM20608D},
{"icm20690", ICM20690},
{"icm20602", ICM20602},
{"iam20680", IAM20680},
{"icm42600", ICM42600},
#endif
{}
};
MODULE_DEVICE_TABLE(spi, inv_mpu_id);
static const struct of_device_id inv_mpu_of_match[] = {
#ifdef CONFIG_INV_MPU_IIO_ICM20648
{
.compatible = "invensense,icm20645",
.data = (void *)ICM20645,
}, {
.compatible = "invensense,icm10340",
.data = (void *)ICM10340,
}, {
.compatible = "invensense,icm20648",
.data = (void *)ICM20648,
},
#else
{
.compatible = "invensense,icm20608d",
.data = (void *)ICM20608D,
}, {
.compatible = "invensense,icm20690",
.data = (void *)ICM20690,
}, {
.compatible = "invensense,icm20602",
.data = (void *)ICM20602,
}, {
.compatible = "invensense,iam20680",
.data = (void *)IAM20680,
}, {
.compatible = "invensense,icm42600",
.data = (void *)ICM42600,
},
#endif
{ }
};
MODULE_DEVICE_TABLE(of, inv_mpu_of_match);
static struct spi_driver inv_mpu_driver = {
.probe = inv_mpu_probe,
.remove = inv_mpu_remove,
.shutdown = inv_mpu_shutdown,
.id_table = inv_mpu_id,
.driver = {
.owner = THIS_MODULE,
.of_match_table = inv_mpu_of_match,
.name = "inv-mpu-iio-spi",
.pm = &inv_mpu_spi_pmops,
},
};
module_spi_driver(inv_mpu_driver);
MODULE_AUTHOR("Invensense Corporation");
MODULE_DESCRIPTION("Invensense SPI device driver");
MODULE_LICENSE("GPL");

294
drivers/iio/imu/inv_mpu/inv_mpu_timestamp.c Normal file
View File

@@ -0,0 +1,294 @@
/*
* Copyright (C) 2012-2018 InvenSense, Inc.
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#define pr_fmt(fmt) "inv_mpu: " fmt
#include <linux/module.h>
#include <linux/init.h>
#include <linux/slab.h>
#include <linux/err.h>
#include <linux/delay.h>
#include <linux/sysfs.h>
#include <linux/jiffies.h>
#include <linux/irq.h>
#include <linux/interrupt.h>
#include <linux/kfifo.h>
#include <linux/poll.h>
#include <linux/miscdevice.h>
#include <linux/math64.h>
#include "inv_mpu_iio.h"
#define INV_TIME_CALIB_THRESHOLD_1 2
#define MIN_DELAY (3 * NSEC_PER_MSEC)
#define JITTER_THRESH (1 * NSEC_PER_MSEC)
int inv_update_dmp_ts(struct inv_mpu_state *st, int ind)
{
int i;
u32 counter;
u64 ts;
enum INV_ENGINE en_ind;
struct inv_timestamp_algo *ts_algo = &st->ts_algo;
u32 base_time;
u64 cal_period;
if (st->mode_1k_on)
cal_period = (NSEC_PER_SEC >> 2);
else
cal_period = 2 * NSEC_PER_SEC;
ts = ts_algo->last_run_time - st->sensor[ind].time_calib;
counter = st->sensor[ind].sample_calib;
en_ind = st->sensor[ind].engine_base;
if (en_ind != ts_algo->clock_base)
return 0;
/* we average over 2 seconds period to do the timestamp calculation */
if (ts < cal_period)
return 0;
/* this is the first time we do timestamp averaging, return */
/* after resume from suspend, the clock of linux has up to 1 seconds
drift. We should start from the resume clock instead of using clock
before resume */
if ((!st->sensor[ind].calib_flag) || ts_algo->resume_flag) {
st->sensor[ind].sample_calib = 0;
st->sensor[ind].time_calib = ts_algo->last_run_time;
st->sensor[ind].calib_flag = 1;
ts_algo->resume_flag = false;
return 0;
}
/* if the sample number in current FIFO is not zero and between now and
last update time is more than 2 seconds, we do calculation */
if ((counter > 0) &&
(ts_algo->last_run_time - st->eng_info[en_ind].last_update_time >
cal_period)) {
/* duration for each sensor */
st->sensor[ind].dur = (u32) div_u64(ts, counter);
/* engine duration derived from each sensor */
if (st->sensor[ind].div)
st->eng_info[en_ind].dur = st->sensor[ind].dur /
st->sensor[ind].div;
else
pr_err("sensor %d divider zero!\n", ind);
/* update base time for each sensor */
if (st->eng_info[en_ind].divider) {
base_time = (st->eng_info[en_ind].dur /
st->eng_info[en_ind].divider) *
st->eng_info[en_ind].orig_rate;
if (st->mode_1k_on)
st->eng_info[en_ind].base_time_1k = base_time;
else
st->eng_info[en_ind].base_time = base_time;
} else {
pr_err("engine %d divider zero!\n", en_ind);
}
st->eng_info[en_ind].last_update_time = ts_algo->last_run_time;
/* update all the sensors duration based on the same engine */
for (i = 0; i < SENSOR_NUM_MAX; i++) {
if (st->sensor[i].on &&
(st->sensor[i].engine_base == en_ind))
st->sensor[i].dur = st->sensor[i].div *
st->eng_info[en_ind].dur;
}
}
st->sensor[ind].sample_calib = 0;
st->sensor[ind].time_calib = ts_algo->last_run_time;
return 0;
}
/**
* int inv_get_last_run_time_non_dmp_record_mode(struct inv_mpu_state *st)
* This is the function to get last run time in non dmp and record mode.
* This function will update the last_run_time, which is important parameter
* in overall timestamp algorithm.
* return value: this function returns fifo count value.
*/
int inv_get_last_run_time_non_dmp_record_mode(struct inv_mpu_state *st)
{
long long t_pre, t_post, dur;
int fifo_count;
#ifndef SENSOR_DATA_FROM_REGISTERS
int res;
u8 data[2];
#endif
t_pre = get_time_ns();
#ifndef SENSOR_DATA_FROM_REGISTERS
res = inv_plat_read(st, REG_FIFO_COUNT_H, FIFO_COUNT_BYTE, data);
if (res) {
pr_info("read REG_FIFO_COUNT_H failed= %d\n", res);
return 0;
}
#endif
t_post = get_time_ns();
#ifdef SENSOR_DATA_FROM_REGISTERS
if (st->fifo_count_mode == BYTE_MODE)
fifo_count = st->batch.pk_size;
else
fifo_count = 1;
#else
fifo_count = be16_to_cpup((__be16 *) (data));
#endif
pr_debug("fifc=%d\n", fifo_count);
if (!fifo_count)
return 0;
if (st->special_mag_mode && (fifo_count == 2)) {
pr_debug("special trigger\n");
fifo_count = 1;
}
/* In non DMP mode, either gyro or accel duration is the duration
for each sample */
if (st->chip_config.gyro_enable)
dur = st->eng_info[ENGINE_GYRO].dur;
else
dur = st->eng_info[ENGINE_ACCEL].dur;
if (st->fifo_count_mode == BYTE_MODE) {
fifo_count /= st->batch.pk_size;
}
/* In record mode, each number in fifo_count is 1 record or 1 sample */
st->ts_algo.last_run_time += dur * fifo_count;
if (st->ts_algo.last_run_time < t_pre)
st->ts_algo.last_run_time = t_pre;
if (st->ts_algo.last_run_time > t_post)
st->ts_algo.last_run_time = t_post;
return fifo_count;
}
int inv_get_dmp_ts(struct inv_mpu_state *st, int i)
{
u64 current_time;
int expected_lower_duration, expected_upper_duration;
current_time = get_time_ns();
st->sensor[i].ts += st->sensor[i].dur + st->sensor[i].ts_adj;
if (st->sensor[i].ts < st->sensor[i].previous_ts)
st->sensor[i].ts = st->sensor[i].previous_ts + st->sensor[i].dur;
/* hifi sensor limits ts jitter to +/- 2% */
#if defined(CONFIG_INV_MPU_IIO_ICM20602) || \
defined(CONFIG_INV_MPU_IIO_ICM20690) || \
defined(CONFIG_INV_MPU_IIO_IAM20680) || \
defined(CONFIG_INV_MPU_IIO_ICM42600)
/* for no DMP devices */
expected_upper_duration = st->eng_info[st->sensor[i].engine_base].divider * 1020000;
expected_lower_duration = st->eng_info[st->sensor[i].engine_base].divider * 980000;
#else
/* for DMP enabled devices */
expected_upper_duration = NSEC_PER_SEC /
st->eng_info[st->sensor[i].engine_base].running_rate *
st->sensor[i].div / 1000 * 1020;
expected_lower_duration = NSEC_PER_SEC /
st->eng_info[st->sensor[i].engine_base].running_rate *
st->sensor[i].div / 1000 * 980;
#endif
if (st->sensor[i].ts < st->sensor[i].previous_ts + expected_lower_duration)
st->sensor[i].ts = st->sensor[i].previous_ts + expected_lower_duration;
if (st->sensor[i].ts > st->sensor[i].previous_ts + expected_upper_duration)
st->sensor[i].ts = st->sensor[i].previous_ts + expected_upper_duration;
if (st->sensor[i].ts > current_time)
st->sensor[i].ts = current_time;
st->sensor[i].previous_ts = st->sensor[i].ts;
pr_debug("ts=%lld, reset=%lld\n", st->sensor[i].ts, st->ts_algo.reset_ts);
if (st->sensor[i].ts < st->ts_algo.reset_ts) {
pr_debug("less than reset\n");
st->sensor[i].send = false;
} else {
st->sensor[i].send = true;
}
if (st->header_count == 1)
inv_update_dmp_ts(st, i);
return 0;
}
static void process_sensor_bounding(struct inv_mpu_state *st, int i)
{
s64 elaps_time, thresh1, thresh2;
struct inv_timestamp_algo *ts_algo = &st->ts_algo;
u32 dur;
elaps_time = ((u64) (st->sensor[i].dur)) * st->sensor[i].count;
thresh1 = ts_algo->last_run_time - elaps_time;
dur = max(st->sensor[i].dur, (int)MIN_DELAY);
thresh2 = thresh1 - dur;
if (thresh1 < 0)
thresh1 = 0;
if (thresh2 < 0)
thresh2 = 0;
st->sensor[i].ts_adj = 0;
if ((ts_algo->calib_counter >= INV_TIME_CALIB_THRESHOLD_1) &&
(!ts_algo->resume_flag)) {
if (st->sensor[i].ts < thresh2)
st->sensor[i].ts_adj = thresh2 - st->sensor[i].ts;
} else if ((ts_algo->calib_counter >=
INV_TIME_CALIB_THRESHOLD_1) && ts_algo->resume_flag) {
if (st->sensor[i].ts < thresh2)
st->sensor[i].ts = ts_algo->last_run_time -
elaps_time - JITTER_THRESH;
} else {
st->sensor[i].ts = ts_algo->last_run_time - elaps_time -
JITTER_THRESH;
st->sensor[i].previous_ts = st->sensor[i].ts;
}
if (st->sensor[i].ts > thresh1)
st->sensor[i].ts_adj = thresh1 - st->sensor[i].ts;
pr_debug("cali=%d\n", st->ts_algo.calib_counter);
pr_debug("adj= %lld\n", st->sensor[i].ts_adj);
pr_debug("dur= %d count= %d last= %lld\n", st->sensor[i].dur,
st->sensor[i].count, ts_algo->last_run_time);
if (st->sensor[i].ts_adj && (st->sensor[i].count > 1))
st->sensor[i].ts_adj = div_s64(st->sensor[i].ts_adj,
st->sensor[i].count);
}
/* inv_bound_timestamp (struct inv_mpu_state *st)
The purpose this function is to give a generic bound to each
sensor timestamp. The timestamp cannot exceed current time.
The timestamp cannot backwards one sample time either, otherwise, there
would be another sample in between. Using this principle, we can bound
the sensor samples */
int inv_bound_timestamp(struct inv_mpu_state *st)
{
int i;
struct inv_timestamp_algo *ts_algo = &st->ts_algo;
for (i = 0; i < SENSOR_NUM_MAX; i++) {
if (st->sensor[i].on) {
if (st->sensor[i].count) {
process_sensor_bounding(st, i);
} else if (ts_algo->calib_counter <
INV_TIME_CALIB_THRESHOLD_1) {
st->sensor[i].ts = ts_algo->reset_ts;
st->sensor[i].previous_ts = st->sensor[i].ts;
}
}
}
return 0;
}

13
drivers/iio/imu/inv_mpu/inv_test/Kconfig Normal file
View File

@@ -0,0 +1,13 @@
#
# Kconfig for Invensense IIO testing hooks
#
config INV_TESTING
boolean "Invensense IIO testing hooks"
depends on INV_MPU_IIO || INV_AMI306_IIO || INV_YAS530 || INV_HUB_IIO
default n
help
This flag enables display of additional testing information from the
Invensense IIO drivers
It also enables the I2C counters facility to perform IO profiling.
Some additional sysfs entries will appear when this flag is enabled.

6
drivers/iio/imu/inv_mpu/inv_test/Makefile Normal file
View File

@@ -0,0 +1,6 @@
#
# Makefile for Invensense IIO testing hooks.
#
obj-$(CONFIG_INV_TESTING) += inv_counters.o

159
drivers/iio/imu/inv_mpu/inv_test/inv_counters.c Normal file
View File

@@ -0,0 +1,159 @@
/*
* Copyright (C) 2012-2017 InvenSense, Inc.
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#include <linux/module.h>
#include <linux/init.h>
#include <linux/device.h>
#include <linux/miscdevice.h>
#include <linux/err.h>
#include <linux/sysfs.h>
#include <linux/kdev_t.h>
#include <linux/string.h>
#include <linux/jiffies.h>
#include <linux/spinlock.h>
#include <linux/kernel_stat.h>
#include "inv_counters.h"
static int mpu_irq;
static int accel_irq;
static int compass_irq;
struct inv_counters {
uint32_t i2c_tempreads;
uint32_t i2c_mpureads;
uint32_t i2c_mpuwrites;
uint32_t i2c_accelreads;
uint32_t i2c_accelwrites;
uint32_t i2c_compassreads;
uint32_t i2c_compasswrites;
uint32_t i2c_compassirq;
uint32_t i2c_accelirq;
};
static struct inv_counters Counters;
static ssize_t i2c_counters_show(struct class *cls,
struct class_attribute *attr, char *buf)
{
return scnprintf(buf, PAGE_SIZE,
"%ld.%03ld %u %u %u %u %u %u %u %u %u %u\n",
jiffies / HZ, ((jiffies % HZ) * (1024 / HZ)),
mpu_irq ? kstat_irqs(mpu_irq) : 0,
Counters.i2c_tempreads,
Counters.i2c_mpureads, Counters.i2c_mpuwrites,
accel_irq ? kstat_irqs(accel_irq) : Counters.i2c_accelirq,
Counters.i2c_accelreads, Counters.i2c_accelwrites,
compass_irq ? kstat_irqs(compass_irq) : Counters.i2c_compassirq,
Counters.i2c_compassreads, Counters.i2c_compasswrites);
}
void inv_iio_counters_set_i2cirq(enum irqtype type, int irq)
{
switch (type) {
case IRQ_MPU:
mpu_irq = irq;
break;
case IRQ_ACCEL:
accel_irq = irq;
break;
case IRQ_COMPASS:
compass_irq = irq;
break;
}
}
EXPORT_SYMBOL_GPL(inv_iio_counters_set_i2cirq);
void inv_iio_counters_tempread(int count)
{
Counters.i2c_tempreads += count;
}
EXPORT_SYMBOL_GPL(inv_iio_counters_tempread);
void inv_iio_counters_mpuread(int count)
{
Counters.i2c_mpureads += count;
}
EXPORT_SYMBOL_GPL(inv_iio_counters_mpuread);
void inv_iio_counters_mpuwrite(int count)
{
Counters.i2c_mpuwrites += count;
}
EXPORT_SYMBOL_GPL(inv_iio_counters_mpuwrite);
void inv_iio_counters_accelread(int count)
{
Counters.i2c_accelreads += count;
}
EXPORT_SYMBOL_GPL(inv_iio_counters_accelread);
void inv_iio_counters_accelwrite(int count)
{
Counters.i2c_accelwrites += count;
}
EXPORT_SYMBOL_GPL(inv_iio_counters_accelwrite);
void inv_iio_counters_compassread(int count)
{
Counters.i2c_compassreads += count;
}
EXPORT_SYMBOL_GPL(inv_iio_counters_compassread);
void inv_iio_counters_compasswrite(int count)
{
Counters.i2c_compasswrites += count;
}
EXPORT_SYMBOL_GPL(inv_iio_counters_compasswrite);
void inv_iio_counters_compassirq(void)
{
Counters.i2c_compassirq++;
}
EXPORT_SYMBOL_GPL(inv_iio_counters_compassirq);
void inv_iio_counters_accelirq(void)
{
Counters.i2c_accelirq++;
}
EXPORT_SYMBOL_GPL(inv_iio_counters_accelirq);
static struct class_attribute inv_class_attr[] = {
__ATTR(i2c_counter, S_IRUGO, i2c_counters_show, NULL),
__ATTR_NULL
};
static struct class inv_counters_class = {
.name = "inv_counters",
.owner = THIS_MODULE,
.class_attrs = (struct class_attribute *) &inv_class_attr
};
static int __init inv_counters_init(void)
{
memset(&Counters, 0, sizeof(Counters));
return class_register(&inv_counters_class);
}
static void __exit inv_counters_exit(void)
{
class_unregister(&inv_counters_class);
}
module_init(inv_counters_init);
module_exit(inv_counters_exit);
MODULE_LICENSE("GPL");
MODULE_AUTHOR("GESL");
MODULE_DESCRIPTION("inv_counters debug support");

76
drivers/iio/imu/inv_mpu/inv_test/inv_counters.h Normal file
View File

@@ -0,0 +1,76 @@
/*
* Copyright (C) 2012-2017 InvenSense, Inc.
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#ifndef _INV_COUNTERS_H_
#define _INV_COUNTERS_H_
#include <linux/module.h>
#include <linux/init.h>
#include <linux/err.h>
#include <linux/sysfs.h>
#include <linux/string.h>
#include <linux/jiffies.h>
#include <linux/spinlock.h>
#ifdef CONFIG_INV_TESTING
enum irqtype {
IRQ_MPU,
IRQ_ACCEL,
IRQ_COMPASS
};
#define INV_I2C_INC_MPUREAD(x) inv_iio_counters_mpuread(x)
#define INV_I2C_INC_MPUWRITE(x) inv_iio_counters_mpuwrite(x)
#define INV_I2C_INC_ACCELREAD(x) inv_iio_counters_accelread(x)
#define INV_I2C_INC_ACCELWRITE(x) inv_iio_counters_accelwrite(x)
#define INV_I2C_INC_COMPASSREAD(x) inv_iio_counters_compassread(x)
#define INV_I2C_INC_COMPASSWRITE(x) inv_iio_counters_compasswrite(x)
#define INV_I2C_INC_TEMPREAD(x) inv_iio_counters_tempread(x)
#define INV_I2C_SETIRQ(type, irq) inv_iio_counters_set_i2cirq(type, irq)
#define INV_I2C_INC_COMPASSIRQ() inv_iio_counters_compassirq()
#define INV_I2C_INC_ACCELIRQ() inv_iio_counters_accelirq()
void inv_iio_counters_mpuread(int count);
void inv_iio_counters_mpuwrite(int count);
void inv_iio_counters_accelread(int count);
void inv_iio_counters_accelwrite(int count);
void inv_iio_counters_compassread(int count);
void inv_iio_counters_compasswrite(int count);
void inv_iio_counters_tempread(int count);
void inv_iio_counters_set_i2cirq(enum irqtype type, int irq);
void inv_iio_counters_compassirq(void);
void inv_iio_counters_accelirq(void);
#else
#define INV_I2C_INC_MPUREAD(x)
#define INV_I2C_INC_MPUWRITE(x)
#define INV_I2C_INC_ACCELREAD(x)
#define INV_I2C_INC_ACCELWRITE(x)
#define INV_I2C_INC_COMPASSREAD(x)
#define INV_I2C_INC_COMPASSWRITE(x)
#define INV_I2C_INC_TEMPREAD(x)
#define INV_I2C_SETIRQ(type, irq)
#define INV_I2C_INC_COMPASSIRQ()
#define INV_I2C_INC_ACCELIRQ()
#endif /* CONFIG_INV_TESTING */
#endif /* _INV_COUNTERS_H_ */

124
include/linux/iio/imu/mpu.h Normal file
View File

@@ -0,0 +1,124 @@
/*
* Copyright (C) 2012-2017 InvenSense, Inc.
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#ifndef __MPU_H_
#define __MPU_H_
#ifdef __KERNEL__
#include <linux/types.h>
#include <linux/ioctl.h>
#endif
enum secondary_slave_type {
SECONDARY_SLAVE_TYPE_NONE,
SECONDARY_SLAVE_TYPE_ACCEL,
SECONDARY_SLAVE_TYPE_COMPASS,
SECONDARY_SLAVE_TYPE_PRESSURE,
SECONDARY_SLAVE_TYPE_ALS,
SECONDARY_SLAVE_TYPE_TYPES
};
enum ext_slave_id {
ID_INVALID = 0,
GYRO_ID_MPU3050,
GYRO_ID_MPU6050A2,
GYRO_ID_MPU6050B1,
GYRO_ID_MPU6050B1_NO_ACCEL,
GYRO_ID_ITG3500,
ACCEL_ID_LIS331,
ACCEL_ID_LSM303DLX,
ACCEL_ID_LIS3DH,
ACCEL_ID_KXSD9,
ACCEL_ID_KXTF9,
ACCEL_ID_BMA150,
ACCEL_ID_BMA222,
ACCEL_ID_BMA250,
ACCEL_ID_ADXL34X,
ACCEL_ID_MMA8450,
ACCEL_ID_MMA845X,
ACCEL_ID_MPU6050,
COMPASS_ID_AK8963,
COMPASS_ID_AK8975,
COMPASS_ID_AK8972,
COMPASS_ID_AMI30X,
COMPASS_ID_AMI306,
COMPASS_ID_YAS529,
COMPASS_ID_YAS530,
COMPASS_ID_HMC5883,
COMPASS_ID_LSM303DLH,
COMPASS_ID_LSM303DLM,
COMPASS_ID_MMC314X,
COMPASS_ID_HSCDTD002B,
COMPASS_ID_HSCDTD004A,
COMPASS_ID_MLX90399,
COMPASS_ID_AK09911,
COMPASS_ID_AK09912,
COMPASS_ID_AK09916,
PRESSURE_ID_BMP085,
PRESSURE_ID_BMP280,
ALS_ID_APDS_9900,
ALS_ID_APDS_9930,
ALS_ID_TSL_2772,
};
#define INV_PROD_KEY(ver, rev) (ver * 100 + rev)
/**
* struct mpu_platform_data - Platform data for the mpu driver
* @int_config: Bits [7:3] of the int config register.
* @level_shifter: 0: VLogic, 1: VDD
* @orientation: Orientation matrix of the gyroscope
* @sec_slave_type: secondary slave device type, can be compass, accel, etc
* @sec_slave_id: id of the secondary slave device
* @secondary_i2c_address: secondary device's i2c address
* @secondary_orientation: secondary device's orientation matrix
* @aux_slave_type: auxiliary slave. Another slave device type
* @aux_slave_id: auxiliary slave ID.
* @aux_i2c_addr: auxiliary device I2C address.
* @read_only_slave_type: read only slave type.
* @read_only_slave_id: read only slave device ID.
* @read_only_i2c_addr: read only slave device address.
*
* Contains platform specific information on how to configure the MPU3050 to
* work on this platform. The orientation matricies are 3x3 rotation matricies
* that are applied to the data to rotate from the mounting orientation to the
* platform orientation. The values must be one of 0, 1, or -1 and each row and
* column should have exactly 1 non-zero value.
*/
struct mpu_platform_data {
__u8 int_config;
__u8 level_shifter;
__s8 orientation[9];
enum secondary_slave_type sec_slave_type;
enum ext_slave_id sec_slave_id;
__u16 secondary_i2c_addr;
__s8 secondary_orientation[9];
enum secondary_slave_type aux_slave_type;
enum ext_slave_id aux_slave_id;
__u16 aux_i2c_addr;
enum secondary_slave_type read_only_slave_type;
enum ext_slave_id read_only_slave_id;
__u16 read_only_i2c_addr;
#ifdef CONFIG_OF
int (*power_on)(struct mpu_platform_data *);
int (*power_off)(struct mpu_platform_data *);
struct regulator *vdd_ana;
struct regulator *vdd_i2c;
#endif
};
#endif /* __MPU_H_ */