diff --git a/drivers/iio/imu/inv_mpu/Kconfig b/drivers/iio/imu/inv_mpu/Kconfig new file mode 100644 index 000000000000..7505454f8763 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/Kconfig @@ -0,0 +1,63 @@ +# +# inv-mpu-iio driver for Invensense MPU devices +# + +config INV_MPU_IIO + tristate + select IIO_BUFFER + select IIO_KFIFO_BUF + select IIO_TRIGGER + 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/ICM20609/ICM20689" + help + Select this if you are using a ICM20608D/ICM20609/ICM20689 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. + +endchoice + +config INV_MPU_IIO_I2C + tristate "Invensense ICM20xxx devices (I2C)" + depends on I2C && !INV_MPU6050_IIO + select INV_MPU_IIO + default n + 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 + default n + 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" diff --git a/drivers/iio/imu/inv_mpu/Makefile b/drivers/iio/imu/inv_mpu/Makefile new file mode 100644 index 000000000000..dfc4c257ef73 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/Makefile @@ -0,0 +1,61 @@ +# +# 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 +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/ diff --git a/drivers/iio/imu/inv_mpu/README b/drivers/iio/imu/inv_mpu/README new file mode 100644 index 000000000000..47ff5029ee6e --- /dev/null +++ b/drivers/iio/imu/inv_mpu/README @@ -0,0 +1,117 @@ +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 /include/linux/iio/imu/ +- Copy inv_mpu folder under /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 /drivers/iio/imu/Kconfig + + add "obj-y += inv_mpu/" + in /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. + + +License +======= +Copyright (C) 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. + diff --git a/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_core_20680.c b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_core_20680.c new file mode 100644 index 000000000000..b429f57be5ca --- /dev/null +++ b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_core_20680.c @@ -0,0 +1,1072 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../inv_mpu_iio.h" + +static const struct inv_hw_s hw_info[INV_NUM_PARTS] = { + {128, "ICM20608D"}, + {128, "ICM20690"}, + {128, "ICM20602"}, + {128, "IAM20680"}, +}; + +#ifndef SUPPORT_ONLY_BASIC_FEATURES +static char debug_reg_addr = 0x6; +#endif + +const char sensor_l_info[][30] = { + "SENSOR_L_ACCEL", + "SENSOR_L_GYRO", + "SENSOR_L_MAG", + "SENSOR_L_ALS", + "SENSOR_L_SIXQ", + "SENSOR_L_THREEQ", + "SENSOR_L_NINEQ", + "SENSOR_L_PEDQ", + "SENSOR_L_GEOMAG", + "SENSOR_L_PRESSURE", + "SENSOR_L_GYRO_CAL", + "SENSOR_L_MAG_CAL", + "SENSOR_L_EIS_GYRO", + "SENSOR_L_ACCEL_WAKE", + "SENSOR_L_GYRO_WAKE", + "SENSOR_L_MAG_WAKE", + "SENSOR_L_ALS_WAKE", + "SENSOR_L_SIXQ_WAKE", + "SENSOR_L_NINEQ_WAKE", + "SENSOR_L_PEDQ_WAKE", + "SENSOR_L_GEOMAG_WAKE", + "SENSOR_L_PRESSURE_WAKE", + "SENSOR_L_GYRO_CAL_WAKE", + "SENSOR_L_MAG_CAL_WAKE", + "SENSOR_L_NUM_MAX", +}; + +static int inv_set_accel_bias_reg(struct inv_mpu_state *st, + int accel_bias, int axis) +{ + int accel_reg_bias; + u8 addr; + u8 d[2]; + int result = 0; + + switch (axis) { + case 0: + /* X */ + addr = REG_XA_OFFS_H; + break; + case 1: + /* Y */ + addr = REG_YA_OFFS_H; + break; + case 2: + /* Z* */ + addr = REG_ZA_OFFS_H; + break; + default: + result = -EINVAL; + goto accel_bias_set_err; + } + + result = inv_plat_read(st, addr, 2, d); + if (result) + goto accel_bias_set_err; + accel_reg_bias = ((int)d[0] << 8) | d[1]; + + /* accel_bias is 2g scaled by 1<<16. + * Convert to 16g, and mask bit0 */ + accel_reg_bias -= ((accel_bias / 8 / 65536) & ~1); + + d[0] = (accel_reg_bias >> 8) & 0xff; + d[1] = (accel_reg_bias) & 0xff; + result = inv_plat_single_write(st, addr, d[0]); + if (result) + goto accel_bias_set_err; + result = inv_plat_single_write(st, addr + 1, d[1]); + if (result) + goto accel_bias_set_err; + +accel_bias_set_err: + return result; +} + +static int inv_set_gyro_bias_reg(struct inv_mpu_state *st, + const int gyro_bias, int axis) +{ + int gyro_reg_bias; + u8 addr; + u8 d[2]; + int result = 0; + + switch (axis) { + case 0: + /* X */ + addr = REG_XG_OFFS_USR_H; + break; + case 1: + /* Y */ + addr = REG_YG_OFFS_USR_H; + break; + case 2: + /* Z */ + addr = REG_ZG_OFFS_USR_H; + break; + default: + result = -EINVAL; + goto gyro_bias_set_err; + } + + /* gyro_bias is 2000dps scaled by 1<<16. + * Convert to 1000dps */ + gyro_reg_bias = (-gyro_bias * 2 / 65536); + + d[0] = (gyro_reg_bias >> 8) & 0xff; + d[1] = (gyro_reg_bias) & 0xff; + result = inv_plat_single_write(st, addr, d[0]); + if (result) + goto gyro_bias_set_err; + result = inv_plat_single_write(st, addr + 1, d[1]); + if (result) + goto gyro_bias_set_err; + +gyro_bias_set_err: + return result; +} + +static int _bias_store(struct device *dev, + struct device_attribute *attr, const char *buf, + size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); + int result, data; + + result = inv_switch_power_in_lp(st, true); + if (result) + return result; + + result = kstrtoint(buf, 10, &data); + if (result) + goto bias_store_fail; + switch (this_attr->address) { + case ATTR_ACCEL_X_OFFSET: + result = inv_set_accel_bias_reg(st, data, 0); + if (result) + goto bias_store_fail; + st->input_accel_bias[0] = data; + break; + case ATTR_ACCEL_Y_OFFSET: + result = inv_set_accel_bias_reg(st, data, 1); + if (result) + goto bias_store_fail; + st->input_accel_bias[1] = data; + break; + case ATTR_ACCEL_Z_OFFSET: + result = inv_set_accel_bias_reg(st, data, 2); + if (result) + goto bias_store_fail; + st->input_accel_bias[2] = data; + break; + case ATTR_GYRO_X_OFFSET: + result = inv_set_gyro_bias_reg(st, data, 0); + if (result) + goto bias_store_fail; + st->input_gyro_bias[0] = data; + break; + case ATTR_GYRO_Y_OFFSET: + result = inv_set_gyro_bias_reg(st, data, 1); + if (result) + goto bias_store_fail; + st->input_gyro_bias[1] = data; + break; + case ATTR_GYRO_Z_OFFSET: + result = inv_set_gyro_bias_reg(st, data, 2); + if (result) + goto bias_store_fail; + st->input_gyro_bias[2] = data; + break; + default: + break; + } + +bias_store_fail: + if (result) + return result; + result = inv_switch_power_in_lp(st, false); + if (result) + return result; + + return count; +} + +static ssize_t inv_bias_store(struct device *dev, + struct device_attribute *attr, const char *buf, + size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + int result; + + mutex_lock(&indio_dev->mlock); + result = _bias_store(dev, attr, buf, count); + mutex_unlock(&indio_dev->mlock); + + return result; +} + +#ifndef SUPPORT_ONLY_BASIC_FEATURES +static ssize_t inv_debug_store(struct device *dev, + struct device_attribute *attr, const char *buf, + size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); + int result, data; + + result = kstrtoint(buf, 10, &data); + if (result) + return result; + switch (this_attr->address) { + case ATTR_DMP_LP_EN_OFF: + st->chip_config.lp_en_mode_off = !!data; + inv_switch_power_in_lp(st, !!data); + break; + case ATTR_DMP_CLK_SEL: + st->chip_config.clk_sel = !!data; + inv_switch_power_in_lp(st, !!data); + break; + case ATTR_DEBUG_REG_ADDR: + debug_reg_addr = data; + break; + case ATTR_DEBUG_REG_WRITE: + inv_plat_single_write(st, debug_reg_addr, data); + break; + } + return count; +} +#endif + +static int _misc_attr_store(struct device *dev, + struct device_attribute *attr, const char *buf, + size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); + int result, data; + + result = inv_switch_power_in_lp(st, true); + if (result) + return result; + result = kstrtoint(buf, 10, &data); + if (result) + return result; + switch (this_attr->address) { + case ATTR_GYRO_SCALE: + if (data > 3) + return -EINVAL; + st->chip_config.fsr = data; + result = inv_set_gyro_sf(st); + return result; + case ATTR_ACCEL_SCALE: + if (data > 3) + return -EINVAL; + st->chip_config.accel_fs = data; + result = inv_set_accel_sf(st); + return result; + default: + return -EINVAL; + } + st->trigger_state = MISC_TRIGGER; + result = set_inv_enable(indio_dev); + + return result; +} + +/* + * inv_misc_attr_store() - calling this function + */ +static ssize_t inv_misc_attr_store(struct device *dev, + struct device_attribute *attr, const char *buf, + size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + int result; + + mutex_lock(&indio_dev->mlock); + result = _misc_attr_store(dev, attr, buf, count); + mutex_unlock(&indio_dev->mlock); + if (result) + return result; + + return count; +} + +static ssize_t inv_sensor_rate_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); + + return snprintf(buf, MAX_WR_SZ, "%d\n", + st->sensor_l[this_attr->address].rate); +} + +static ssize_t inv_sensor_rate_store(struct device *dev, + struct device_attribute *attr, const char *buf, + size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); + int data, rate, ind; + int result; + + result = kstrtoint(buf, 10, &data); + if (result) + return -EINVAL; + if (data <= 0) { + pr_err("sensor_rate_store: invalid data=%d\n", data); + return -EINVAL; + } + ind = this_attr->address; + rate = inv_rate_convert(st, ind, data); + + pr_debug("sensor [%s] requested rate %d input [%d]\n", + sensor_l_info[ind], rate, data); + + if (rate == st->sensor_l[ind].rate) + return count; + mutex_lock(&indio_dev->mlock); + st->sensor_l[ind].rate = rate; + st->trigger_state = DATA_TRIGGER; + inv_check_sensor_on(st); + result = set_inv_enable(indio_dev); + pr_debug("%s rate %d div %d\n", sensor_l_info[ind], + st->sensor_l[ind].rate, st->sensor_l[ind].div); + mutex_unlock(&indio_dev->mlock); + + return count; +} + +static ssize_t inv_sensor_on_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); + + return snprintf(buf, MAX_WR_SZ, "%d\n", st->sensor_l[this_attr->address].on); +} + +static ssize_t inv_sensor_on_store(struct device *dev, + struct device_attribute *attr, const char *buf, + size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); + int data, on, ind; + int result; + + result = kstrtoint(buf, 10, &data); + if (result) + return -EINVAL; + if (data < 0) { + pr_err("sensor_on_store: invalid data=%d\n", data); + return -EINVAL; + } + ind = this_attr->address; + on = !!data; + + pr_debug("sensor [%s] requested %s, input [%d]\n", + sensor_l_info[ind], (on == 1) ? "On" : "Off", data); + + if (on == st->sensor_l[ind].on) { + pr_debug("sensor [%s] is already %s, input [%d]\n", + sensor_l_info[ind], (on == 1) ? "On" : "Off", data); + return count; + } + + mutex_lock(&indio_dev->mlock); + st->sensor_l[ind].on = on; + st->trigger_state = RATE_TRIGGER; + inv_check_sensor_on(st); + result = set_inv_enable(indio_dev); + mutex_unlock(&indio_dev->mlock); + if (result) + return result; + + pr_debug("Sensor [%s] is %s by sysfs\n", + sensor_l_info[ind], (on == 1) ? "On" : "Off"); + return count; +} + +static int inv_check_l_step(struct inv_mpu_state *st) +{ + if (st->step_counter_l_on || st->step_counter_wake_l_on) + st->ped.on = true; + else + st->ped.on = false; + + return 0; +} + +static int _basic_attr_store(struct device *dev, + struct device_attribute *attr, const char *buf, + size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); + int data; + int result; + u32 power_on_data; + + result = kstrtoint(buf, 10, &data); + if (result || (data < 0)) + return -EINVAL; + + switch (this_attr->address) { + case ATTR_DMP_PED_ON: + if ((!!data) == st->ped.on) + return count; + st->ped.on = !!data; + break; + case ATTR_DMP_TILT_ENABLE: + if ((!!data) == st->chip_config.tilt_enable) + return count; + st->chip_config.tilt_enable = !!data; + pr_info("Tile %s\n", + st->chip_config.tilt_enable == + 1 ? "Enabled" : "Disabled"); + break; + case ATTR_DMP_PICK_UP_ENABLE: + if ((!!data) == st->chip_config.pick_up_enable) { + pr_info("Pick_up enable already %s\n", + st->chip_config.pick_up_enable == + 1 ? "Enabled" : "Disabled"); + return count; + } + st->chip_config.pick_up_enable = !!data; + pr_info("Pick up %s\n", + st->chip_config.pick_up_enable == + 1 ? "Enable" : "Disable"); + break; + case ATTR_IN_POWER_ON: + { + u8 p0[2]; + u8 p1[2]; + + power_on_data = (u32)data; + p0[0] = (power_on_data & 0xff); + p0[1] = ((power_on_data >> 8) & 0xff); + p1[0] = ((power_on_data >> 16) & 0xff); + p1[1] = ((power_on_data >> 24) & 0xff); + + if (st->bus_type == BUS_SPI) { + struct spi_transfer power_on; + struct spi_message msg; + + memset(&power_on, 0, sizeof(struct spi_transfer)); + + power_on.bits_per_word = 8; + power_on.len = 2; + + power_on.tx_buf = p0; + power_on.rx_buf = p1; + spi_message_init(&msg); + spi_message_add_tail(&power_on, &msg); + spi_sync(to_spi_device(st->dev), &msg); + + } else if (st->bus_type == BUS_I2C) { + struct i2c_msg msgs[2]; + + p0[0] &= 0x7f; + + msgs[0].addr = st->i2c_addr; + msgs[0].flags = 0; /* write */ + msgs[0].buf = &p0[0]; + msgs[0].len = 1; + + msgs[1].addr = st->i2c_addr; + msgs[1].flags = I2C_M_RD; + msgs[1].buf = &p1[1]; + msgs[1].len = 1; + + result = i2c_transfer(st->sl_handle, msgs, 2); + if (result < 2) + return -EIO; + } + st->power_on_data = ((p0[0] << 24) | (p0[1] << 16) | + (p1[0] << 8) | p1[1]); + return count; + } + case ATTR_DMP_EIS_ENABLE: + if ((!!data) == st->chip_config.eis_enable) + return count; + st->chip_config.eis_enable = !!data; + pr_info("Eis %s\n", + st->chip_config.eis_enable == 1 ? "Enable" : "Disable"); + break; + case ATTR_DMP_STEP_DETECTOR_ON: + st->step_detector_l_on = !!data; + break; + case ATTR_DMP_STEP_DETECTOR_WAKE_ON: + st->step_detector_wake_l_on = !!data; + break; + case ATTR_DMP_STEP_COUNTER_ON: + st->step_counter_l_on = !!data; + break; + case ATTR_DMP_STEP_COUNTER_WAKE_ON: + st->step_counter_wake_l_on = !!data; + break; + case ATTR_DMP_BATCHMODE_TIMEOUT: + if (data == st->batch.timeout) + return count; + st->batch.timeout = data; + break; + default: + return -EINVAL; + }; + inv_check_l_step(st); + inv_check_sensor_on(st); + + st->trigger_state = EVENT_TRIGGER; + result = set_inv_enable(indio_dev); + if (result) + return result; + + return count; +} + +/* + * inv_basic_attr_store() + */ +static ssize_t inv_basic_attr_store(struct device *dev, + struct device_attribute *attr, const char *buf, + size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + int result; + + mutex_lock(&indio_dev->mlock); + result = _basic_attr_store(dev, attr, buf, count); + + mutex_unlock(&indio_dev->mlock); + + return result; +} + +/* + * inv_attr_show() + */ +static ssize_t inv_attr_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); + s8 *m; + + switch (this_attr->address) { + case ATTR_GYRO_SCALE: + { + const s16 gyro_scale[] = { 250, 500, 1000, 2000 }; + + return snprintf(buf, MAX_WR_SZ, "%d\n", + gyro_scale[st->chip_config.fsr]); + } + case ATTR_ACCEL_SCALE: + { + const s16 accel_scale[] = { 2, 4, 8, 16 }; + return snprintf(buf, MAX_WR_SZ, "%d\n", + accel_scale[st->chip_config.accel_fs]); + } + case ATTR_GYRO_ENABLE: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->chip_config.gyro_enable); + case ATTR_ACCEL_ENABLE: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->chip_config.accel_enable); + case ATTR_IN_POWER_ON: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->power_on_data); + case ATTR_DMP_BATCHMODE_TIMEOUT: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->batch.timeout); + case ATTR_DMP_PED_ON: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->ped.on); + case ATTR_DMP_TILT_ENABLE: + return snprintf(buf, MAX_WR_SZ, "%d\n", + st->chip_config.tilt_enable); + case ATTR_DMP_PICK_UP_ENABLE: + return snprintf(buf, MAX_WR_SZ, "%d\n", + st->chip_config.pick_up_enable); + case ATTR_DMP_EIS_ENABLE: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->chip_config.eis_enable); + case ATTR_DMP_LP_EN_OFF: + return snprintf(buf, MAX_WR_SZ, "%d\n", + st->chip_config.lp_en_mode_off); + case ATTR_DMP_STEP_COUNTER_ON: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->step_counter_l_on); + case ATTR_DMP_STEP_COUNTER_WAKE_ON: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->step_counter_wake_l_on); + case ATTR_DMP_STEP_DETECTOR_ON: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->step_detector_l_on); + case ATTR_DMP_STEP_DETECTOR_WAKE_ON: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->step_detector_wake_l_on); + case ATTR_GYRO_MATRIX: + m = st->plat_data.orientation; + return snprintf(buf, MAX_WR_SZ, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n", + m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], + m[8]); + case ATTR_ACCEL_MATRIX: + m = st->plat_data.orientation; + return snprintf(buf, MAX_WR_SZ, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n", + m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], + m[8]); + case ATTR_GYRO_SF: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->gyro_sf); + case ATTR_ANGLVEL_X_ST_CALIBBIAS: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->gyro_st_bias[0]); + case ATTR_ANGLVEL_Y_ST_CALIBBIAS: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->gyro_st_bias[1]); + case ATTR_ANGLVEL_Z_ST_CALIBBIAS: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->gyro_st_bias[2]); + case ATTR_ACCEL_X_ST_CALIBBIAS: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->accel_st_bias[0]); + case ATTR_ACCEL_Y_ST_CALIBBIAS: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->accel_st_bias[1]); + case ATTR_ACCEL_Z_ST_CALIBBIAS: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->accel_st_bias[2]); + case ATTR_GYRO_X_OFFSET: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->input_gyro_bias[0]); + case ATTR_GYRO_Y_OFFSET: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->input_gyro_bias[1]); + case ATTR_GYRO_Z_OFFSET: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->input_gyro_bias[2]); + case ATTR_ACCEL_X_OFFSET: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->input_accel_bias[0]); + case ATTR_ACCEL_Y_OFFSET: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->input_accel_bias[1]); + case ATTR_ACCEL_Z_OFFSET: + return snprintf(buf, MAX_WR_SZ, "%d\n", st->input_accel_bias[2]); + default: + return -EPERM; + } +} + +static ssize_t inv_self_test(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + int res; + + mutex_lock(&indio_dev->mlock); + res = inv_hw_self_test(st); + set_inv_enable(indio_dev); + mutex_unlock(&indio_dev->mlock); + + return snprintf(buf, MAX_WR_SZ, "%d\n", res); +} + + +/* + * inv_temperature_show() - Read temperature data directly from registers. + */ +static ssize_t inv_temperature_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + + u8 data[2]; + s32 temp; + int res; + + mutex_lock(&indio_dev->mlock); + res = inv_plat_read(st, REG_RAW_TEMP, 2, data); + if (res) + return res; + mutex_unlock(&indio_dev->mlock); + + temp = (s32)be16_to_cpup((__be16 *)(data)) * 10000; + temp = temp / TEMP_SENSITIVITY + TEMP_OFFSET; + + return snprintf(buf, MAX_WR_SZ, "%d %lld\n", temp, get_time_ns()); +} + +/* + * inv_reg_dump_show() - Register dump for testing. + */ +static ssize_t inv_reg_dump_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int ii; + char data; + int bytes_printed = 0; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + bytes_printed += snprintf(buf + bytes_printed, MAX_WR_SZ, "bank 0\n"); + + for (ii = 0; ii < 0x7F; ii++) { + /* don't read fifo r/w register */ + if ((ii == REG_MEM_R_W) || (ii == REG_FIFO_R_W)) + data = 0; + else + inv_plat_read(st, ii, 1, &data); + bytes_printed += snprintf(buf + bytes_printed, MAX_WR_SZ, + "%#2x: %#2x\n", ii, data); + } + set_inv_enable(indio_dev); + mutex_unlock(&indio_dev->mlock); + + return bytes_printed; +} + +static ssize_t inv_flush_batch_store(struct device *dev, + struct device_attribute *attr, const char *buf, + size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + int result, data; + + result = kstrtoint(buf, 10, &data); + if (result) + return result; + + mutex_lock(&indio_dev->mlock); + result = inv_flush_batch_data(indio_dev, data); + mutex_unlock(&indio_dev->mlock); + + return count; +} + +static const struct iio_chan_spec inv_mpu_channels[] = { + IIO_CHAN_SOFT_TIMESTAMP(INV_MPU_SCAN_TIMESTAMP), +}; + +/* special run time sysfs entry, read only */ +static DEVICE_ATTR(debug_reg_dump, S_IRUGO | S_IWUSR, inv_reg_dump_show, NULL); +static DEVICE_ATTR(out_temperature, S_IRUGO | S_IWUSR, + inv_temperature_show, NULL); +static DEVICE_ATTR(misc_self_test, S_IRUGO | S_IWUSR, inv_self_test, NULL); + +static IIO_DEVICE_ATTR(info_anglvel_matrix, S_IRUGO, inv_attr_show, NULL, + ATTR_GYRO_MATRIX); +static IIO_DEVICE_ATTR(info_accel_matrix, S_IRUGO, inv_attr_show, NULL, + ATTR_ACCEL_MATRIX); + +static IIO_DEVICE_ATTR(info_gyro_sf, S_IRUGO, inv_attr_show, NULL, + ATTR_GYRO_SF); +/* write only sysfs */ +static DEVICE_ATTR(misc_flush_batch, S_IWUSR, NULL, inv_flush_batch_store); + +/* sensor on/off sysfs control */ +static IIO_DEVICE_ATTR(in_accel_enable, S_IRUGO | S_IWUSR, + inv_sensor_on_show, inv_sensor_on_store, SENSOR_L_ACCEL); +static IIO_DEVICE_ATTR(in_anglvel_enable, S_IRUGO | S_IWUSR, + inv_sensor_on_show, inv_sensor_on_store, SENSOR_L_GYRO); +#ifndef SUPPORT_ONLY_BASIC_FEATURES +static IIO_DEVICE_ATTR(in_eis_enable, S_IRUGO | S_IWUSR, + inv_sensor_on_show, inv_sensor_on_store, + SENSOR_L_EIS_GYRO); +#endif +static IIO_DEVICE_ATTR(in_accel_wake_enable, S_IRUGO | S_IWUSR, + inv_sensor_on_show, inv_sensor_on_store, + SENSOR_L_ACCEL_WAKE); +static IIO_DEVICE_ATTR(in_anglvel_wake_enable, S_IRUGO | S_IWUSR, + inv_sensor_on_show, inv_sensor_on_store, + SENSOR_L_GYRO_WAKE); + +/* sensor rate sysfs control */ +static IIO_DEVICE_ATTR(in_accel_rate, S_IRUGO | S_IWUSR, + inv_sensor_rate_show, inv_sensor_rate_store, + SENSOR_L_ACCEL); +static IIO_DEVICE_ATTR(in_anglvel_rate, S_IRUGO | S_IWUSR, inv_sensor_rate_show, + inv_sensor_rate_store, SENSOR_L_GYRO); +#ifndef SUPPORT_ONLY_BASIC_FEATURES +static IIO_DEVICE_ATTR(in_eis_rate, S_IRUGO | S_IWUSR, + inv_sensor_rate_show, inv_sensor_rate_store, + SENSOR_L_EIS_GYRO); +#endif +static IIO_DEVICE_ATTR(in_accel_wake_rate, S_IRUGO | S_IWUSR, + inv_sensor_rate_show, inv_sensor_rate_store, + SENSOR_L_ACCEL_WAKE); +static IIO_DEVICE_ATTR(in_anglvel_wake_rate, S_IRUGO | S_IWUSR, + inv_sensor_rate_show, inv_sensor_rate_store, + SENSOR_L_GYRO_WAKE); + +static IIO_DEVICE_ATTR(misc_batchmode_timeout, S_IRUGO | S_IWUSR, + inv_attr_show, inv_basic_attr_store, + ATTR_DMP_BATCHMODE_TIMEOUT); + +/* engine scale */ +static IIO_DEVICE_ATTR(in_accel_scale, S_IRUGO | S_IWUSR, inv_attr_show, + inv_misc_attr_store, ATTR_ACCEL_SCALE); +static IIO_DEVICE_ATTR(in_anglvel_scale, S_IRUGO | S_IWUSR, inv_attr_show, + inv_misc_attr_store, ATTR_GYRO_SCALE); + + +#ifndef SUPPORT_ONLY_BASIC_FEATURES +static IIO_DEVICE_ATTR(debug_lp_en_off, S_IRUGO | S_IWUSR, inv_attr_show, + inv_debug_store, ATTR_DMP_LP_EN_OFF); +static IIO_DEVICE_ATTR(debug_clock_sel, S_IRUGO | S_IWUSR, inv_attr_show, + inv_debug_store, ATTR_DMP_CLK_SEL); +static IIO_DEVICE_ATTR(debug_reg_write, S_IRUGO | S_IWUSR, inv_attr_show, + inv_debug_store, ATTR_DEBUG_REG_WRITE); +static IIO_DEVICE_ATTR(debug_reg_write_addr, S_IRUGO | S_IWUSR, inv_attr_show, + inv_debug_store, ATTR_DEBUG_REG_ADDR); +#endif + +static IIO_DEVICE_ATTR(in_accel_x_st_calibbias, S_IRUGO | S_IWUSR, + inv_attr_show, NULL, ATTR_ACCEL_X_ST_CALIBBIAS); +static IIO_DEVICE_ATTR(in_accel_y_st_calibbias, S_IRUGO | S_IWUSR, + inv_attr_show, NULL, ATTR_ACCEL_Y_ST_CALIBBIAS); +static IIO_DEVICE_ATTR(in_accel_z_st_calibbias, S_IRUGO | S_IWUSR, + inv_attr_show, NULL, ATTR_ACCEL_Z_ST_CALIBBIAS); + +static IIO_DEVICE_ATTR(in_anglvel_x_st_calibbias, S_IRUGO | S_IWUSR, + inv_attr_show, NULL, ATTR_ANGLVEL_X_ST_CALIBBIAS); +static IIO_DEVICE_ATTR(in_anglvel_y_st_calibbias, S_IRUGO | S_IWUSR, + inv_attr_show, NULL, ATTR_ANGLVEL_Y_ST_CALIBBIAS); +static IIO_DEVICE_ATTR(in_anglvel_z_st_calibbias, S_IRUGO | S_IWUSR, + inv_attr_show, NULL, ATTR_ANGLVEL_Z_ST_CALIBBIAS); + +static IIO_DEVICE_ATTR(in_accel_x_offset, S_IRUGO | S_IWUSR, + inv_attr_show, inv_bias_store, ATTR_ACCEL_X_OFFSET); +static IIO_DEVICE_ATTR(in_accel_y_offset, S_IRUGO | S_IWUSR, + inv_attr_show, inv_bias_store, ATTR_ACCEL_Y_OFFSET); +static IIO_DEVICE_ATTR(in_accel_z_offset, S_IRUGO | S_IWUSR, + inv_attr_show, inv_bias_store, ATTR_ACCEL_Z_OFFSET); + +static IIO_DEVICE_ATTR(in_anglvel_x_offset, S_IRUGO | S_IWUSR, + inv_attr_show, inv_bias_store, ATTR_GYRO_X_OFFSET); +static IIO_DEVICE_ATTR(in_anglvel_y_offset, S_IRUGO | S_IWUSR, + inv_attr_show, inv_bias_store, ATTR_GYRO_Y_OFFSET); +static IIO_DEVICE_ATTR(in_anglvel_z_offset, S_IRUGO | S_IWUSR, + inv_attr_show, inv_bias_store, ATTR_GYRO_Z_OFFSET); + +#ifndef SUPPORT_ONLY_BASIC_FEATURES +static IIO_DEVICE_ATTR(in_step_detector_enable, S_IRUGO | S_IWUSR, + inv_attr_show, inv_basic_attr_store, + ATTR_DMP_STEP_DETECTOR_ON); +static IIO_DEVICE_ATTR(in_step_detector_wake_enable, S_IRUGO | S_IWUSR, + inv_attr_show, inv_basic_attr_store, + ATTR_DMP_STEP_DETECTOR_WAKE_ON); +static IIO_DEVICE_ATTR(in_step_counter_enable, S_IRUGO | S_IWUSR, inv_attr_show, + inv_basic_attr_store, ATTR_DMP_STEP_COUNTER_ON); +static IIO_DEVICE_ATTR(in_step_counter_wake_enable, S_IRUGO | S_IWUSR, + inv_attr_show, inv_basic_attr_store, + ATTR_DMP_STEP_COUNTER_WAKE_ON); + +static IIO_DEVICE_ATTR(event_tilt_enable, S_IRUGO | S_IWUSR, + inv_attr_show, inv_basic_attr_store, + ATTR_DMP_TILT_ENABLE); + +static IIO_DEVICE_ATTR(event_eis_enable, S_IRUGO | S_IWUSR, + inv_attr_show, inv_basic_attr_store, + ATTR_DMP_EIS_ENABLE); + +static IIO_DEVICE_ATTR(event_pick_up_enable, S_IRUGO | S_IWUSR, + inv_attr_show, inv_basic_attr_store, + ATTR_DMP_PICK_UP_ENABLE); + +static IIO_DEVICE_ATTR(in_power_on, S_IRUGO | S_IWUSR, + inv_attr_show, inv_basic_attr_store, + ATTR_IN_POWER_ON); +#endif + +static const struct attribute *inv_raw_attributes[] = { + &dev_attr_debug_reg_dump.attr, + &dev_attr_out_temperature.attr, + &dev_attr_misc_flush_batch.attr, + &dev_attr_misc_self_test.attr, +#ifndef SUPPORT_ONLY_BASIC_FEATURES + &iio_dev_attr_in_power_on.dev_attr.attr, +#endif + &iio_dev_attr_in_accel_enable.dev_attr.attr, + &iio_dev_attr_in_accel_wake_enable.dev_attr.attr, + &iio_dev_attr_info_accel_matrix.dev_attr.attr, + &iio_dev_attr_in_accel_scale.dev_attr.attr, + &iio_dev_attr_misc_batchmode_timeout.dev_attr.attr, + &iio_dev_attr_in_accel_rate.dev_attr.attr, + &iio_dev_attr_in_accel_wake_rate.dev_attr.attr, +}; + +#ifndef SUPPORT_ONLY_BASIC_FEATURES +static const struct attribute *inv_debug_attributes[] = { + &iio_dev_attr_debug_lp_en_off.dev_attr.attr, + &iio_dev_attr_debug_clock_sel.dev_attr.attr, + &iio_dev_attr_debug_reg_write.dev_attr.attr, + &iio_dev_attr_debug_reg_write_addr.dev_attr.attr, +}; +#endif + +static const struct attribute *inv_gyro_attributes[] = { + &iio_dev_attr_info_anglvel_matrix.dev_attr.attr, + &iio_dev_attr_in_anglvel_enable.dev_attr.attr, + &iio_dev_attr_in_anglvel_rate.dev_attr.attr, +#ifndef SUPPORT_ONLY_BASIC_FEATURES + &iio_dev_attr_in_eis_enable.dev_attr.attr, +#endif + &iio_dev_attr_in_anglvel_wake_enable.dev_attr.attr, + &iio_dev_attr_in_anglvel_scale.dev_attr.attr, +#ifndef SUPPORT_ONLY_BASIC_FEATURES + &iio_dev_attr_in_eis_rate.dev_attr.attr, +#endif + &iio_dev_attr_in_anglvel_wake_rate.dev_attr.attr, + &iio_dev_attr_info_gyro_sf.dev_attr.attr, +}; + +static const struct attribute *inv_bias_attributes[] = { + &iio_dev_attr_in_accel_x_st_calibbias.dev_attr.attr, + &iio_dev_attr_in_accel_y_st_calibbias.dev_attr.attr, + &iio_dev_attr_in_accel_z_st_calibbias.dev_attr.attr, + &iio_dev_attr_in_accel_x_offset.dev_attr.attr, + &iio_dev_attr_in_accel_y_offset.dev_attr.attr, + &iio_dev_attr_in_accel_z_offset.dev_attr.attr, + &iio_dev_attr_in_anglvel_x_st_calibbias.dev_attr.attr, + &iio_dev_attr_in_anglvel_y_st_calibbias.dev_attr.attr, + &iio_dev_attr_in_anglvel_z_st_calibbias.dev_attr.attr, + &iio_dev_attr_in_anglvel_x_offset.dev_attr.attr, + &iio_dev_attr_in_anglvel_y_offset.dev_attr.attr, + &iio_dev_attr_in_anglvel_z_offset.dev_attr.attr, +}; + +#ifndef SUPPORT_ONLY_BASIC_FEATURES +static const struct attribute *inv_pedometer_attributes[] = { + &iio_dev_attr_event_tilt_enable.dev_attr.attr, + &iio_dev_attr_event_eis_enable.dev_attr.attr, + &iio_dev_attr_event_pick_up_enable.dev_attr.attr, + &iio_dev_attr_in_step_counter_enable.dev_attr.attr, + &iio_dev_attr_in_step_counter_wake_enable.dev_attr.attr, + &iio_dev_attr_in_step_detector_enable.dev_attr.attr, + &iio_dev_attr_in_step_detector_wake_enable.dev_attr.attr, +}; +#endif + +static struct attribute *inv_attributes[ARRAY_SIZE(inv_raw_attributes) + +#ifndef SUPPORT_ONLY_BASIC_FEATURES + ARRAY_SIZE(inv_debug_attributes) + +#endif + ARRAY_SIZE(inv_gyro_attributes) + + ARRAY_SIZE(inv_bias_attributes) + +#ifndef SUPPORT_ONLY_BASIC_FEATURES + ARRAY_SIZE(inv_pedometer_attributes) + +#endif + + 1]; + +static const struct attribute_group inv_attribute_group = { + .name = "mpu", + .attrs = inv_attributes +}; + +static const struct iio_info mpu_info = { + .driver_module = THIS_MODULE, + .attrs = &inv_attribute_group, +}; + +/* + * inv_check_chip_type() - check and setup chip type. + */ +int inv_check_chip_type(struct iio_dev *indio_dev, const char *name) +{ + int result; + int t_ind; + struct inv_chip_config_s *conf; + struct mpu_platform_data *plat; + struct inv_mpu_state *st; + + st = iio_priv(indio_dev); + conf = &st->chip_config; + plat = &st->plat_data; + + if (!strcmp(name, "iam20680")) + st->chip_type = IAM20680; + else + return -EPERM; + st->chip_config.has_gyro = 1; + + st->hw = &hw_info[st->chip_type]; + result = inv_mpu_initialize(st); + if (result) + return result; + + t_ind = 0; + memcpy(&inv_attributes[t_ind], inv_raw_attributes, + sizeof(inv_raw_attributes)); + t_ind += ARRAY_SIZE(inv_raw_attributes); + +#ifndef SUPPORT_ONLY_BASIC_FEATURES + memcpy(&inv_attributes[t_ind], inv_pedometer_attributes, + sizeof(inv_pedometer_attributes)); + t_ind += ARRAY_SIZE(inv_pedometer_attributes); +#endif + + memcpy(&inv_attributes[t_ind], inv_gyro_attributes, + sizeof(inv_gyro_attributes)); + t_ind += ARRAY_SIZE(inv_gyro_attributes); + + memcpy(&inv_attributes[t_ind], inv_bias_attributes, + sizeof(inv_bias_attributes)); + t_ind += ARRAY_SIZE(inv_bias_attributes); + +#ifndef SUPPORT_ONLY_BASIC_FEATURES + memcpy(&inv_attributes[t_ind], inv_debug_attributes, + sizeof(inv_debug_attributes)); + t_ind += ARRAY_SIZE(inv_debug_attributes); +#endif + + inv_attributes[t_ind] = NULL; + + indio_dev->channels = inv_mpu_channels; + indio_dev->num_channels = ARRAY_SIZE(inv_mpu_channels); + + indio_dev->info = &mpu_info; + indio_dev->modes = INDIO_DIRECT_MODE; + indio_dev->currentmode = INDIO_DIRECT_MODE; + + return result; +} +EXPORT_SYMBOL_GPL(inv_check_chip_type); + +int inv_create_dmp_sysfs(struct iio_dev *ind) +{ + // dummy + return 0; +} +EXPORT_SYMBOL_GPL(inv_create_dmp_sysfs); + +MODULE_AUTHOR("Invensense Corporation"); +MODULE_DESCRIPTION("Invensense device ICM20xxx driver"); +MODULE_LICENSE("GPL"); diff --git a/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_iio_reg_20680.h b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_iio_reg_20680.h new file mode 100644 index 000000000000..3f8ce71be024 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_iio_reg_20680.h @@ -0,0 +1,236 @@ +/* + * 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_20680_H_ +#define _INV_MPU_IIO_REG_20680_H_ + +/* Uncomment when HAL does not support the algorithm library + * for calibration and sensor fusion not to expose unused + * sysfs entries */ +#define SUPPORT_ONLY_BASIC_FEATURES + +/* Uncomment to read data registers for sensor data instead of FIFO */ +//#define SENSOR_DATA_FROM_REGISTERS + +/* Uncomment to enable timer based batching */ +#define TIMER_BASED_BATCHING + +/* Polling (batch mode) can be enabled only when FIFO read */ +#if defined(SENSOR_DATA_FROM_REGISTERS) +#undef TIMER_BASED_BATCHING +#endif + +/*register and associated bit definition*/ +#define REG_XA_OFFS_H 0x77 +#define REG_YA_OFFS_H 0x7A +#define REG_ZA_OFFS_H 0x7D +#define REG_XG_OFFS_USR_H 0x13 +#define REG_YG_OFFS_USR_H 0x15 +#define REG_ZG_OFFS_USR_H 0x17 +#define REG_SAMPLE_RATE_DIV 0x19 + +#define REG_CONFIG 0x1A +#define EXT_SYNC_SET 8 + +#define REG_GYRO_CONFIG 0x1B +#define BITS_SELF_TEST_EN 0xE0 +#define SHIFT_GYRO_FS_SEL 0x03 + +#define REG_ACCEL_CONFIG 0x1C +#define SHIFT_ACCEL_FS 0x03 + +#define REG_LP_MODE_CTRL 0x1E +#define BIT_GYRO_CYCLE_EN 0x80 + +#define REG_ACCEL_WOM_THR 0x1F +#define REG_ACCEL_WOM_X_THR 0x20 +#define REG_ACCEL_WOM_Y_THR 0x21 +#define REG_ACCEL_WOM_Z_THR 0x22 + +#define REG_ACCEL_MOT_THR 0x1F +#define REG_ACCEL_MOT_DUR 0x20 + +#define REG_ACCEL_CONFIG_2 0x1D +#define BIT_ACCEL_FCHOCIE_B 0x08 + +#define REG_FIFO_EN 0x23 +#define BITS_GYRO_FIFO_EN 0x70 +#define BIT_ACCEL_FIFO_EN 0x08 + +#define REG_FSYNC_INT 0x36 +#define BIT_FSYNC_INT 0x80 + +#define REG_INT_PIN_CFG 0x37 + +#define REG_INT_ENABLE 0x38 +#define BIT_WOM_X_INT_EN 0x80 +#define BIT_WOM_Y_INT_EN 0x40 +#define BIT_WOM_Z_INT_EN 0x20 +#define BIT_WOM_ALL_INT_EN 0xE0 +#define BIT_FSYNC_INT_EN 0x8 +#define BIT_DATA_RDY_EN 0x1 + +#define REG_INT_STATUS 0x3A +#define BIT_WOM_X_INT 0x80 +#define BIT_WOM_Y_INT 0x40 +#define BIT_WOM_Z_INT 0x20 + +#define REG_RAW_ACCEL 0x3B +#define REG_RAW_TEMP 0x41 +#define REG_RAW_GYRO 0x43 +#define REG_EXT_SENS_DATA_00 0x49 +#define REG_EXT_SENS_DATA_08 0x51 +#define REG_EXT_SENS_DATA_09 0x52 + +#define REG_ACCEL_INTEL_CTRL 0x69 +#define BIT_ACCEL_INTEL_EN 0x80 +#define BIT_ACCEL_INTEL_MODE 0x40 + +#define REG_USER_CTRL 0x6A +#define BIT_COND_RST 0x01 +#define BIT_FIFO_RST 0x04 +#define BIT_FIFO_EN 0x40 + +#define REG_PWR_MGMT_1 0x6B +#define BIT_H_RESET 0x80 +#define BIT_SLEEP 0x40 +#define BIT_LP_EN 0x20 +#define BIT_CLK_PLL 0x01 +#define BIT_CLK_MASK 0x07 + +#define REG_PWR_MGMT_2 0x6C +#define BIT_PWR_ACCEL_STBY 0x38 +#define BIT_PWR_GYRO_STBY 0x07 +#define BIT_PWR_ALL_OFF 0x3F +#define BIT_FIFO_LP_EN 0x80 + +#define REG_MEM_BANK_SEL 0x6D +#define REG_MEM_START_ADDR 0x6E +#define REG_MEM_R_W 0x6F + +#define REG_FIFO_COUNT_H 0x72 +#define REG_FIFO_R_W 0x74 +#define REG_WHO_AM_I 0x75 + +#define REG_6500_XG_ST_DATA 0x50 +#define REG_6500_XA_ST_DATA 0xD +#define REG_6500_XA_OFFS_H 0x77 +#define REG_6500_YA_OFFS_H 0x7A +#define REG_6500_ZA_OFFS_H 0x7D +#define REG_6500_ACCEL_CONFIG2 0x1D +#define BIT_ACCEL_FCHOCIE_B 0x08 +#define BIT_FIFO_SIZE_1K 0x40 + +#define REG_LP_MODE_CFG 0x1E + +#define REG_6500_LP_ACCEL_ODR 0x1E +#define REG_6500_ACCEL_WOM_THR 0x1F + +/* data output control reg 2 */ +#define ACCEL_ACCURACY_SET 0x4000 +#define GYRO_ACCURACY_SET 0x2000 +#define CPASS_ACCURACY_SET 0x1000 + +/* data definitions */ +#define ACCEL_COVARIANCE 0 +#define BYTES_PER_SENSOR 6 +#define BYTES_FOR_TEMP 2 +#define FIFO_COUNT_BYTE 2 +#define HARDWARE_FIFO_SIZE 512 +#define FIFO_SIZE (HARDWARE_FIFO_SIZE * 7 / 10) +#define POWER_UP_TIME 100 +#define REG_UP_TIME_USEC 100 +#define LEFT_OVER_BYTES 128 +#define IIO_BUFFER_BYTES 8 +#define BASE_SAMPLE_RATE 1000 +#define DRY_RUN_TIME 50 +#define INV_IAM20680_GYRO_START_TIME 35 +#define INV_IAM20680_ACCEL_START_TIME 30 +#define MODE_1K_INIT_SAMPLE 5 +#define FIRST_SAMPLE_BUF_MS 30 + +#ifdef BIAS_CONFIDENCE_HIGH +#define DEFAULT_ACCURACY 3 +#else +#define DEFAULT_ACCURACY 1 +#endif + +/* temperature */ +#define TEMP_SENSITIVITY 32680 // 326.8 LSB/degC * 100 +#define TEMP_OFFSET 2500 // 25 degC * 100 + +/* enum for sensor */ +enum INV_SENSORS { + SENSOR_ACCEL = 0, + SENSOR_TEMP, + SENSOR_GYRO, + SENSOR_COMPASS, + SENSOR_NUM_MAX, + SENSOR_INVALID, +}; + +enum inv_filter_e { + INV_FILTER_256HZ_NOLPF2 = 0, + INV_FILTER_188HZ, + INV_FILTER_98HZ, + INV_FILTER_42HZ, + INV_FILTER_20HZ, + INV_FILTER_10HZ, + INV_FILTER_5HZ, + INV_FILTER_2100HZ_NOLPF, + NUM_FILTER +}; + +#define MPU_DEFAULT_DMP_FREQ 200 +#define PEDOMETER_FREQ (MPU_DEFAULT_DMP_FREQ >> 2) +#define SENSOR_FUSION_MIN_RATE 100 +#define GESTURE_ACCEL_RATE 50 +#define ESI_GYRO_RATE 1000 +#define MAX_FIFO_PACKET_READ 6 +#define MAX_BATCH_FIFO_SIZE FIFO_SIZE + +#define MIN_MST_ODR_CONFIG 4 +#define MAX_MST_ODR_CONFIG 5 +/* initial rate is important. For non-DMP mode, it is set as 4 1000/256*/ +#define MPU_INIT_SENSOR_RATE 4 +#define MAX_MST_NON_COMPASS_ODR_CONFIG 7 +#define THREE_AXES 3 +#define NINE_ELEM (THREE_AXES * THREE_AXES) +#define MPU_TEMP_SHIFT 16 + +#define DMP_DIVIDER (BASE_SAMPLE_RATE / MPU_DEFAULT_DMP_FREQ) +#define DEFAULT_BATCH_RATE 400 +#define DEFAULT_BATCH_TIME (MSEC_PER_SEC / DEFAULT_BATCH_RATE) + +#define TEMPERATURE_SCALE 3340827L +#define TEMPERATURE_OFFSET 1376256L +#define SECONDARY_INIT_WAIT 100 +#define MPU_SOFT_REV_ADDR 0x86 +#define MPU_SOFT_REV_MASK 0xf +#define SW_REV_LP_EN_MODE 4 + +/* data limit definitions */ +#define MIN_FIFO_RATE 4 +#define MAX_FIFO_RATE MPU_DEFAULT_DMP_FREQ + +#define MAX_MPU_MEM 8192 +#define MAX_PRS_RATE 281 + +enum inv_devices { + ICM20608D, + ICM20690, + ICM20602, + IAM20680, + INV_NUM_PARTS, +}; +#endif diff --git a/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_init_20680.c b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_init_20680.c new file mode 100644 index 000000000000..58bd8d073890 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_init_20680.c @@ -0,0 +1,258 @@ +/* + * 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_calc_gyro_sf(s8 pll) +{ + int a, r; + int value, t; + + t = 102870L + 81L * pll; + a = (1L << 30) / t; + r = (1L << 30) - a * t; + value = a * 797 * DMP_DIVIDER; + value += (s64) ((a * 1011387LL * DMP_DIVIDER) >> 20); + value += r * 797L * DMP_DIVIDER / t; + value += (s32) ((s64) ((r * 1011387LL * DMP_DIVIDER) >> 20)) / t; + value <<= 1; + + return value; +} + +static int inv_read_timebase(struct inv_mpu_state *st) +{ + + inv_plat_single_write(st, REG_CONFIG, 3); + + st->eng_info[ENGINE_ACCEL].base_time = NSEC_PER_SEC; + st->eng_info[ENGINE_ACCEL].base_time_1k = 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_I2C].base_time = NSEC_PER_SEC; + st->eng_info[ENGINE_I2C].base_time_1k = 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; + + st->gyro_sf = inv_calc_gyro_sf(0); + + return 0; +} + +int inv_set_gyro_sf(struct inv_mpu_state *st) +{ + int result; + + result = inv_plat_single_write(st, REG_GYRO_CONFIG, + st->chip_config.fsr << SHIFT_GYRO_FS_SEL); + + return result; +} + +int inv_set_accel_sf(struct inv_mpu_state *st) +{ + int result; + + result = inv_plat_single_write(st, REG_ACCEL_CONFIG, + st->chip_config.accel_fs << SHIFT_ACCEL_FS); + return result; +} + +// dummy for 20602 +int inv_set_accel_intel(struct inv_mpu_state *st) +{ + return 0; +} + +static void inv_init_sensor_struct(struct inv_mpu_state *st) +{ + int i; + + for (i = 0; i < SENSOR_NUM_MAX; i++) + st->sensor[i].rate = MPU_INIT_SENSOR_RATE; + + 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; +#ifdef TIMER_BASED_BATCHING + st->batch_timeout = 0; + st->is_batch_timer_running = false; +#endif + + 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; + + 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; + struct inv_chip_config_s *conf; + struct mpu_platform_data *plat; + + conf = &st->chip_config; + plat = &st->plat_data; + + /* 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_single_write(st, REG_PWR_MGMT_1, BIT_H_RESET); + if (result) + return result; + usleep_range(REG_UP_TIME_USEC, REG_UP_TIME_USEC); + msleep(100); + /* toggle power state */ + result = inv_set_power(st, false); + if (result) + return result; + result = inv_set_power(st, true); + if (result) + return result; + + result = inv_plat_single_write(st, REG_USER_CTRL, st->i2c_dis); + if (result) + return result; + result = inv_init_config(st); + if (result) + return result; + + result = mem_r(MPU_SOFT_REV_ADDR, 1, &v); + pr_info("sw_rev=%x, res=%d\n", v, result); + if (result) + return result; + st->chip_config.lp_en_mode_off = 0; + + pr_info("%s: Mask %X, v = %X, lp mode = %d\n", __func__, + MPU_SOFT_REV_MASK, v, st->chip_config.lp_en_mode_off); + result = inv_set_power(st, false); + + pr_info("%s: initialize result is %d....\n", __func__, result); + return 0; +} diff --git a/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_parsing_20680.c b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_parsing_20680.c new file mode 100644 index 000000000000..0f17b6dd2c52 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_parsing_20680.c @@ -0,0 +1,421 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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_FSYNC_INT, 1, data); + if (data[0] & BIT_FSYNC_INT) { + 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 int inv_push_20680_data(struct inv_mpu_state *st, u8 *d) +{ + u8 *dptr; + int i; + + dptr = d; + + for (i = 0; i < SENSOR_NUM_MAX; i++) { + if (st->sensor[i].on) { + inv_get_dmp_ts(st, i); + if (st->sensor[i].send && (!st->ts_algo.first_sample)) { + st->sensor[i].sample_calib++; + inv_push_sensor(st, i, st->sensor[i].ts, dptr); + } + dptr += st->sensor[i].sample_size; + } + } + if (st->ts_algo.first_sample) + st->ts_algo.first_sample--; + st->header_count--; + + return 0; +} + +static int inv_process_20680_data(struct inv_mpu_state *st) +{ + int total_bytes, tmp, res, fifo_count, pk_size, i; + u8 *dptr, *d; + u8 data[14]; + bool done_flag; + u8 v; +#ifdef SENSOR_DATA_FROM_REGISTERS + u8 reg; + int len; +#endif + + if(st->gesture_only_on && (!st->batch.timeout)) { + res = inv_plat_read(st, REG_INT_STATUS, 1, data); + if (res) + return res; + pr_debug("ges cnt=%d, statu=%x\n", + st->gesture_int_count, data[0]); + if (data[0] & (BIT_WOM_ALL_INT_EN)) { + if (!st->gesture_int_count) { + inv_switch_power_in_lp(st, true); + res = inv_plat_single_write(st, REG_INT_ENABLE, + BIT_WOM_ALL_INT_EN | BIT_DATA_RDY_EN); + if (res) + return res; + v = 0; + if (st->chip_config.gyro_enable) + v |= BITS_GYRO_FIFO_EN; + + if (st->chip_config.accel_enable) + v |= BIT_ACCEL_FIFO_EN; + res = inv_plat_single_write(st, REG_FIFO_EN, v); + 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; + res = inv_switch_power_in_lp(st, false); + st->gesture_int_count = WOM_DELAY_THRESHOLD; + + return res; + } + st->gesture_int_count = WOM_DELAY_THRESHOLD; + } else { + if (!st->gesture_int_count) { + inv_switch_power_in_lp(st, true); + res = inv_plat_single_write(st, REG_FIFO_EN, 0); + res = inv_plat_single_write(st, REG_INT_ENABLE, + BIT_WOM_ALL_INT_EN); + inv_switch_power_in_lp(st, false); + + 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; + +#ifdef SENSOR_DATA_FROM_REGISTERS + len = 0; + if (st->sensor[SENSOR_GYRO].on) { + reg = REG_RAW_GYRO; + len += BYTES_PER_SENSOR; + if (st->sensor[SENSOR_ACCEL].on && !st->sensor[SENSOR_TEMP].on) + len += BYTES_FOR_TEMP; + } + if (st->sensor[SENSOR_TEMP].on) { + reg = REG_RAW_TEMP; + len += BYTES_FOR_TEMP; + } + if (st->sensor[SENSOR_ACCEL].on) { + reg = REG_RAW_ACCEL; + len += BYTES_PER_SENSOR; + } + + if (len == 0) { + pr_debug("No sensor is enabled\n"); + return 0; + } + + /* read data registers */ + res = inv_plat_read(st, reg, len, data); + if (res < 0) { + pr_err("read data registers is failed\n"); + return res; + } + + /* copy sensor data to buffer as FIFO data format */ + tmp = 0; + if (st->sensor[SENSOR_ACCEL].on) { + for (i = 0; i < BYTES_PER_SENSOR; i++) + dptr[i] = data[tmp + i]; + dptr += BYTES_PER_SENSOR; + tmp += BYTES_PER_SENSOR; + } + + if (st->sensor[SENSOR_TEMP].on) { + for (i = 0; i < BYTES_FOR_TEMP; i++) + dptr[i] = data[tmp + i]; + dptr += BYTES_FOR_TEMP; + tmp += BYTES_FOR_TEMP; + } + + if (st->sensor[SENSOR_GYRO].on) { + if (st->sensor[SENSOR_ACCEL].on && !st->sensor[SENSOR_TEMP].on) + tmp += BYTES_FOR_TEMP; + for (i = 0; i < BYTES_PER_SENSOR; i++) + dptr[i] = data[tmp + i]; + } +#else + 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_R_W, 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; + } +#endif /* SENSOR_DATA_FROM_REGISTERS */ + 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; + + for (i = 0; i < SENSOR_NUM_MAX; i++) { + if (st->sensor[i].on) { + st->sensor[i].count = total_bytes / pk_size; + } + } + 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_20680_data(st, dptr); + if (res) + return res; + total_bytes -= pk_size; + dptr += pk_size; + } else { + done_flag = true; + } + } + + return 0; +} + +/* + * _inv_read_fifo() - Transfer data from FIFO to ring buffer. + */ +static void _inv_read_fifo(struct inv_mpu_state *st) +{ + struct iio_dev *indio_dev = iio_priv_to_dev(st); + int result; + + result = wait_event_interruptible_timeout(st->wait_queue, + st->resume_state, msecs_to_jiffies(300)); + if (result <= 0) + return; + mutex_lock(&indio_dev->mlock); +#ifdef TIMER_BASED_BATCHING + if (st->batch_timeout) { + if (inv_plat_single_write(st, REG_INT_ENABLE, st->int_en)) + pr_err("REG_INT_ENABLE write error\n"); + } +#endif + st->wake_sensor_received = false; + result = inv_process_20680_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 + return; + +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_switch_power_in_lp(st, false); + mutex_unlock(&indio_dev->mlock); + + return; + } + + pr_err("error to reset fifo\n"); + inv_switch_power_in_lp(st, true); + inv_reset_fifo(st, true); + inv_switch_power_in_lp(st, false); + mutex_unlock(&indio_dev->mlock); + + return; +} + +irqreturn_t inv_read_fifo(int irq, void *dev_id) +{ + struct inv_mpu_state *st = (struct inv_mpu_state *)dev_id; + + _inv_read_fifo(st); + + return IRQ_HANDLED; +} + +#ifdef TIMER_BASED_BATCHING +void inv_batch_work(struct work_struct *work) +{ + struct inv_mpu_state *st = + container_of(work, struct inv_mpu_state, batch_work); + struct iio_dev *indio_dev = iio_priv_to_dev(st); + + mutex_lock(&indio_dev->mlock); + if (inv_plat_single_write(st, REG_INT_ENABLE, st->int_en | BIT_DATA_RDY_EN)) + pr_err("REG_INT_ENABLE write error\n"); + mutex_unlock(&indio_dev->mlock); + + return; +} +#endif + +int inv_flush_batch_data(struct iio_dev *indio_dev, int data) +{ + struct inv_mpu_state *st = iio_priv(indio_dev); + +#ifndef SENSOR_DATA_FROM_REGISTERS + if (st->chip_config.gyro_enable || + st->chip_config.accel_enable || + st->chip_config.slave_enable || + st->chip_config.pressure_enable) { + st->wake_sensor_received = 0; + inv_process_20680_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_switch_power_in_lp(st, false); + } +#endif /* SENSOR_DATA_FROM_REGISTERS */ + inv_push_marker_to_buffer(st, END_MARKER, data); + + return 0; +} + diff --git a/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_selftest_20680.c b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_selftest_20680.c new file mode 100644 index 000000000000..7a90b4d8b882 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_selftest_20680.c @@ -0,0 +1,752 @@ +/* +* 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" + +/* register settings */ +#define DEF_SELFTEST_GYRO_SENS (32768 / 250) +/* wait time before collecting data */ +#define MAX_PACKETS 20 +#define SELFTEST_WAIT_TIME (MAX_PACKETS * 10) +#define DEF_ST_STABLE_TIME 20 +#define DEF_GYRO_SCALE 131 +#define DEF_ST_PRECISION 1000 +#define DEF_ST_ACCEL_FS_MG 2000UL +#define DEF_ST_SCALE 32768 +#define DEF_ST_TRY_TIMES 2 +#define DEF_ST_ACCEL_RESULT_SHIFT 1 +#define DEF_ST_SAMPLES 200 + +#define DEF_ACCEL_ST_SHIFT_DELTA_MIN 500 +#define DEF_ACCEL_ST_SHIFT_DELTA_MAX 1500 +#define DEF_GYRO_CT_SHIFT_DELTA 500 + +#define SENSOR_UP_TIME 30 +#define REG_UP_TIME 2 + +#define DEF_ST_ACCEL_FS_MG 2000UL +#define DEF_ACCEL_ST_SHIFT_DELTA 500 +#define ACCEL_ST_AL_MIN ((DEF_ACCEL_ST_AL_MIN * DEF_ST_SCALE \ + / DEF_ST_ACCEL_FS_MG) * DEF_ST_PRECISION) +#define ACCEL_ST_AL_MAX ((DEF_ACCEL_ST_AL_MAX * DEF_ST_SCALE \ + / DEF_ST_ACCEL_FS_MG) * DEF_ST_PRECISION) + +#define THREE_AXIS 3 +#define DEF_ST_MPU6500_ACCEL_LPF 2 +#define DEF_SELFTEST_SAMPLE_RATE 0 /* 1000Hz */ +#define DEF_SELFTEST_SAMPLE_RATE_LP 3 /* 250Hz */ +#define DEF_SELFTEST_SAMPLE_RATE_ACC_LP 10 /* 250Hz LPOSC_CLKSEL */ +#define INV_MPU_SAMPLE_RATE_CHANGE_STABLE 50 +#define DEF_SELFTEST_6500_ACCEL_FS (0 << 3) +#define DEF_SELFTEST_GYRO_FS (0 << 3) +#define DEF_ST_6500_STABLE_TIME 20 +#define BIT_ACCEL_OUT 0x08 +#define BITS_GYRO_OUT 0x70 +#define THREE_AXIS 3 +#define DEF_GYRO_WAIT_TIME 10 +#define DEF_GYRO_WAIT_TIME_LP 50 + +/* Gyro Offset Max Value (dps) */ +#define DEF_GYRO_OFFSET_MAX 20 +/* Gyro Self Test Absolute Limits ST_AL (dps) */ +#define DEF_GYRO_ST_AL 60 +/* Accel Self Test Absolute Limits ST_AL (mg) */ +#define DEF_ACCEL_ST_AL_MIN 225 +#define DEF_ACCEL_ST_AL_MAX 675 + +struct recover_regs { + u8 int_enable; /* REG_INT_ENABLE */ + u8 fifo_en; /* REG_FIFO_EN */ + u8 user_ctrl; /* REG_USER_CTRL */ + u8 config; /* REG_CONFIG */ + u8 gyro_config; /* REG_GYRO_CONFIG */ + u8 accel_config; /* REG_ACCEL_CONFIG */ + u8 accel_config_2; /* REG_ACCEL_CONFIG_2 */ + u8 smplrt_div; /* REG_SAMPLE_RATE_DIV */ + u8 lp_mode; /* REG_LP_MODE_CTRL */ + u8 pwr_mgmt_1; /* REG_PWR_MGMT_1 */ + u8 pwr_mgmt_2; /* REG_PWR_MGMT_2 */ +}; + +static struct recover_regs saved_regs; + +static const u16 mpu_st_tb[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 void inv_show_saved_setting(struct inv_mpu_state *st) +{ + pr_debug(" REG_INT_ENABLE : 0x%02X\n", saved_regs.int_enable); + pr_debug(" REG_FIFO_EN : 0x%02X\n", saved_regs.fifo_en); + pr_debug(" REG_USER_CTRL : 0x%02X\n", saved_regs.user_ctrl); + pr_debug(" REG_CONFIG : 0x%02X\n", saved_regs.config); + pr_debug(" REG_GYRO_CONFIG : 0x%02X\n", saved_regs.gyro_config); + pr_debug(" REG_ACCEL_CONFIG : 0x%02X\n", saved_regs.accel_config); + pr_debug(" REG_ACCEL_CONFIG_2 : 0x%02X\n", saved_regs.accel_config_2); + pr_debug(" REG_SAMPLE_RATE_DIV : 0x%02X\n", saved_regs.smplrt_div); + pr_debug(" REG_LP_MODE_CTRL : 0x%02X\n", saved_regs.lp_mode); + pr_debug(" REG_PWR_MGMT_1 : 0x%02X\n", saved_regs.pwr_mgmt_1); + pr_debug(" REG_PWR_MGMT_2 : 0x%02X\n", saved_regs.pwr_mgmt_2); +} + +static int inv_save_setting(struct inv_mpu_state *st) +{ + int result; + + result = inv_plat_read(st, REG_PWR_MGMT_1, 1, + &saved_regs.pwr_mgmt_1); + if (result) + return result; + + /* wake up */ + result = inv_plat_single_write(st, REG_PWR_MGMT_1, + (saved_regs.pwr_mgmt_1 & ~BIT_SLEEP)); + if (result) + return result; + + result = inv_plat_read(st, REG_INT_ENABLE, 1, + &saved_regs.int_enable); + if (result) + return result; + result = inv_plat_read(st, REG_FIFO_EN, 1, + &saved_regs.fifo_en); + if (result) + return result; + result = inv_plat_read(st, REG_USER_CTRL, 1, + &saved_regs.user_ctrl); + if (result) + return result; + result = inv_plat_read(st, REG_CONFIG, 1, + &saved_regs.config); + if (result) + return result; + result = inv_plat_read(st, REG_GYRO_CONFIG, 1, + &saved_regs.gyro_config); + if (result) + return result; + result = inv_plat_read(st, REG_ACCEL_CONFIG, 1, + &saved_regs.accel_config); + if (result) + return result; + result = inv_plat_read(st, REG_ACCEL_CONFIG_2, 1, + &saved_regs.accel_config_2); + if (result) + return result; + result = inv_plat_read(st, REG_SAMPLE_RATE_DIV, 1, + &saved_regs.smplrt_div); + if (result) + return result; + result = inv_plat_read(st, REG_LP_MODE_CTRL, 1, + &saved_regs.lp_mode); + if (result) + return result; + result = inv_plat_read(st, REG_PWR_MGMT_2, 1, + &saved_regs.pwr_mgmt_2); + if (result) + return result; + + inv_show_saved_setting(st); + + return result; +} + +static int inv_recover_setting(struct inv_mpu_state *st) +{ + int result; + /* Stop sensors */ + result = inv_plat_single_write(st, REG_PWR_MGMT_2, + BIT_PWR_ACCEL_STBY | BIT_PWR_GYRO_STBY); + if (result) + return result; + + /* Restore sensor configurations */ + result = inv_plat_single_write(st, REG_INT_ENABLE, + saved_regs.int_enable); + if (result) + return result; + result = inv_plat_single_write(st, REG_FIFO_EN, + saved_regs.fifo_en); + if (result) + return result; + result = inv_plat_single_write(st, REG_USER_CTRL, + saved_regs.user_ctrl); + if (result) + return result; + result = inv_plat_single_write(st, REG_CONFIG, + saved_regs.config); + if (result) + return result; + result = inv_plat_single_write(st, REG_GYRO_CONFIG, + saved_regs.gyro_config); + if (result) + return result; + result = inv_plat_single_write(st, REG_ACCEL_CONFIG, + saved_regs.accel_config); + if (result) + return result; + result = inv_plat_single_write(st, REG_ACCEL_CONFIG_2, + saved_regs.accel_config_2); + if (result) + return result; + result = inv_plat_single_write(st, REG_SAMPLE_RATE_DIV, + saved_regs.smplrt_div); + if (result) + return result; + result = inv_plat_single_write(st, REG_LP_MODE_CTRL, + saved_regs.lp_mode); + if (result) + return result; + result = inv_plat_single_write(st, REG_PWR_MGMT_1, + saved_regs.pwr_mgmt_1); + if (result) + return result; + + result = inv_plat_single_write(st, REG_PWR_MGMT_2, + saved_regs.pwr_mgmt_2); + if (result) + return result; + + return result; +} + +int inv_switch_engine(struct inv_mpu_state *st, bool en, u32 mask) +{ + u8 data, mgmt_1; + int result; + + if (BIT_PWR_GYRO_STBY == mask) { + result = inv_plat_read(st, REG_PWR_MGMT_1, 1, &mgmt_1); + if (result) + return result; + mgmt_1 &= ~BIT_CLK_MASK; + } + + if ((BIT_PWR_GYRO_STBY == mask) && (!en)) { + result = inv_plat_single_write(st, REG_PWR_MGMT_1, mgmt_1); + if (result) + return result; + } + + result = inv_plat_read(st, REG_PWR_MGMT_2, 1, &data); + if (result) + return result; + if (en) + data &= (~mask); + else + data |= mask; + data |= BIT_FIFO_LP_EN; + result = inv_plat_single_write(st, REG_PWR_MGMT_2, data); + if (result) + return result; + + if ((BIT_PWR_GYRO_STBY == mask) && en) { + /* only gyro on needs sensor up time */ + msleep(SENSOR_UP_TIME); + /* after gyro is on & stable, switch internal clock to PLL */ + mgmt_1 |= BIT_CLK_PLL; + result = inv_plat_single_write(st, REG_PWR_MGMT_1, mgmt_1); + if (result) + return result; + } + if ((BIT_PWR_ACCEL_STBY == mask) && en) + msleep(REG_UP_TIME); + + return 0; +} + +int inv_set_offset_reg(struct inv_mpu_state *st, int reg, int val) +{ + int result; + u8 d; + + d = ((val >> 8) & 0xff); + result = inv_plat_single_write(st, reg, d); + if (result) + return result; + + d = (val & 0xff); + result = inv_plat_single_write(st, reg + 1, d); + + return result; +} + +/** +* inv_check_gyro_self_test() - check gyro self test. this function +* returns zero as success. A non-zero return +* value indicates failure in self test. +* @*st: main data structure. +* @*reg_avg: average value of normal test. +* @*st_avg: average value of self test +*/ +int inv_check_gyro_self_test(struct inv_mpu_state *st, + int *reg_avg, int *st_avg) { + u8 regs[3]; + int ret_val, result; + int otp_value_zero = 0; + int st_shift_prod[3], st_shift_cust[3], i; + + ret_val = 0; + result = inv_plat_read(st, REG_6500_XG_ST_DATA, 3, regs); + if (result) + return result; + pr_debug("%s self_test gyro shift_code - %02x %02x %02x\n", + st->hw->name, regs[0], regs[1], regs[2]); + + for (i = 0; i < 3; i++) { + if (regs[i] != 0) { + st_shift_prod[i] = mpu_st_tb[regs[i] - 1]; + } else { + st_shift_prod[i] = 0; + otp_value_zero = 1; + } + } + pr_debug("%s self_test gyro st_shift_prod - %+d %+d %+d\n", + st->hw->name, st_shift_prod[0], st_shift_prod[1], + st_shift_prod[2]); + + for (i = 0; i < 3; i++) { + st_shift_cust[i] = st_avg[i] - reg_avg[i]; + if (!otp_value_zero) { + /* Self Test Pass/Fail Criteria A */ + if (st_shift_cust[i] < DEF_GYRO_CT_SHIFT_DELTA + * st_shift_prod[i]) + ret_val = 1; + } else { + /* Self Test Pass/Fail Criteria B */ + if (st_shift_cust[i] < DEF_GYRO_ST_AL * + DEF_SELFTEST_GYRO_SENS * + DEF_ST_PRECISION) + ret_val = 1; + } + } + pr_debug("%s self_test gyro st_shift_cust - %+d %+d %+d\n", + st->hw->name, st_shift_cust[0], st_shift_cust[1], + st_shift_cust[2]); + + if (ret_val == 0) { + /* Self Test Pass/Fail Criteria C */ + for (i = 0; i < 3; i++) + if (abs(reg_avg[i]) > DEF_GYRO_OFFSET_MAX * + DEF_SELFTEST_GYRO_SENS * + DEF_ST_PRECISION) + ret_val = 1; + } + + return ret_val; +} + +/** +* inv_check_accel_self_test() - check 6500 accel self test. this function +* returns zero as success. A non-zero return +* value indicates failure in self test. +* @*st: main data structure. +* @*reg_avg: average value of normal test. +* @*st_avg: average value of self test +*/ +int inv_check_accel_self_test(struct inv_mpu_state *st, + int *reg_avg, int *st_avg) { + int ret_val, result; + int st_shift_prod[3], st_shift_cust[3], st_shift_ratio[3], i; + u8 regs[3]; + int otp_value_zero = 0; + + ret_val = 0; + result = inv_plat_read(st, REG_6500_XA_ST_DATA, 3, regs); + if (result) + return result; + pr_debug("%s self_test accel shift_code - %02x %02x %02x\n", + st->hw->name, regs[0], regs[1], regs[2]); + + for (i = 0; i < 3; i++) { + if (regs[i] != 0) { + st_shift_prod[i] = mpu_st_tb[regs[i] - 1]; + } else { + st_shift_prod[i] = 0; + otp_value_zero = 1; + } + } + pr_debug("%s self_test accel st_shift_prod - %+d %+d %+d\n", + st->hw->name, st_shift_prod[0], st_shift_prod[1], + st_shift_prod[2]); + + if (!otp_value_zero) { + /* Self Test Pass/Fail Criteria A */ + for (i = 0; i < 3; i++) { + st_shift_cust[i] = st_avg[i] - reg_avg[i]; + st_shift_ratio[i] = abs(st_shift_cust[i] / + st_shift_prod[i] - DEF_ST_PRECISION); + if (st_shift_ratio[i] > DEF_ACCEL_ST_SHIFT_DELTA) + ret_val = 1; + } + } else { + /* Self Test Pass/Fail Criteria B */ + for (i = 0; i < 3; i++) { + st_shift_cust[i] = abs(st_avg[i] - reg_avg[i]); + if (st_shift_cust[i] < ACCEL_ST_AL_MIN || + st_shift_cust[i] > ACCEL_ST_AL_MAX) + ret_val = 1; + } + } + pr_debug("%s self_test accel st_shift_cust - %+d %+d %+d\n", + st->hw->name, st_shift_cust[0], st_shift_cust[1], + st_shift_cust[2]); + + return ret_val; +} + +/* + * inv_do_test() - do the actual test of self testing + */ +int inv_do_test(struct inv_mpu_state *st, int self_test_flag, + int *gyro_result, int *accel_result, int lp_mode) +{ + int result, i, j, packet_size; + u8 data[BYTES_PER_SENSOR * 2], d, dd; + int fifo_count, packet_count, ind, s; + + packet_size = BYTES_PER_SENSOR * 2; + + /* disable interrupt */ + result = inv_plat_single_write(st, REG_INT_ENABLE, 0); + if (result) + return result; + /* disable the sensor output to FIFO */ + result = inv_plat_single_write(st, REG_FIFO_EN, 0); + if (result) + return result; + /* disable fifo reading */ + result = inv_plat_single_write(st, REG_USER_CTRL, 0); + if (result) + return result; + /* clear FIFO */ + result = inv_plat_single_write(st, REG_USER_CTRL, BIT_FIFO_RST); + if (result) + return result; + /* setup parameters */ + result = inv_plat_single_write(st, REG_CONFIG, INV_FILTER_98HZ); + if (result) + return result; + + /* gyro lp mode */ + if (lp_mode == 1) + d = BIT_GYRO_CYCLE_EN; + else if (lp_mode == 2) + d = DEF_SELFTEST_SAMPLE_RATE_ACC_LP; + else + d = 0; + result = inv_plat_single_write(st, REG_LP_MODE_CTRL, d); + if (result) + return result; + + /* config accel LPF register */ + if (lp_mode == 2) + d = BIT_ACCEL_FCHOCIE_B; + else + d = DEF_ST_MPU6500_ACCEL_LPF; + result = inv_plat_single_write(st, REG_6500_ACCEL_CONFIG2, d); + if (result) + return result; + + if (lp_mode) { + result = inv_plat_single_write(st, REG_SAMPLE_RATE_DIV, + DEF_SELFTEST_SAMPLE_RATE_LP); + } else { + result = inv_plat_single_write(st, REG_SAMPLE_RATE_DIV, + DEF_SELFTEST_SAMPLE_RATE); + } + if (result) + return result; + /* wait for the sampling rate change to stabilize */ + mdelay(INV_MPU_SAMPLE_RATE_CHANGE_STABLE); + result = inv_plat_single_write(st, REG_GYRO_CONFIG, + self_test_flag | DEF_SELFTEST_GYRO_FS); + if (result) + return result; + + d = DEF_SELFTEST_6500_ACCEL_FS; + d |= self_test_flag; + result = inv_plat_single_write(st, REG_ACCEL_CONFIG, d); + if (result) + return result; + + /* wait for the output to get stable */ + msleep(DEF_ST_6500_STABLE_TIME); + + /* enable FIFO reading */ + result = inv_plat_single_write(st, REG_USER_CTRL, BIT_FIFO_EN); + if (result) + return result; + /* enable sensor output to FIFO */ + d = BITS_GYRO_OUT | BIT_ACCEL_OUT; + for (i = 0; i < THREE_AXIS; i++) { + gyro_result[i] = 0; + accel_result[i] = 0; + } + s = 0; + while (s < 200 /*st->self_test.samples*/) { + /* Stop FIFO */ + result = inv_plat_single_write(st, REG_USER_CTRL, 0); + if (result) + return result; + /* clear FIFO */ + result = inv_plat_single_write(st, REG_USER_CTRL, BIT_FIFO_RST); + if (result) + return result; + /* enable FIFO reading */ + result = inv_plat_single_write(st, REG_USER_CTRL, BIT_FIFO_EN); + if (result) + return result; + + /* accel lp mode */ + dd = BIT_CLK_PLL; + if (lp_mode == 2) + dd |= BIT_LP_EN; + else + dd &= ~BIT_LP_EN; + result = inv_plat_single_write(st, REG_PWR_MGMT_1, dd); + if (result) + return result; + + result = inv_plat_single_write(st, REG_FIFO_EN, d); + if (result) + return result; + if (lp_mode) + mdelay(DEF_GYRO_WAIT_TIME_LP); + else + mdelay(DEF_GYRO_WAIT_TIME); + + result = inv_plat_single_write(st, REG_FIFO_EN, 0); + if (result) + return result; + + result = inv_plat_read(st, REG_FIFO_COUNT_H, + FIFO_COUNT_BYTE, data); + if (result) + return result; + fifo_count = be16_to_cpup((__be16 *)(&data[0])); + pr_debug("%s self_test fifo_count - %d\n", + st->hw->name, fifo_count); + packet_count = fifo_count / packet_size; + i = 0; + while ((i < packet_count) && (s < 200 /*st->self_test.samples*/)) { + short vals[3]; + result = inv_plat_read(st, REG_FIFO_R_W, + packet_size, data); + if (result) + return result; + ind = 0; + + for (j = 0; j < THREE_AXIS; j++) { + vals[j] = (short)be16_to_cpup( + (__be16 *)(&data[ind + 2 * j])); + accel_result[j] += vals[j]; + } + ind += BYTES_PER_SENSOR; + pr_debug( + "%s self_test accel data - %d %+d %+d %+d", + st->hw->name, s, vals[0], vals[1], vals[2]); + + for (j = 0; j < THREE_AXIS; j++) { + vals[j] = (short)be16_to_cpup( + (__be16 *)(&data[ind + 2 * j])); + gyro_result[j] += vals[j]; + } + pr_debug("%s self_test gyro data - %d %+d %+d %+d", + st->hw->name, s, vals[0], vals[1], vals[2]); + + s++; + i++; + } + } + + for (j = 0; j < THREE_AXIS; j++) { + accel_result[j] = accel_result[j] / s; + accel_result[j] *= DEF_ST_PRECISION; + } + for (j = 0; j < THREE_AXIS; j++) { + gyro_result[j] = gyro_result[j] / s; + gyro_result[j] *= DEF_ST_PRECISION; + } + + return 0; +} + + +int inv_power_up_self_test(struct inv_mpu_state *st) +{ + int result; + + result = inv_switch_power_in_lp(st, true); + + /* make sure no interrupts */ + result = inv_plat_single_write(st, REG_INT_ENABLE, 0); + if (result) + return result; + + if (result) + return result; + result = inv_switch_engine(st, true, BIT_PWR_ACCEL_STBY); + if (result) + return result; + result = inv_switch_engine(st, true, BIT_PWR_GYRO_STBY); + if (result) + return result; + + return 0; +} + +/* + * 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_st[THREE_AXIS], gyro_bias_regular[THREE_AXIS]; + int accel_bias_st[THREE_AXIS], accel_bias_regular[THREE_AXIS]; +#if 0 + int gyro_bias_regular_lp[THREE_AXIS]; + int accel_bias_regular_lp[THREE_AXIS]; + int dummy_bias_regular[THREE_AXIS]; +#endif + int test_times, i; + char accel_result, gyro_result; + + result = inv_save_setting(st); + if (result) + return result; + + result = inv_power_up_self_test(st); + if (result) + return result; + accel_result = 0; + gyro_result = 0; + test_times = DEF_ST_TRY_TIMES; + while (test_times > 0) { + result = inv_do_test(st, 0, gyro_bias_regular, + accel_bias_regular, 0); + if (result == -EAGAIN) + test_times--; + else + test_times = 0; + } + if (result) + goto test_fail; + pr_debug("%s self_test accel bias_regular - %+d %+d %+d\n", + st->hw->name, accel_bias_regular[0], + accel_bias_regular[1], accel_bias_regular[2]); + pr_debug("%s self_test gyro bias_regular - %+d %+d %+d\n", + st->hw->name, gyro_bias_regular[0], gyro_bias_regular[1], + gyro_bias_regular[2]); + + test_times = DEF_ST_TRY_TIMES; + while (test_times > 0) { + result = inv_do_test(st, BITS_SELF_TEST_EN, gyro_bias_st, + accel_bias_st, 0); + if (result == -EAGAIN) + test_times--; + else + break; + } + if (result) + goto test_fail; + pr_debug("%s self_test accel bias_st - %+d %+d %+d\n", + st->hw->name, accel_bias_st[0], accel_bias_st[1], + accel_bias_st[2]); + pr_debug("%s self_test gyro bias_st - %+d %+d %+d\n", + st->hw->name, gyro_bias_st[0], gyro_bias_st[1], + gyro_bias_st[2]); + +#if 0 + /* lp gyro mode */ + test_times = DEF_ST_TRY_TIMES; + while (test_times > 0) { + result = inv_do_test(st, 0, gyro_bias_regular_lp, + dummy_bias_regular, 1); + if (result == -EAGAIN) + test_times--; + else + test_times = 0; + } + if (result) + goto test_fail; + pr_debug("%s self_test gyro bias_regular lp - %+d %+d %+d\n", + st->hw->name, gyro_bias_regular_lp[0], gyro_bias_regular_lp[1], + gyro_bias_regular_lp[2]); + + /* lp accel mode */ + test_times = DEF_ST_TRY_TIMES; + while (test_times > 0) { + result = inv_do_test(st, 0, dummy_bias_regular, + accel_bias_regular_lp, 2); + if (result == -EAGAIN) + test_times--; + else + test_times = 0; + } + if (result) + goto test_fail; + pr_debug("%s self_test accel bias_regular lp - %+d %+d %+d\n", + st->hw->name, accel_bias_regular_lp[0], + accel_bias_regular_lp[1], accel_bias_regular_lp[2]); +#endif + + /* copy bias */ + for (i = 0; i < 3; i++) { + /* gyro : LN bias as LN is default mode */ + st->gyro_st_bias[i] = gyro_bias_regular[i] / DEF_ST_PRECISION; + /* accel : LN bias as LN is default mode */ + st->accel_st_bias[i] = accel_bias_regular[i] / DEF_ST_PRECISION; + } + + /* Check is done on continuous mode data */ + accel_result = !inv_check_accel_self_test(st, + accel_bias_regular, accel_bias_st); + gyro_result = !inv_check_gyro_self_test(st, + gyro_bias_regular, gyro_bias_st); + +test_fail: + inv_recover_setting(st); + return (accel_result << DEF_ST_ACCEL_RESULT_SHIFT) | gyro_result; +} diff --git a/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_setup_20680.c b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_setup_20680.c new file mode 100644 index 000000000000..5e9cf8906d79 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_setup_20680.c @@ -0,0 +1,466 @@ +/* +* 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" + +/* set LN mode for gyro regardless of conditions */ +#define USE_GYRO_LN_MODE + +static int inv_calc_engine_dur(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; +} + +static int inv_turn_on_fifo(struct inv_mpu_state *st) +{ + u8 int_en, fifo_en, mode, user; + int r; + + r = inv_plat_single_write(st, REG_FIFO_EN, 0); + if (r) + return r; + r = inv_plat_single_write(st, REG_USER_CTRL, BIT_FIFO_RST); + if (r) + return r; + fifo_en = 0; + int_en = 0; + + if (st->gesture_only_on && (!st->batch.timeout)) { + st->gesture_int_count = WOM_DELAY_THRESHOLD; + int_en |= BIT_WOM_ALL_INT_EN; + } +#ifdef TIMER_BASED_BATCHING + if (st->chip_config.eis_enable) + int_en |= BIT_FSYNC_INT_EN; + if (!st->batch_timeout) { + int_en |= BIT_DATA_RDY_EN; + } +#else + if (st->batch.timeout) { + if(!st->batch.fifo_wm_th) + int_en = BIT_DATA_RDY_EN; + } else { + int_en = BIT_DATA_RDY_EN; + if (st->chip_config.eis_enable) + int_en |= BIT_FSYNC_INT_EN; + } +#endif + if (st->sensor[SENSOR_GYRO].on) + fifo_en |= BITS_GYRO_FIFO_EN; + + if (st->sensor[SENSOR_ACCEL].on) + fifo_en |= BIT_ACCEL_FIFO_EN; + r = inv_plat_single_write(st, REG_FIFO_EN, fifo_en); + if (r) + return r; + st->int_en = int_en; + r = inv_plat_single_write(st, REG_INT_ENABLE, int_en); + if (r) + return r; + if (st->gesture_only_on && (!st->batch.timeout)) { + mode = BIT_ACCEL_INTEL_EN | BIT_ACCEL_INTEL_MODE; + } else { + mode = 0; + } + r = inv_plat_single_write(st, REG_ACCEL_INTEL_CTRL, mode); +#ifdef SENSOR_DATA_FROM_REGISTERS + user = 0; +#else + user = BIT_FIFO_EN; +#endif + r = inv_plat_single_write(st, REG_USER_CTRL, user | st->i2c_dis); +#ifdef TIMER_BASED_BATCHING + if (fifo_en && st->batch_timeout) { + if (st->is_batch_timer_running) + hrtimer_cancel(&st ->hr_batch_timer); + st->is_batch_timer_running = true; + hrtimer_start(&st ->hr_batch_timer, + ns_to_ktime(st->batch_timeout), HRTIMER_MODE_REL); + } else { + if (st->is_batch_timer_running) + hrtimer_cancel(&st ->hr_batch_timer); + st->is_batch_timer_running = false; + } +#endif + + 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 dur_ms; + + 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; + if (st->mode_1k_on) + ts_algo->first_sample = MODE_1K_INIT_SAMPLE; + else + ts_algo->first_sample = 1; + + dur_ms = st->smplrt_div + 1; + if ((ts_algo->first_sample * dur_ms) < FIRST_SAMPLE_BUF_MS) + ts_algo->first_sample = FIRST_SAMPLE_BUF_MS / dur_ms; + if (ts_algo->first_sample == 0) + ts_algo->first_sample = 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; + + if (st->chip_config.gyro_enable | st->chip_config.accel_enable) { + w = 0; + if (!st->chip_config.gyro_enable) + w |= BIT_PWR_GYRO_STBY; + if (!st->chip_config.accel_enable) + w |= BIT_PWR_ACCEL_STBY; + } else if (st->chip_config.compass_enable) { + w = BIT_PWR_GYRO_STBY; + } else { + w = (BIT_PWR_GYRO_STBY | BIT_PWR_ACCEL_STBY); + } + + r = inv_plat_read(st, REG_PWR_MGMT_2, 1, &v); + if (r) + return r; + r = inv_plat_single_write(st, REG_PWR_MGMT_2, w); + if (r) + return r; + + wait_ms = 0; + if (st->chip_config.gyro_enable + && (v & BIT_PWR_GYRO_STBY)) { + wait_ms = INV_IAM20680_GYRO_START_TIME; + } + if (st->chip_config.accel_enable + && (v & BIT_PWR_ACCEL_STBY)) { + if (INV_IAM20680_ACCEL_START_TIME > wait_ms) + wait_ms = INV_IAM20680_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, int rate) +{ + const short hz[] = {188, 98, 42, 20, 10, 5}; + const int d[] = {INV_FILTER_188HZ, INV_FILTER_98HZ, + INV_FILTER_42HZ, INV_FILTER_20HZ, + INV_FILTER_10HZ, INV_FILTER_5HZ}; + int i, h, data, result; + +#ifdef USE_GYRO_LN_MODE + if (1) { +#else + if (st->chip_config.eis_enable || st->ois.en || st->mode_1k_on) { +#endif + h = (rate >> 1); + i = 0; + while ((h < hz[i]) && (i < ARRAY_SIZE(d) - 1)) + i++; + data = d[i]; + data |= EXT_SYNC_SET; + result = inv_plat_single_write(st, REG_CONFIG, data); + if (result) + return result; + + st->chip_config.lpf = data; + result = inv_plat_single_write(st, REG_LP_MODE_CTRL, 0); + } else { + result = inv_plat_single_write(st, REG_LP_MODE_CTRL, + BIT_GYRO_CYCLE_EN); + if (result) + return result; + data = 0; + result = inv_plat_single_write(st, REG_CONFIG, data | 3); + } + + return result; +} + +static int inv_set_div(struct inv_mpu_state *st, int a_d, int g_d) +{ + int result, div; + + if (st->chip_config.gyro_enable) + div = g_d; + else + div = a_d; + if (st->chip_config.eis_enable) + div = 0; + + st->smplrt_div = div; + pr_debug("div= %d\n", div); + result = inv_plat_single_write(st, REG_SAMPLE_RATE_DIV, div); + + return result; +} + +// 20680 does not support batching +static int inv_set_batch(struct inv_mpu_state *st) +{ +#ifdef TIMER_BASED_BATCHING + u64 timeout; + int required_fifo_size; + + if (st->batch.timeout) { + required_fifo_size = st->batch.timeout * st->eng_info[ENGINE_GYRO].running_rate + * st->batch.pk_size / 1000; + if (required_fifo_size > MAX_BATCH_FIFO_SIZE) { + required_fifo_size = MAX_BATCH_FIFO_SIZE; + timeout = (required_fifo_size / st->batch.pk_size) * (1000 / st->eng_info[ENGINE_GYRO].running_rate); + } else { + timeout = st->batch.timeout; + } + } else { + timeout = 1000 / st->eng_info[ENGINE_GYRO].running_rate; + } + if (timeout <= 1000 / st->eng_info[ENGINE_GYRO].running_rate) + st->batch_timeout = 0; + else + st->batch_timeout = timeout * 1000000; // ms to ns +#endif + st->batch.fifo_wm_th = 0; + + return 0; +} + +static int inv_set_rate(struct inv_mpu_state *st) +{ + int g_d, a_d, result, i; + + 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, st->eng_info[ENGINE_GYRO].running_rate); + if (result) + return result; + // set ADLPF at this point not to change after accel is enabled + result = inv_set_accel_config2(st, false); + st->batch.pk_size = 0; + for (i = 0; i < SENSOR_NUM_MAX; i++) { + if (st->sensor[i].on) + st->batch.pk_size += st->sensor[i].sample_size; + } + + inv_set_batch(st); + + 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; + accel_rate = MPU_INIT_SENSOR_RATE; + /* 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); + + st->ts_algo.clock_base = ENGINE_ACCEL; + + if (g_en) { + /* gyro engine needs to be fastest */ + if (a_en) + gyro_rate = max(gyro_rate, accel_rate); + accel_rate = gyro_rate; + st->ts_algo.clock_base = ENGINE_GYRO; + } else if (a_en) { + /* accel engine needs to be fastest if gyro engine is off */ + gyro_rate = accel_rate; + 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; + if ((gyro_rate >= BASE_SAMPLE_RATE) || + (accel_rate >= BASE_SAMPLE_RATE)) + st->mode_1k_on = true; + else + st->mode_1k_on = false; + /* engine divider for pressure and compass is set later */ + if (st->chip_config.eis_enable || st->mode_1k_on) { + 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->sensor_l[i].rate; + else + st->sensor_l[i].div = 0xffff; + } + } + } else { + st->eng_info[ENGINE_GYRO].divider = BASE_SAMPLE_RATE / + st->eng_info[ENGINE_GYRO].running_rate; + st->eng_info[ENGINE_ACCEL].divider = BASE_SAMPLE_RATE / + st->eng_info[ENGINE_ACCEL].running_rate; + } + + for ( i = 0 ; i < SENSOR_L_NUM_MAX ; i++ ) + st->sensor_l[i].counter = 0; + + inv_calc_engine_dur(&st->eng_info[ENGINE_GYRO]); + inv_calc_engine_dur(&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); + + result = inv_switch_power_in_lp(st, true); + if (result) + return result; + inv_stop_interrupt(st); + inv_determine_engine(st); + result = inv_set_rate(st); + if (result) { + pr_err("inv_set_rate error\n"); + return result; + } + result = inv_turn_on_engine(st); + if (result) { + pr_err("inv_turn_on_engine error\n"); + return result; + } + result = inv_reset_fifo(st, false); + if (result) + return result; + result = inv_switch_power_in_lp(st, false); + if ((!st->chip_config.gyro_enable) && + (!st->chip_config.accel_enable)) { + 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; +} diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_common.c b/drivers/iio/imu/inv_mpu/inv_mpu_common.c new file mode 100644 index 000000000000..33db03418b92 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_mpu_common.c @@ -0,0 +1,988 @@ +/* + * 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. + */ +#define pr_fmt(fmt) "inv_mpu: " fmt +#include "inv_mpu_iio.h" +#ifdef CONFIG_RTC_INTF_ALARM +#include +#endif +#include + +#ifdef CONFIG_RTC_INTF_ALARM +s64 get_time_ns(void) +{ + struct timespec ts; + + /* get_monotonic_boottime(&ts); */ + + /* Workaround for some platform on which monotonic clock and + * Android SystemClock has a gap. + * Use ktime_to_timespec(alarm_get_elapsed_realtime()) instead of + * get_monotonic_boottime() for these platform + */ + + ts = ktime_to_timespec(alarm_get_elapsed_realtime()); + + return timespec_to_ns(&ts); +} +#else +s64 get_time_ns(void) +{ + struct timespec ts; + + get_monotonic_boottime(&ts); + + /* Workaround for some platform on which monotonic clock and + * Android SystemClock has a gap. + * Use ktime_to_timespec(alarm_get_elapsed_realtime()) instead of + * get_monotonic_boottime() for these platform + */ + return timespec_to_ns(&ts); +} + +#endif + +#ifdef ACCEL_BIAS_TEST +int inv_get_3axis_average(s16 src[], s16 dst[], s16 reset) +{ +#define BUFFER_SIZE 200 + static s16 buffer[BUFFER_SIZE][3]; + static s16 current_position = 0; + static s16 ready = 0; + int sum[3]= {0,}; + int i; + + if(reset){ + current_position = 0; + ready = 0; + } + buffer[current_position][0] = src[0]; + buffer[current_position][1] = src[1]; + buffer[current_position][2] = src[2]; + current_position++; + if(current_position == BUFFER_SIZE){ + ready = 1; + current_position = 0; + } + if(ready){ + for(i = 0 ; i < BUFFER_SIZE ; i++){ + sum[0] += buffer[i][0]; + sum[1] += buffer[i][1]; + sum[2] += buffer[i][2]; + } + dst[0] = sum[0]/BUFFER_SIZE; + dst[1] = sum[1]/BUFFER_SIZE; + dst[2] = sum[2]/BUFFER_SIZE; + return 1; + } + return 0; +} +#endif + +int inv_q30_mult(int a, int b) +{ +#define DMP_MULTI_SHIFT 30 + u64 temp; + int result; + + temp = ((u64)a) * b; + result = (int)(temp >> DMP_MULTI_SHIFT); + + return result; +} +#if defined(CONFIG_INV_MPU_IIO_ICM20648) || \ + defined(CONFIG_INV_MPU_IIO_ICM20690) +/* inv_read_secondary(): set secondary registers for reading. + The chip must be set as bank 3 before calling. + */ +int inv_read_secondary(struct inv_mpu_state *st, int ind, int addr, + int reg, int len) +{ + int result; + + result = inv_plat_single_write(st, st->slv_reg[ind].addr, + INV_MPU_BIT_I2C_READ | addr); + if (result) + return result; + result = inv_plat_single_write(st, st->slv_reg[ind].reg, reg); + if (result) + return result; + result = inv_plat_single_write(st, st->slv_reg[ind].ctrl, + INV_MPU_BIT_SLV_EN | len); + + return result; +} + +int inv_execute_read_secondary(struct inv_mpu_state *st, int ind, int addr, + int reg, int len, u8 *d) +{ + int result; + + inv_set_bank(st, BANK_SEL_3); + result = inv_read_secondary(st, ind, addr, reg, len); + if (result) + return result; + inv_set_bank(st, BANK_SEL_0); + result = inv_plat_single_write(st, REG_USER_CTRL, st->i2c_dis | + BIT_I2C_MST_EN); + msleep(SECONDARY_INIT_WAIT); + result = inv_plat_single_write(st, REG_USER_CTRL, st->i2c_dis); + if (result) + return result; + result = inv_plat_read(st, REG_EXT_SLV_SENS_DATA_00, len, d); + + return result; +} + +/* inv_write_secondary(): set secondary registers for writing. + The chip must be set as bank 3 before calling. + */ +int inv_write_secondary(struct inv_mpu_state *st, int ind, int addr, + int reg, int v) +{ + int result; + + result = inv_plat_single_write(st, st->slv_reg[ind].addr, addr); + if (result) + return result; + result = inv_plat_single_write(st, st->slv_reg[ind].reg, reg); + if (result) + return result; + result = inv_plat_single_write(st, st->slv_reg[ind].ctrl, + INV_MPU_BIT_SLV_EN | 1); + + result = inv_plat_single_write(st, st->slv_reg[ind].d0, v); + + return result; +} + +int inv_execute_write_secondary(struct inv_mpu_state *st, int ind, int addr, + int reg, int v) +{ + int result; + + inv_set_bank(st, BANK_SEL_3); + result = inv_write_secondary(st, ind, addr, reg, v); + if (result) + return result; + inv_set_bank(st, BANK_SEL_0); + result = inv_plat_single_write(st, REG_USER_CTRL, st->i2c_dis | + BIT_I2C_MST_EN); + msleep(SECONDARY_INIT_WAIT); + result = inv_plat_single_write(st, REG_USER_CTRL, st->i2c_dis); + + return result; +} + +int inv_set_bank(struct inv_mpu_state *st, u8 bank) +{ +#ifdef CONFIG_INV_MPU_IIO_ICM20648 + int r; + + r = inv_plat_single_write(st, REG_BANK_SEL, bank); + + return r; +#else + return 0; +#endif +} +#endif + +#ifdef CONFIG_INV_MPU_IIO_ICM20648 +/** + * inv_write_cntl() - Write control word to designated address. + * @st: Device driver instance. + * @wd: control word. + * @en: enable/disable. + * @cntl: control address to be written. + */ +int inv_write_cntl(struct inv_mpu_state *st, u16 wd, bool en, int cntl) +{ + int result; + u8 reg[2], d_out[2]; + + result = mem_r(cntl, 2, d_out); + if (result) + return result; + reg[0] = ((wd >> 8) & 0xff); + reg[1] = (wd & 0xff); + if (!en) { + d_out[0] &= ~reg[0]; + d_out[1] &= ~reg[1]; + } else { + d_out[0] |= reg[0]; + d_out[1] |= reg[1]; + } + result = mem_w(cntl, 2, d_out); + + return result; +} +#endif + +int inv_set_power(struct inv_mpu_state *st, bool power_on) +{ + u8 d; + int r; + + if ((!power_on) == st->chip_config.is_asleep) + return 0; + + d = BIT_CLK_PLL; + if (!power_on) + d |= BIT_SLEEP; + + r = inv_plat_single_write(st, REG_PWR_MGMT_1, d); + if (r) + return r; + + if (power_on) + usleep_range(REG_UP_TIME_USEC, REG_UP_TIME_USEC); + + st->chip_config.is_asleep = !power_on; + + return 0; +} +EXPORT_SYMBOL_GPL(inv_set_power); + +int inv_stop_interrupt(struct inv_mpu_state *st) +{ + int res; +#if defined(CONFIG_INV_MPU_IIO_ICM20648) + /* disable_irq_wake alone should work already. However, + it might need system configuration change. From driver side, + we will disable IRQ altogether for non-wakeup sensors. */ + res = inv_plat_read(st, REG_INT_ENABLE, 1, &st->int_en); + if (res) + return res; + res = inv_plat_read(st, REG_INT_ENABLE_2, 1, &st->int_en_2); + if (res) + return res; + res = inv_plat_single_write(st, REG_INT_ENABLE, 0); + if (res) + return res; + res = inv_plat_single_write(st, REG_INT_ENABLE_2, 0); + if (res) + return res; +#endif +#if defined(CONFIG_INV_MPU_IIO_ICM20608D) + res = inv_plat_read(st, REG_INT_ENABLE, 1, &st->int_en); + if (res) + return res; + res = inv_plat_single_write(st, REG_INT_ENABLE, 0); + if (res) + return res; +#endif +#if defined(CONFIG_INV_MPU_IIO_ICM20602) \ + || defined(CONFIG_INV_MPU_IIO_ICM20690) \ + || defined(CONFIG_INV_MPU_IIO_IAM20680) + res = inv_plat_read(st, REG_INT_ENABLE, 1, &st->int_en); + if (res) + return res; + res = inv_plat_single_write(st, REG_INT_ENABLE, 0); + if (res) + return res; +#endif + return 0; +} +int inv_reenable_interrupt(struct inv_mpu_state *st) +{ + int res = 0; +#if defined(CONFIG_INV_MPU_IIO_ICM20648) + res = inv_plat_single_write(st, REG_INT_ENABLE, st->int_en); + if (res) + return res; + res = inv_plat_single_write(st, REG_INT_ENABLE_2, st->int_en_2); + if (res) + return res; +#elif defined(CONFIG_INV_MPU_IIO_ICM20608D) + res = inv_plat_single_write(st, REG_INT_ENABLE, st->int_en); + if (res) + return res; +#endif +#if defined(CONFIG_INV_MPU_IIO_ICM20602) \ + || defined(CONFIG_INV_MPU_IIO_ICM20690) \ + || defined(CONFIG_INV_MPU_IIO_IAM20680) + res = inv_plat_single_write(st, REG_INT_ENABLE, st->int_en); + if (res) + return res; +#endif + return res; +} + +static int inv_lp_en_off_mode(struct inv_mpu_state *st, bool on) +{ + int r; + + if (!st->chip_config.is_asleep) + return 0; + + r = inv_plat_single_write(st, REG_PWR_MGMT_1, BIT_CLK_PLL); + st->chip_config.is_asleep = 0; + + return r; +} +#ifdef CONFIG_INV_MPU_IIO_ICM20648 +static int inv_lp_en_on_mode(struct inv_mpu_state *st, bool on) +{ + int r = 0; + u8 w; + + if ((!st->chip_config.is_asleep) && + ((!on) == st->chip_config.lp_en_set)) + return 0; + + w = BIT_CLK_PLL; + if ((!on) && (!st->eis.eis_triggered)) + w |= BIT_LP_EN; + r = inv_plat_single_write(st, REG_PWR_MGMT_1, w); + st->chip_config.is_asleep = 0; + st->chip_config.lp_en_set = (!on); + return r; +} +#endif +#if defined(CONFIG_INV_MPU_IIO_ICM20602) \ + || defined(CONFIG_INV_MPU_IIO_ICM20690) \ + || defined(CONFIG_INV_MPU_IIO_IAM20680) +int inv_set_accel_config2(struct inv_mpu_state *st, bool cycle_mode) +{ + int cycle_freq[] = {275, 192, 111, 59}; + int cont_freq[] = {219, 219, 99, 45, 22, 11, 6}; + int i, r, rate; + u8 v; + + v = 0; +#ifdef CONFIG_INV_MPU_IIO_ICM20690 + v |= BIT_FIFO_SIZE_1K; +#endif + if (cycle_mode) { + rate = (st->eng_info[ENGINE_ACCEL].running_rate << 1); + i = ARRAY_SIZE(cycle_freq) - 1; + while (i > 0) { + if (rate < cycle_freq[i]) { + break; + } + i--; + } + r = inv_plat_single_write(st, REG_ACCEL_CONFIG_2, v | + (i << 4) | 7); + if (r) + return r; + } else { + rate = (st->eng_info[ENGINE_ACCEL].running_rate >> 1); + for (i = 1; i < ARRAY_SIZE(cont_freq); i++) { + if (rate >= cont_freq[i]) + break; + } + if (i > 6) + i = 6; + r = inv_plat_single_write(st, REG_ACCEL_CONFIG_2, v | i); + if (r) + return r; + } + + return 0; +} +static int inv_lp_en_on_mode(struct inv_mpu_state *st, bool on) +{ + int r = 0; + u8 w; + bool cond_check; + + if ((!st->chip_config.is_asleep) && + ((!on) == st->chip_config.lp_en_set)) + return 0; + cond_check = (!on) && st->cycle_on; + + w = BIT_CLK_PLL; + r = inv_plat_single_write(st, REG_PWR_MGMT_1, w); + if (cond_check) { + w |= BIT_LP_EN; + inv_set_accel_config2(st, true); + st->chip_config.lp_en_set = true; + r = inv_plat_single_write(st, REG_PWR_MGMT_1, w); + } else { + inv_set_accel_config2(st, false); +#ifdef CONFIG_INV_MPU_IIO_ICM20690 + r = inv_plat_single_write(st, REG_PWR_MGMT_1, w | BIT_SLEEP); + if (r) + return r; +#endif + st->chip_config.lp_en_set = false; + r = inv_plat_single_write(st, REG_PWR_MGMT_1, w); + msleep(10); + } + st->chip_config.is_asleep = 0; + + return r; +} +#endif +#ifdef CONFIG_INV_MPU_IIO_ICM20608D +static int inv_set_accel_config2(struct inv_mpu_state *st) +{ + int cont_freq[] = {219, 219, 99, 45, 22, 11, 6}; + int dec2_cfg = 0; + int i, r, rate; + + rate = (st->eng_info[ENGINE_ACCEL].running_rate << 1); + i = 0; + if (!st->chip_config.eis_enable){ + while ((rate < cont_freq[i]) && (i < ARRAY_SIZE(cont_freq) - 1)) + i++; + dec2_cfg = 2<<4; //4x + } + r = inv_plat_single_write(st, REG_ACCEL_CONFIG_2, i | dec2_cfg); + if (r) + return r; + return 0; +} +static int inv_lp_en_on_mode(struct inv_mpu_state *st, bool on) +{ + int r = 0; + u8 w; + + w = BIT_CLK_PLL; + if ((!on) && (!st->chip_config.eis_enable)) + w |= BIT_LP_EN; + inv_set_accel_config2(st); + r = inv_plat_single_write(st, REG_PWR_MGMT_1, w); + + return r; +} +#endif +int inv_switch_power_in_lp(struct inv_mpu_state *st, bool on) +{ + int r; + + if (st->chip_config.lp_en_mode_off) + r = inv_lp_en_off_mode(st, on); + else + r = inv_lp_en_on_mode(st, on); + + return r; +} +EXPORT_SYMBOL_GPL(inv_switch_power_in_lp); + +int write_be16_to_mem(struct inv_mpu_state *st, u16 data, int addr) +{ + u8 d[2]; + + d[0] = (data >> 8) & 0xff; + d[1] = data & 0xff; + + return mem_w(addr, sizeof(d), d); +} + +int write_be32_to_mem(struct inv_mpu_state *st, u32 data, int addr) +{ + cpu_to_be32s(&data); + return mem_w(addr, sizeof(data), (u8 *)&data); +} + +int read_be16_from_mem(struct inv_mpu_state *st, u16 *o, int addr) +{ + int result; + u8 d[2]; + + result = mem_r(addr, 2, (u8 *) &d); + *o = d[0] << 8 | d[1]; + + return result; +} + +int read_be32_from_mem(struct inv_mpu_state *st, u32 *o, int addr) +{ + int result; + u32 d = 0; + + result = mem_r(addr, 4, (u8 *) &d); + *o = be32_to_cpup((__be32 *)(&d)); + + return result; +} + +int be32_to_int(u8 *d) +{ + return (d[0] << 24) | (d[1] << 16) | (d[2] << 8) | d[3]; +} + +u32 inv_get_cntr_diff(u32 curr_counter, u32 prev) +{ + u32 diff; + + if (curr_counter > prev) + diff = curr_counter - prev; + else + diff = 0xffffffff - prev + curr_counter + 1; + + return diff; +} + +int inv_write_2bytes(struct inv_mpu_state *st, int addr, int data) +{ + u8 d[2]; + + if (data < 0 || data > USHRT_MAX) + return -EINVAL; + + d[0] = (u8) ((data >> 8) & 0xff); + d[1] = (u8) (data & 0xff); + + return mem_w(addr, ARRAY_SIZE(d), d); +} + + + +int inv_process_eis(struct inv_mpu_state *st, u16 delay) +{ + int tmp1, tmp2, tmp3; + + switch (st->eis.voting_state) { + case 0: + st->eis.gyro_counter_s[0] = st->eis.gyro_counter; + st->eis.fsync_delay_s[0] = delay - st->eis.fsync_delay; + st->eis.voting_count = 1; + st->eis.voting_count_sub = 0; + st->eis.voting_state = 1; + break; + case 1: + if (abs(st->eis.gyro_counter_s[0] - + st->eis.gyro_counter) <= 1) { + st->eis.voting_count++; + } else { + st->eis.gyro_counter_s[2] = st->eis.gyro_counter; + st->eis.voting_count_sub++; + st->eis.voting_state = 2; + } + if (st->eis.voting_count > 5) + st->eis.voting_state = 3; + break; + case 2: + tmp1 = abs(st->eis.gyro_counter_s[0] - st->eis.gyro_counter); + tmp2 = abs(st->eis.gyro_counter_s[2] - st->eis.gyro_counter); + + if ((tmp1 < tmp2) && (tmp1 <= 1)) + st->eis.voting_count++; + else + st->eis.voting_count_sub++; + if (st->eis.voting_count > 5) { + st->eis.voting_state = 3; + st->eis.voting_count = 0; + st->eis.voting_count_sub = 0; + } + + if (st->eis.voting_count_sub > 5) { + st->eis.gyro_counter_s[0] = st->eis.gyro_counter; + st->eis.fsync_delay_s[0] = delay - st->eis.fsync_delay; + st->eis.voting_state = 1; + st->eis.voting_count = 1; + st->eis.voting_count_sub = 0; + } + break; + case 3: + tmp1 = abs(st->eis.gyro_counter_s[0] - st->eis.gyro_counter); + if (tmp1 == 1) { + st->eis.gyro_counter_s[1] = st->eis.gyro_counter; + st->eis.fsync_delay_s[1] = delay - st->eis.fsync_delay; + st->eis.voting_state = 4; + st->eis.voting_count_sub = 1; + st->eis.voting_count = 1; + } + break; + case 4: + if (st->eis.gyro_counter == st->eis.gyro_counter_s[0]) { + tmp1 = delay - st->eis.fsync_delay; + tmp2 = abs(tmp1 - st->eis.fsync_delay_s[0]); + if (tmp2 < 3) { + st->eis.voting_count++; + } else { + st->eis.fsync_delay_s[2] = tmp1; + st->eis.voting_count_sub = 1; + st->eis.voting_state = 5; + } + if (st->eis.voting_count > 5) { + st->eis.voting_count = 1; + st->eis.voting_state = 6; + } + } + break; + case 5: + if (st->eis.gyro_counter == st->eis.gyro_counter_s[0]) { + tmp1 = delay - st->eis.fsync_delay; + + tmp2 = abs(tmp1 - st->eis.fsync_delay_s[0]); + tmp3 = abs(tmp1 - st->eis.fsync_delay_s[2]); + if ((tmp2 < tmp3) && (tmp2 < 3)) + st->eis.voting_count++; + else + st->eis.voting_count_sub++; + if ((st->eis.voting_count > 5) && + (st->eis.voting_count_sub + < st->eis.voting_count)) { + st->eis.voting_state = 6; + st->eis.voting_count = 1; + } else if (st->eis.voting_count_sub > 5) { + st->eis.fsync_delay_s[0] = tmp1; + st->eis.voting_state = 4; + st->eis.voting_count = 1; + } + + } + break; + case 6: + if (st->eis.gyro_counter == st->eis.gyro_counter_s[1]) { + tmp1 = delay - st->eis.fsync_delay; + tmp2 = abs(tmp1 - st->eis.fsync_delay_s[1]); + if (tmp2 < 3) { + st->eis.voting_count++; + } else { + st->eis.fsync_delay_s[2] = tmp1; + st->eis.voting_count_sub = 1; + st->eis.voting_count = 1; + st->eis.voting_state = 7; + } + if (st->eis.voting_count > 5) + st->eis.voting_state = 8; + } + break; + case 7: + if (st->eis.gyro_counter == st->eis.gyro_counter_s[1]) { + tmp1 = delay - st->eis.fsync_delay; + + tmp2 = abs(tmp1 - st->eis.fsync_delay_s[1]); + tmp3 = abs(tmp1 - st->eis.fsync_delay_s[2]); + if ((tmp2 < tmp3) && (tmp2 < 3)) + st->eis.voting_count++; + else + st->eis.voting_count_sub++; + if ((st->eis.voting_count > 5) && + (st->eis.voting_count_sub + < st->eis.voting_count)) { + st->eis.voting_state = 8; + } else if (st->eis.voting_count_sub > 5) { + st->eis.fsync_delay_s[1] = tmp1; + st->eis.voting_state = 6; + st->eis.voting_count = 1; + } + + } + break; + default: + break; + } + + pr_debug("de= %d gc= %d\n", delay, st->eis.gyro_counter); + st->eis.fsync_delay = delay; + st->eis.gyro_counter = 0; + + pr_debug("state=%d g1= %d d1= %d g2= %d d2= %d\n", + st->eis.voting_state, + st->eis.gyro_counter_s[0], + st->eis.fsync_delay_s[0], + st->eis.gyro_counter_s[1], + st->eis.fsync_delay_s[1]); + + return 0; +} + +int inv_rate_convert(struct inv_mpu_state *st, int ind, int data) +{ + int t, out, out1, out2; + int base_freq; + + if (data <= MPU_DEFAULT_DMP_FREQ) + base_freq = MPU_DEFAULT_DMP_FREQ; + else + base_freq = BASE_SAMPLE_RATE; + + t = base_freq / data; + if (!t) + t = 1; + out1 = base_freq / (t + 1); + out2 = base_freq / t; + if ((data - out1) * INV_ODR_BUFFER_MULTI < data) + out = out1; + else + out = out2; + + return out; +} + +static void inv_check_wake_non_wake(struct inv_mpu_state *st, + enum SENSOR_L wake, enum SENSOR_L non_wake) +{ + int tmp_rate; + + if (!st->sensor_l[wake].on && !st->sensor_l[non_wake].on) + return; + + tmp_rate = MPU_INIT_SENSOR_RATE; + if (st->sensor_l[wake].on) + tmp_rate = st->sensor_l[wake].rate; + if (st->sensor_l[non_wake].on) + tmp_rate = max(tmp_rate, st->sensor_l[non_wake].rate); + st->sensor_l[wake].rate = tmp_rate; + st->sensor_l[non_wake].rate = tmp_rate; +} + +static void inv_check_wake_non_wake_divider(struct inv_mpu_state *st, + enum SENSOR_L wake, enum SENSOR_L non_wake) +{ + if (st->sensor_l[wake].on && st->sensor_l[non_wake].on) + st->sensor_l[non_wake].div = 0xffff; + +} + +#if defined(CONFIG_INV_MPU_IIO_ICM20602) \ + || defined(CONFIG_INV_MPU_IIO_ICM20690) \ + || defined(CONFIG_INV_MPU_IIO_IAM20680) +int inv_check_sensor_on(struct inv_mpu_state *st) +{ + int i, max_rate; + enum SENSOR_L wake[] = {SENSOR_L_GYRO_WAKE, SENSOR_L_ACCEL_WAKE, + SENSOR_L_MAG_WAKE}; + enum SENSOR_L non_wake[] = {SENSOR_L_GYRO, SENSOR_L_ACCEL, + SENSOR_L_MAG}; + + st->sensor_l[SENSOR_L_GESTURE_ACCEL].rate = GESTURE_ACCEL_RATE; + for (i = 0; i < SENSOR_NUM_MAX; i++) + st->sensor[i].on = false; + for (i = 0; i < SENSOR_NUM_MAX; i++) + st->sensor[i].rate = MPU_INIT_SENSOR_RATE; + + 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.pick_up_enable + || st->chip_config.tilt_enable) + && (!st->sensor_l[SENSOR_L_ACCEL].on) + && (!st->sensor_l[SENSOR_L_ACCEL_WAKE].on)) + st->sensor_l[SENSOR_L_GESTURE_ACCEL].on = true; + else + st->sensor_l[SENSOR_L_GESTURE_ACCEL].on = false; + + + st->chip_config.wake_on = false; + for (i = 0; i < SENSOR_L_NUM_MAX; i++) { + if (st->sensor_l[i].on && st->sensor_l[i].rate) { + st->sensor[st->sensor_l[i].base].on = true; + st->chip_config.wake_on |= st->sensor_l[i].wake_on; + } + } + if (st->sensor_l[SENSOR_L_GESTURE_ACCEL].on && + (!st->sensor[SENSOR_GYRO].on) && + (!st->sensor[SENSOR_COMPASS].on)) + st->gesture_only_on = true; + else + st->gesture_only_on = false; + + for (i = 0; i < SENSOR_L_NUM_MAX; i++) { + if (st->sensor_l[i].on) { + st->sensor[st->sensor_l[i].base].rate = + max(st->sensor[st->sensor_l[i].base].rate, + st->sensor_l[i].rate); + } + } + max_rate = MPU_INIT_SENSOR_RATE; + if (st->chip_config.eis_enable) { + max_rate = ESI_GYRO_RATE; + st->sensor_l[SENSOR_L_EIS_GYRO].rate = ESI_GYRO_RATE; + } + + for (i = 0; i < SENSOR_NUM_MAX; i++) { + if (st->sensor[i].on) { + max_rate = max(max_rate, st->sensor[i].rate); + } + } + for (i = 0; i < SENSOR_NUM_MAX; i++) { + if (st->sensor[i].on) { + st->sensor[i].rate = max_rate; + } + } + for (i = 0; i < ARRAY_SIZE(wake); i++) + inv_check_wake_non_wake(st, wake[i], non_wake[i]); + + for (i = 0; i < SENSOR_L_NUM_MAX; i++) { + if (st->sensor_l[i].on) { + if (st->sensor_l[i].rate) + st->sensor_l[i].div = + st->sensor[st->sensor_l[i].base].rate + / st->sensor_l[i].rate; + else + st->sensor_l[i].div = 0xffff; + pr_debug("sensor= %d, div= %d\n", + i, st->sensor_l[i].div); + } + } + for (i = 0; i < ARRAY_SIZE(wake); i++) + inv_check_wake_non_wake_divider(st, wake[i], non_wake[i]); + + if (st->step_detector_wake_l_on || + st->step_counter_wake_l_on || + st->chip_config.pick_up_enable || + st->chip_config.tilt_enable) + st->chip_config.wake_on = true; + + return 0; +} +#else +static void inv_do_check_sensor_on(struct inv_mpu_state *st, + enum SENSOR_L *wake, + enum SENSOR_L *non_wake, int sensor_size) +{ + int i; + + for (i = 0; i < SENSOR_NUM_MAX; i++) + st->sensor[i].on = false; + + for (i = 0; i < SENSOR_NUM_MAX; i++) + st->sensor[i].rate = MPU_INIT_SENSOR_RATE; + + st->chip_config.wake_on = false; + for (i = 0; i < SENSOR_L_NUM_MAX; i++) { + if (st->sensor_l[i].on && st->sensor_l[i].rate) { + st->sensor[st->sensor_l[i].base].on = true; + st->chip_config.wake_on |= st->sensor_l[i].wake_on; + } + } + + for (i = 0; i < SENSOR_L_NUM_MAX; i++) { + if (st->sensor_l[i].on) { + st->sensor[st->sensor_l[i].base].rate = + max(st->sensor[st->sensor_l[i].base].rate, + st->sensor_l[i].rate); + } + } + for (i = 0; i < sensor_size; i++) + inv_check_wake_non_wake(st, wake[i], non_wake[i]); + + for (i = 0; i < SENSOR_L_NUM_MAX; i++) { + if (st->sensor_l[i].on) { + if (st->sensor_l[i].rate) + st->sensor_l[i].div = + st->sensor[st->sensor_l[i].base].rate + / st->sensor_l[i].rate; + else + st->sensor_l[i].div = 0xffff; + } + } + for (i = 0; i < sensor_size; i++) + inv_check_wake_non_wake_divider(st, wake[i], non_wake[i]); + + if (st->step_detector_wake_l_on || + st->step_counter_wake_l_on || + st->chip_config.pick_up_enable || + st->chip_config.tilt_enable || + st->smd.on) + st->chip_config.wake_on = true; + +} +#endif + +#if defined(CONFIG_INV_MPU_IIO_ICM20608D) +int inv_check_sensor_on(struct inv_mpu_state *st) +{ + enum SENSOR_L wake[] = {SENSOR_L_GYRO_WAKE, SENSOR_L_ACCEL_WAKE, + SENSOR_L_SIXQ_WAKE, SENSOR_L_PEDQ_WAKE, + SENSOR_L_GYRO_CAL_WAKE}; + enum SENSOR_L non_wake[] = {SENSOR_L_GYRO, SENSOR_L_ACCEL, + SENSOR_L_SIXQ, SENSOR_L_PEDQ, + SENSOR_L_GYRO_CAL}; + + inv_do_check_sensor_on(st, wake, non_wake, ARRAY_SIZE(wake)); + + return 0; +} +#endif + +#if defined(CONFIG_INV_MPU_IIO_ICM20648) +int inv_check_sensor_on(struct inv_mpu_state *st) +{ + enum SENSOR_L wake[] = {SENSOR_L_GYRO_WAKE, SENSOR_L_ACCEL_WAKE, + SENSOR_L_MAG_WAKE, SENSOR_L_ALS_WAKE, + SENSOR_L_SIXQ_WAKE, SENSOR_L_PEDQ_WAKE, + SENSOR_L_NINEQ_WAKE, SENSOR_L_GEOMAG_WAKE, + SENSOR_L_PRESSURE_WAKE, + SENSOR_L_GYRO_CAL_WAKE, + SENSOR_L_MAG_CAL_WAKE}; + enum SENSOR_L non_wake[] = {SENSOR_L_GYRO, SENSOR_L_ACCEL, + SENSOR_L_MAG, SENSOR_L_ALS, + SENSOR_L_SIXQ, SENSOR_L_PEDQ, + SENSOR_L_NINEQ, SENSOR_L_GEOMAG, + SENSOR_L_PRESSURE, + SENSOR_L_GYRO_CAL, + SENSOR_L_MAG_CAL}; + + inv_do_check_sensor_on(st, wake, non_wake, ARRAY_SIZE(wake)); + + return 0; +} +#endif + +#ifdef CONFIG_PM_SLEEP +int inv_mpu_suspend(struct iio_dev *indio_dev) +{ + struct inv_mpu_state *st = iio_priv(indio_dev); + + /* add code according to different request Start */ + dev_info(st->dev, "%s suspend\n", st->hw->name); + mutex_lock(&indio_dev->mlock); + + st->resume_state = false; + if (st->chip_config.wake_on) { + enable_irq_wake(st->irq); + } else { + inv_stop_interrupt(st); + } + + mutex_unlock(&indio_dev->mlock); + + return 0; +} +EXPORT_SYMBOL_GPL(inv_mpu_suspend); + +/* + * inv_mpu_complete(): complete method for this driver. + * This method can be modified according to the request of different + * customers. It basically undo everything suspend is doing + * and recover the chip to what it was before suspend. We use complete to + * make sure that alarm clock resume is finished. If we use resume, the + * alarm clock may not resume yet and get incorrect clock reading. + */ +void inv_mpu_complete(struct iio_dev *indio_dev) +{ + struct inv_mpu_state *st = iio_priv(indio_dev); + + dev_info(st->dev, "%s resume\n", st->hw->name); + if (st->resume_state) + return; + + mutex_lock(&indio_dev->mlock); + + if (!st->chip_config.wake_on) { + inv_reenable_interrupt(st); + } else { + disable_irq_wake(st->irq); + } + /* resume state is used to synchronize read_fifo such that it won't + proceed unless resume is finished. */ + st->resume_state = true; + /* resume flag is indicating that current clock reading is from resume, + it has up to 1 second drift and should do proper processing */ + st->ts_algo.resume_flag = true; + mutex_unlock(&indio_dev->mlock); + wake_up_interruptible(&st->wait_queue); + + return; +} +EXPORT_SYMBOL_GPL(inv_mpu_complete); +#endif diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_dts.c b/drivers/iio/imu/inv_mpu/inv_mpu_dts.c new file mode 100644 index 000000000000..0b8b3fc29b0a --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_mpu_dts.c @@ -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 +#include +#include +#include +#include +#include +#include + +#include +#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 */ diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_dts.h b/drivers/iio/imu/inv_mpu/inv_mpu_dts.h new file mode 100644 index 000000000000..90966febb930 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_mpu_dts.h @@ -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 +#include + +#ifdef CONFIG_OF +int invensense_mpu_parse_dt(struct device *dev, + struct mpu_platform_data *pdata); +#endif + +#endif /* #ifndef _INV_MPU_DTS_H_ */ diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_i2c.c b/drivers/iio/imu/inv_mpu/inv_mpu_i2c.c new file mode 100644 index 000000000000..e7838fce84a8 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_mpu_i2c.c @@ -0,0 +1,556 @@ +/* +* 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 = ® + 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); +} + +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; +} + +/* + * 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; + } + +#ifdef KERNEL_VERSION_4_X + indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*st)); + if (indio_dev == NULL) { + pr_err("memory allocation failed\n"); + result = -ENOMEM; + goto out_no_free; + } +#else + indio_dev = iio_device_alloc(sizeof(*st)); + if (indio_dev == NULL) { + pr_err("memory allocation failed\n"); + result = -ENOMEM; + goto out_no_free; + } +#endif + 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; + st->mem_write = inv_i2c_mem_write; + st->mem_read = inv_i2c_mem_read; + st->dev = &client->dev; + st->bus_type = BUS_I2C; +#ifdef CONFIG_OF + result = invensense_mpu_parse_dt(st->dev, &st->plat_data); + if (result) +# ifdef KERNEL_VERSION_4_X + return -ENODEV; +# else + goto out_free; +# 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); +# ifdef KERNEL_VERSION_4_X + return -ENODEV; +# else + goto out_free; +# endif + } + pr_info("%s: power on here.\n", __func__); + } + pr_info("%s: power on.\n", __func__); + + msleep(100); +#else + if (dev_get_platdata(st->dev) == NULL) +# ifdef KERNEL_VERSION_4_X + return -ENODEV; +# else + goto out_free; +# endif + st->plat_data = *(struct mpu_platform_data *)dev_get_platdata(st->dev); +#endif + + /* power is turned on inside check chip type */ + result = inv_check_chip_type(indio_dev, id->name); + if (result) +#ifdef KERNEL_VERSION_4_X + return -ENODEV; +#else + goto out_free; +#endif + + /* Make state variables available to all _show and _store functions. */ + i2c_set_clientdata(client, indio_dev); + indio_dev->dev.parent = st->dev; + indio_dev->name = id->name; + + st->irq = client->irq; + + result = inv_mpu_configure_ring(indio_dev); + if (result) { + pr_err("configure ring buffer fail\n"); + goto out_free; + } +#ifdef KERNEL_VERSION_4_X + INV_I2C_SETIRQ(IRQ_MPU, st->irq); + result = devm_iio_device_register(st->dev, indio_dev); + if (result) { + pr_err("IIO device register fail\n"); + goto out_unreg_ring; + } +#else + result = iio_buffer_register(indio_dev, indio_dev->channels, + indio_dev->num_channels); + if (result) { + pr_err("ring buffer register fail\n"); + goto out_unreg_ring; + } + INV_I2C_SETIRQ(IRQ_MPU, client->irq); + result = iio_device_register(indio_dev); + if (result) { + pr_err("IIO device register fail\n"); + goto out_remove_ring; + } +#endif + + 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; +#ifdef KERNEL_VERSION_4_X +out_unreg_iio: + devm_iio_device_unregister(st->dev, indio_dev); +out_unreg_ring: + inv_mpu_unconfigure_ring(indio_dev); +out_free: + devm_iio_device_free(st->dev, indio_dev); +out_no_free: +#else +out_unreg_iio: + iio_device_unregister(indio_dev); +out_remove_ring: + iio_buffer_unregister(indio_dev); +out_unreg_ring: + inv_mpu_unconfigure_ring(indio_dev); +out_free: + iio_device_free(indio_dev); +out_no_free: +#endif + dev_err(st->dev, "%s failed %d\n", __func__, result); + + return -EIO; +} + +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 */ + result = inv_plat_single_write(st, REG_PWR_MGMT_1, BIT_H_RESET); + 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); + +#ifdef KERNEL_VERSION_4_X + devm_iio_device_unregister(st->dev, indio_dev); +#else + iio_device_unregister(indio_dev); + iio_buffer_unregister(indio_dev); +#endif + inv_mpu_unconfigure_ring(indio_dev); +#ifdef KERNEL_VERSION_4_X + devm_iio_device_free(st->dev, indio_dev); +#else + iio_device_free(indio_dev); +#endif + 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}, + {"icm20690", ICM20690}, + {"icm20602", ICM20602}, + {"iam20680", IAM20680}, +#endif + {} +}; + +MODULE_DEVICE_TABLE(i2c, inv_mpu_id); + +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, + .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"); diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu/inv_mpu_iio.h new file mode 100644 index 000000000000..9e7316558eae --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_mpu_iio.h @@ -0,0 +1,1138 @@ +/* + * 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. + */ + +#ifndef _INV_MPU_IIO_H_ +#define _INV_MPU_IIO_H_ + +#include +#if (LINUX_VERSION_CODE >= KERNEL_VERSION(4,0,0)) +#define KERNEL_VERSION_4_X +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef CONFIG_HAS_WAKELOCK +#include +#else +#include +#endif +#include + +#include +#include +#include + +#ifdef CONFIG_INV_MPU_IIO_ICM20648 +#include "icm20648/dmp3Default.h" +#endif +#ifdef CONFIG_INV_MPU_IIO_ICM20608D +#include "icm20608d/dmp3Default_20608D.h" +#endif + +#include "inv_test/inv_counters.h" + +#if defined(CONFIG_INV_MPU_IIO_ICM20648) +#include "icm20648/inv_mpu_iio_reg_20648.h" +#elif defined(CONFIG_INV_MPU_IIO_ICM20602) +#include "icm20602/inv_mpu_iio_reg_20602.h" +#elif defined(CONFIG_INV_MPU_IIO_ICM20608D) +#include "icm20608d/inv_mpu_iio_reg_20608.h" +#elif defined(CONFIG_INV_MPU_IIO_ICM20690) +#include "icm20690/inv_mpu_iio_reg_20690.h" +#elif defined(CONFIG_INV_MPU_IIO_IAM20680) +#include "iam20680/inv_mpu_iio_reg_20680.h" +#endif + +#define INVENSENSE_DRIVER_VERSION "8.1.2-simple-test1" + +/* #define DEBUG */ + +/* #define ACCEL_BIAS_TEST */ + +/* #define BIAS_CONFIDENCE_HIGH 1 */ + +#define MAX_FIFO_READ_SIZE 128 +#define MAX_DMP_READ_SIZE 16 + +/* data header defines */ +#define WAKE_HDR 0x8000 + +#define ACCEL_HDR 1 +#define GYRO_HDR 2 +#define COMPASS_HDR 3 +#define ALS_HDR 4 +#define SIXQUAT_HDR 5 +#define NINEQUAT_HDR 6 +#define PEDQUAT_HDR 7 +#define GEOMAG_HDR 8 +#define PRESSURE_HDR 9 +#define GYRO_CALIB_HDR 10 +#define COMPASS_CALIB_HDR 11 +#define STEP_COUNTER_HDR 12 +#define STEP_DETECTOR_HDR 13 +#define STEP_COUNT_HDR 14 +#define ACTIVITY_HDR 15 +#define PICK_UP_HDR 16 +#define EMPTY_MARKER 17 +#define END_MARKER 18 +#define COMPASS_ACCURACY_HDR 19 +#define ACCEL_ACCURACY_HDR 20 +#define GYRO_ACCURACY_HDR 21 +#define EIS_GYRO_HDR 36 +#define EIS_CALIB_HDR 37 +#define LPQ_HDR 38 + +#define ACCEL_WAKE_HDR (ACCEL_HDR | WAKE_HDR) +#define GYRO_WAKE_HDR (GYRO_HDR | WAKE_HDR) +#define COMPASS_WAKE_HDR (COMPASS_HDR | WAKE_HDR) +#define ALS_WAKE_HDR (ALS_HDR | WAKE_HDR) +#define SIXQUAT_WAKE_HDR (SIXQUAT_HDR | WAKE_HDR) +#define NINEQUAT_WAKE_HDR (NINEQUAT_HDR | WAKE_HDR) +#define PEDQUAT_WAKE_HDR (PEDQUAT_HDR | WAKE_HDR) +#define GEOMAG_WAKE_HDR (GEOMAG_HDR | WAKE_HDR) +#define PRESSURE_WAKE_HDR (PRESSURE_HDR | WAKE_HDR) +#define GYRO_CALIB_WAKE_HDR (GYRO_CALIB_HDR | WAKE_HDR) +#define COMPASS_CALIB_WAKE_HDR (COMPASS_CALIB_HDR | WAKE_HDR) +#define STEP_COUNTER_WAKE_HDR (STEP_COUNTER_HDR | WAKE_HDR) +#define STEP_DETECTOR_WAKE_HDR (STEP_DETECTOR_HDR | WAKE_HDR) + +/* init parameters */ +#define MPU_INIT_SMD_THLD 1500 +#define MPU_INIT_GYRO_SCALE 3 +#define MPU_INIT_ACCEL_SCALE 2 +#define MPU_INIT_PED_INT_THRESH 2 +#define MPU_INIT_PED_STEP_THRESH 6 +#define MPU_4X_TS_GYRO_SHIFT (3160000 / 2) +#define DMP_START_ADDR_20645 0x900 +#define DMP_START_ADDR_20648 0x1000 +#define DMP_START_ADDR_10340 0x0a60 +#define DMP_START_ADDR_20608D 0x4B0 +#define MAX_WR_SZ 100 +#define WOM_DELAY_THRESHOLD 200 +#define INV_ODR_BUFFER_MULTI 20 +#define INV_ODR_OVER_FACTOR 20 + +#define COVARIANCE_SIZE 14 +#define ACCEL_COVARIANCE_SIZE (COVARIANCE_SIZE * sizeof(int)) + +enum inv_bus_type { + BUS_I2C = 0, + BUS_SPI, +}; + +struct inv_mpu_state; + +enum INV_ENGINE { + ENGINE_GYRO = 0, + ENGINE_ACCEL, + ENGINE_PRESSURE, + ENGINE_I2C, + ENGINE_NUM_MAX, +}; + +/** + * struct inv_hw_s - Other important hardware information. + * @num_reg: Number of registers on device. + * @name: name of the chip + */ +struct inv_hw_s { + u8 num_reg; + u8 *name; +}; + +/** + * struct inv_sensor - information for each sensor. + * @ts: this sensors timestamp. + * @ts_adj: sensor timestamp adjustment. + * @previous_ts: previous timestamp for this sensor. + * @dur: duration between samples in ns. + * @rate: sensor data rate. + * @sample_size: number of bytes for the sensor. + * @odr_addr: output data rate address in DMP. + * @counter_addr: output counter address in DMP. + * @output: output on/off control word. + * @time_calib: calibrate timestamp. + * @sample_calib: calibrate bytes accumulated. + * @div: divider in DMP mode. + * @calib_flag: calibrate flag used to improve the accuracy of estimation. + * @on: sensor on/off. + * @a_en: accel engine requirement. + * @g_en: gyro engine requirement. + * @c_en: compass_engine requirement. + * @p_en: pressure engine requirement. + * @engine_base: engine base for this sensor. + * @count: number of samples in one session. + * @send: decide whether to send this sample or not. + */ +struct inv_sensor { + u64 ts; + s64 ts_adj; + u64 previous_ts; + int dur; + int rate; + u8 sample_size; + int odr_addr; + int counter_addr; + u16 output; + u64 time_calib; + u32 sample_calib; + int div; + bool calib_flag; + bool on; + bool a_en; + bool g_en; + bool c_en; + bool p_en; + enum INV_ENGINE engine_base; + int count; + bool send; +}; + +/** + * struct inv_sensor - information for each sensor. + * @sample_size: number of bytes for the sensor. + * @output: output on/off control word. + * @on: sensor on/off. + * @header: accuracy header for communicate with HAL + *dd @count: number of samples in one session. + */ +struct inv_sensor_accuracy { + u16 output; + u8 sample_size; + bool on; + u16 header; +}; + +enum SENSOR_ACCURACY { + SENSOR_ACCEL_ACCURACY = 0, + SENSOR_GYRO_ACCURACY, + SENSOR_COMPASS_ACCURACY, + SENSOR_ACCURACY_NUM_MAX, +}; + +enum SENSOR_L { + SENSOR_L_ACCEL = 0, + SENSOR_L_GYRO, + SENSOR_L_MAG, + SENSOR_L_ALS, + SENSOR_L_SIXQ, + SENSOR_L_THREEQ, + SENSOR_L_NINEQ, + SENSOR_L_PEDQ, + SENSOR_L_GEOMAG, + SENSOR_L_PRESSURE, + SENSOR_L_GYRO_CAL, + SENSOR_L_MAG_CAL, + SENSOR_L_EIS_GYRO, + /*wake sensors */ + SENSOR_L_ACCEL_WAKE = 13, + SENSOR_L_GYRO_WAKE, + SENSOR_L_MAG_WAKE, + SENSOR_L_ALS_WAKE, + SENSOR_L_SIXQ_WAKE, + SENSOR_L_NINEQ_WAKE, + SENSOR_L_PEDQ_WAKE, + SENSOR_L_GEOMAG_WAKE, + SENSOR_L_PRESSURE_WAKE, + SENSOR_L_GYRO_CAL_WAKE, + SENSOR_L_MAG_CAL_WAKE, + SENSOR_L_GESTURE_ACCEL, + SENSOR_L_NUM_MAX, +}; + +/** + * struct android_l_sensor - information for each android sensor. + * @ts: this sensors timestamp. + * @base: android sensor based on invensense sensor. + * @rate: output rate. + * @on: sensor on/off. + * @wake_on: wake on sensor is on/off. + * @div: divider for the output. + * @counter: counter works with the divider. + * @header: header for the output. + */ +struct android_l_sensor { + u64 ts; + enum INV_SENSORS base; + int rate; + bool on; + bool wake_on; + int div; + int counter; + u16 header; +}; + +/** + * struct inv_batch - information for batchmode. + * @on: normal batch mode on. + * @default_on: default batch on. This is optimization option. + * @overflow_on: overflow mode for batchmode. + * @wake_fifo_on: overflow for suspend mode. + * @step_only: mean only step detector data is batched. + * @post_isr_run: mean post isr has runned once. + * @counter: counter for batch mode. + * @timeout: nominal timeout value for batchmode in milliseconds. + * @max_rate: max rate for all batched sensors. + * @pk_size: packet size; + * @engine_base: engine base batch mode should stick to. + */ +struct inv_batch { + bool on; + bool default_on; + bool overflow_on; + bool wake_fifo_on; + bool step_only; + bool post_isr_run; + u32 counter; + u32 timeout; + u32 max_rate; + u32 pk_size; + u32 fifo_wm_th; + enum INV_ENGINE engine_base; +}; + +/** + * struct inv_chip_config_s - Cached chip configuration data. + * @fsr: Full scale range. + * @lpf: Digital low pass filter frequency. + * @accel_fs: accel full scale range. + * @accel_enable: enable accel functionality + * @gyro_enable: enable gyro functionality + * @compass_enable: enable compass functinality. + * @geomag_enable: enable geomag sensor functions. + * @als_enable: enable ALS functionality. + * @pressure_enable: eanble pressure functionality. + * @secondary_enable: secondary I2C bus enabled or not. + * @has_gyro: has gyro or not. + * @has_compass: has secondary I2C compass or not. + * @has_pressure: has secondary I2C pressure or not. + * @has_als: has secondary I2C als or not. + * @slave_enable: secondary I2C interface enabled or not. + * @normal_compass_measure: discard first compass data after reset. + * @is_asleep: 1 if chip is powered down. + * @lp_en_set: 1 if LP_EN bit is set; + * @lp_en_mode_off: debug mode that turns off LP_EN mode off. + * @clk_sel: debug_mode that turns on/off clock selection. + * @dmp_on: dmp is on/off. + * @dmp_event_int_on: dmp event interrupt on/off. + * @wom_on: WOM interrupt on. This is an internal variable. + * @step_indicator_on: step indicate bit added to the sensor or not. + * @tilt_enable: tilt enable. + * @pick_up_enable: pick up gesture enable. + * @step_detector_on: step detector on or not. + * @activity_on: turn on/off activity. + * @activity_eng_on: activity engine on/off. + * @firmware_loaded: flag indicate firmware loaded or not. + * @low_power_gyro_on: flag indicating low power gyro on/off. + * @wake_on: any wake on sensor is on/off. + * @compass_rate: compass engine rate. Determined by underlying data. + */ +struct inv_chip_config_s { + u32 fsr:2; + u32 lpf:3; + u32 accel_fs:2; + u32 accel_enable:1; + u32 gyro_enable:1; + u32 compass_enable:1; + u32 geomag_enable:1; + u32 als_enable:1; + u32 prox_enable:1; + u32 pressure_enable:1; + u32 has_gyro:1; + u32 has_compass:1; + u32 has_pressure:1; + u32 has_als:1; + u32 slave_enable:1; + u32 normal_compass_measure:1; + u32 is_asleep:1; + u32 lp_en_set:1; + u32 lp_en_mode_off:1; + u32 clk_sel:1; + u32 dmp_on:1; + u32 dmp_event_int_on:1; + u32 wom_on:1; + u32 step_indicator_on:1; + u32 tilt_enable:1; + u32 pick_up_enable:1; + u32 eis_enable:1; + u32 step_detector_on:1; + u32 activity_on:1; + u32 activity_eng_on:1; + u32 firmware_loaded:1; + u32 low_power_gyro_on:1; + u32 wake_on:1; + int compass_rate; +}; + +/** + * struct inv_temp_comp - temperature compensation structure. + * @t_lo: raw temperature in low temperature. + * @t_hi: raw temperature in high temperature. + * @b_lo: gyro bias in low temperature. + * @b_hi: gyro bias in high temperature. + * @has_low: flag indicate low temperature parameters is updated. + * @has_high: flag indicates high temperature parameters is updated. + * @slope: slope for temperature compensation. + */ +struct inv_temp_comp { + int t_lo; + int t_hi; + int b_lo[3]; + int b_hi[3]; + bool has_low; + bool has_high; + int slope[3]; +}; + +/** + * struct inv_chip_info_s - Chip related information. + * @product_id: Product id. + * @product_revision: Product revision. + * @silicon_revision: Silicon revision. + * @software_revision: software revision. + * @compass_sens: compass sensitivity. + * @gyro_sens_trim: Gyro sensitivity trim factor. + * @accel_sens_trim: accel sensitivity trim factor. + */ +struct inv_chip_info_s { + u8 product_id; + u8 product_revision; + u8 silicon_revision; + u8 software_revision; + u8 compass_sens[3]; + u32 gyro_sens_trim; + u32 accel_sens_trim; +}; + +/** + * struct inv_smd significant motion detection structure. + * @threshold: accel threshold for motion detection. + * @delay: delay time to confirm 2nd motion. + * @delay2: delay window parameter. + * @on: smd on/off. + */ +struct inv_smd { + u32 threshold; + u32 delay; + u32 delay2; + bool on; +}; + +/** + * struct inv_ped pedometer related data structure. + * @step: steps taken. + * @time: time taken during the period. + * @last_step_time: last time the step is taken. + * @step_thresh: step threshold to show steps. + * @int_thresh: step threshold to generate interrupt. + * @int_on: pedometer interrupt enable/disable. + * @on: pedometer on/off. + * @engine_on: pedometer engine on/off. + */ +struct inv_ped { + u64 step; + u64 time; + u64 last_step_time; + u16 step_thresh; + u16 int_thresh; + bool int_on; + bool on; + bool engine_on; +}; + +/** + * struct inv_eis EIS related data structure. + * @prev_gyro: latest gyro data just before FSYNC triggerd + * @prev_timestamp: latest gyro timestamp just before FSYNC triggered + * @current_gyro: gyro data just after FSYNC triggerd + * @current_timestamp: gyro timestamp just after FSYNC triggered + * @fsync_timestamp: timestamp of FSYNC event + * @fsync_delay: delay time of FSYNC and Gyro data. DMP data of FSYNC event + * @eis_triggered: check fsync event is triggered or not. + * @eis_frame: current frame is eis frame; + * @current_sync: current frame contains fsync counter. + * @frame_count: frame count for synchronization. + */ +struct inv_eis { + int prev_gyro[3]; + u64 prev_timestamp; + int current_gyro[3]; + u64 current_timestamp; + u32 frame_dur; + u64 slope[3]; + u64 fsync_timestamp; + u64 last_fsync_timestamp; + u16 fsync_delay; + bool eis_triggered; + bool eis_frame; + bool current_sync; + bool prev_state; + u32 frame_count; + int gyro_counter; + int gyro_counter_s[3]; + int fsync_delay_s[3]; + int voting_count; + int voting_count_sub; + int voting_state; + int count_precision; +}; + +enum TRIGGER_STATE { + DATA_TRIGGER = 0, + RATE_TRIGGER, + EVENT_TRIGGER, + MISC_TRIGGER, + DEBUG_TRIGGER, +}; + +enum inv_fifo_count_mode { + BYTE_MODE, + RECORD_MODE +}; + +/** + * struct inv_secondary_reg - secondary registers data structure. + * @addr: address of the slave. + * @reg: register address of slave. + * @ctrl: control register. + * @d0: data out register. + */ +struct inv_secondary_reg { + u8 addr; + u8 reg; + u8 ctrl; + u8 d0; +}; + +struct inv_secondary_set { + u8 delay_enable; + u8 delay_time; + u8 odr_config; +}; +/** + * struct inv_engine_info - data structure for engines. + * @base_time: base time for each engine. + * @base_time_1k: base time when chip is running at 1K; + * @divider: divider used to downsample engine rate from original rate. + * @running_rate: the actually running rate of engine. + * @orig_rate: original rate for each engine before downsample. + * @dur: duration for one tick. + * @last_update_time: last update time. + */ +struct inv_engine_info { + u32 base_time; + u32 base_time_1k; + u32 divider; + u32 running_rate; + u32 orig_rate; + u32 dur; + u64 last_update_time; +}; + +struct inv_ois { + int gyro_fs; + int accel_fs; + bool en; +}; + +/** + * struct inv_timestamp_algo - timestamp algorithm . + * @last_run_time: last time the post ISR runs. + * @ts_for_calib: ts storage for calibration. + * @reset_ts: reset time. + * @dmp_ticks: dmp ticks storage for calibration. + * @start_dmp_counter: dmp counter when start a new session. + * @calib_counter: calibration counter for timestamp. + * @resume_flag: flag to indicate this is the first time after resume. time + could have up to 1 seconds difference. + * @clock_base: clock base to calculate the timestamp. + * @gyro_ts_shift: 9 K counter for EIS. + * @first_sample: first of 1K running should be dropped it affects timing + */ +struct inv_timestamp_algo { + u64 last_run_time; + u64 ts_for_calib; + u64 reset_ts; + u32 dmp_ticks; + u32 start_dmp_counter; + int calib_counter; + bool resume_flag; + enum INV_ENGINE clock_base; + u32 gyro_ts_shift; + u32 first_sample; +}; + +struct inv_mpu_slave; +/** + * struct inv_mpu_state - Driver state variables. + * @dev: device address of the current bus, i2c or spi. + * @chip_config: Cached attribute information. + * @chip_info: Chip information from read-only registers. + * @smd: SMD data structure. + * @ped: pedometer data structure. + * @batch: batchmode data structure. + * @temp_comp: gyro temperature compensation structure. + * @slave_compass: slave compass. + * @slave_pressure: slave pressure. + * @slave_als: slave als. + * @slv_reg: slave register data structure. + * @ts_algo: timestamp algorithm data structure. + * @sec_set: slave register odr config. + * @eng_info: information for each engine. + * @hw: Other hardware-specific information. + * @chip_type: chip type. + * @suspend_resume_sema: semaphore for suspend/resume. + * @wake_lock: wake lock of the system. + * @client: i2c client handle. + * @plat_data: platform data. + * @sl_handle: Handle to I2C port. + * @sensor{SENSOR_NUM_MAX]: sensor individual properties. + * @sensor_l[SENSOR_L_NUM_MAX]: android L sensors properties. + * @sensor_accuracy[SENSOR_ACCURACY_NUM_MAX]: sensor accuracy. + * @sensor_acurracy_flag: flag indiciate whether to check output accuracy. + * @irq: irq number store. + * @accel_bias: accel bias store. + * @gyro_bias: gyro bias store. + * @accel_st_bias: accel bias store, result of self-test. + * @gyro_st_bias: gyro bias store, result of self-test. + * @gyro_ois_st_bias: gyro bias store from ois self test result. + * @input_accel_dmp_bias[3]: accel bias for dmp. + * @input_gyro_dmp_bias[3]: gyro bias for dmp. + * @input_compass_dmp_bias[3]: compass bias for dmp. + * @input_accel_bias[3]: accel bias for offset register. + * @input_gyro_bias[3]: gyro bias for offset register. + * @fifo_data[8]: fifo data storage. + * @i2c_addr: i2c address. + * @header_count: header count in current FIFO. + * @step_det_count: number of step detectors in one batch. + * @gyro_sf: gyro scale factor. + * @left_over[LEFT_OVER_BYTES]: left over bytes storage. + * @left_over_size: left over size. + * @fifo_count: current fifo_count; + * @wake_sensor_received: wake up sensor received. + * @accel_cal_enable: accel calibration on/off + * @gyro_cal_enable: gyro calibration on/off + * @calib_compass_on: calibrate compass on. + * @debug_determine_engine_on: determine engine on/off. + * @poke_mode_on: poke mode on/off. + * @mode_1k_on: indicate 1K Hz mode is on. + * @poke_ts: time stamp for poke feature. + * @step_detector_base_ts: base time stamp for step detector calculation. + * @last_temp_comp_time: last time temperature compensation is done. + * @i2c_dis: disable I2C interface or not. + * @name: name for the chip. + * @gyro_st_data: gyro self test data. + * @accel_st_data: accel self test data. + * @secondary_name: name for the slave device in the secondary I2C. + * @compass_var: compass variance from DMP. + * @current_compass_matrix: matrix compass data multiplied to before soft iron. + * @final_compass_matrix: matrix compass data multiplied to before soft iron. + * @trigger_state: information that which part triggers set_inv_enable. + * @firmware: firmware data pointer. + * @accel_calib_threshold: accel calibration threshold; + * @accel_calib_rate: divider for accel calibration rate. + * @accel_covariance[COVARIANCE_SIZE]: accel covariance data; + * @kf: kfifo for activity store. + * @activity_size: size for activity. + * @cntl: control word for sensor enable. + * @cntl2: control word for sensor extension. + * @motion_event_cntl: control word for events. + * @dmp_image_size: dmp image size. + * @dmp_start_address: start address of dmp. + * @step_counter_l_on: step counter android L sensor on/off. + * @step_counter_wake_l_on: step counter android L sensor wake on/off . + * @step_detector_l_on: step detector android L sensor on/off. + * @step_detector_wake_l_on: step detector android L sensor wake on/off . + * @gesture_only_on: indicate it is gesture only. + * @mag_divider: mag divider when gyro/accel is faster than mag maximum rate. + * @special_mag_mode: for 20690, there is special mag mode need to be handled. + * @mag_start_flag: when mag divider is non zero, need to check the start. + * @prev_steps: previous steps sent to the user. + * @aut_key_in: authentication key input. + * @aut_key_out: authentication key output. + * @suspend_state: state variable to indicate that we are in suspend state. + * @secondary_gyro_on: DMP out signal to turn on gyro. + * @secondary_mag_on: DMP out signal to turn on mag. + * @secondary_prox_on: DMP out signal to turn on proximity. + * @secondary_switch: showing this setup is triggerred by secondary switch. + * @send_calib_gyro: flag to indicate to send calibrated gyro. + * @send_raw_compass: flag to send raw compass. + * @resume_state: flag to synchronize the processing of inv_read_fifo() + * @cycle_on: variable indicate accel cycle mode is on. + * @secondary_switch_data: secondary switch data for activity. + * @raw_gyro_data[6]: save raw gyro data. + * @raw_compass_data[3]: save raw compass data. + * @wait_queue: wait queue to wake up inv_read_fifo() + * @bac_drive_conf: bac drive configuration. + * @bac_walk_conf: bac walk configuration. + * @bac_smd_conf: bac smd configuration. + * @bac_bike_conf: bac bike configuration. + * @bac_run_conf: bac run configuration. + * @bac_still_conf: back still configuration. + * @power_on_data: power on data. + * @fifo_data_store: store of FIFO data. + * @int_en: store interrupt enable register data. + * @int_en2: store interrupt enable register 2 data. + * @gesture_int_count: interrupt count for gesture only mode. + * @smplrt_div: SMPLRT_DIV register value. + */ +struct inv_mpu_state { + struct device *dev; + int (*write)(struct inv_mpu_state *st, u8 reg, u8 data); + int (*read)(struct inv_mpu_state *st, u8 reg, int len, u8 *data); + int (*mem_write)(struct inv_mpu_state *st, u8 mpu_addr, u16 mem_addr, + u32 len, u8 const *data); + int (*mem_read)(struct inv_mpu_state *st, u8 mpu_addr, u16 mem_addr, + u32 len, u8 *data); + struct inv_chip_config_s chip_config; + struct inv_chip_info_s chip_info; + struct inv_smd smd; + struct inv_ped ped; + struct inv_eis eis; + struct inv_batch batch; + struct inv_temp_comp temp_comp; + struct inv_mpu_slave *slave_compass; + struct inv_mpu_slave *slave_pressure; + struct inv_mpu_slave *slave_als; + struct inv_secondary_reg slv_reg[4]; + struct inv_timestamp_algo ts_algo; + struct inv_secondary_set sec_set; + struct inv_engine_info eng_info[ENGINE_NUM_MAX]; + const struct inv_hw_s *hw; + enum inv_devices chip_type; + enum inv_bus_type bus_type; + enum inv_fifo_count_mode fifo_count_mode; +#ifdef CONFIG_HAS_WAKELOCK + struct wake_lock wake_lock; +#else + struct wakeup_source wake_lock; +#endif +#ifdef TIMER_BASED_BATCHING + struct hrtimer hr_batch_timer; + u64 batch_timeout; + bool is_batch_timer_running; + struct work_struct batch_work; +#endif + struct i2c_client *client; + struct mpu_platform_data plat_data; + void *sl_handle; + struct inv_sensor sensor[SENSOR_NUM_MAX]; + struct android_l_sensor sensor_l[SENSOR_L_NUM_MAX]; + struct inv_sensor_accuracy sensor_accuracy[SENSOR_ACCURACY_NUM_MAX]; + struct inv_ois ois; + bool sensor_acurracy_flag[SENSOR_ACCURACY_NUM_MAX]; + short irq; + int accel_bias[3]; + int gyro_bias[3]; + int accel_st_bias[3]; + int accel_ois_st_bias[3]; + int gyro_st_bias[3]; + int gyro_ois_st_bias[3]; + int input_accel_dmp_bias[3]; + int input_gyro_dmp_bias[3]; + int input_compass_dmp_bias[3]; + int input_accel_bias[3]; + int input_gyro_bias[3]; + u8 fifo_data[8]; + u8 i2c_addr; + int header_count; + int step_det_count; + s32 gyro_sf; + u8 left_over[LEFT_OVER_BYTES]; + u32 left_over_size; + u32 fifo_count; + bool wake_sensor_received; + bool accel_cal_enable; + bool gyro_cal_enable; + bool calib_compass_on; + bool debug_determine_engine_on; + bool poke_mode_on; + bool mode_1k_on; + u64 poke_ts; + u64 step_detector_base_ts; + u64 last_temp_comp_time; + u8 i2c_dis; + u8 name[20]; + u8 gyro_st_data[3]; + u8 accel_st_data[3]; + u8 secondary_name[20]; + s32 compass_var; + int current_compass_matrix[9]; + int final_compass_matrix[9]; + enum TRIGGER_STATE trigger_state; + u8 *firmware; + int accel_calib_threshold; + int accel_calib_rate; + u32 accel_covariance[COVARIANCE_SIZE]; + DECLARE_KFIFO(kf, u8, 128); + u32 activity_size; + int wom_thld; + u16 cntl; + u16 cntl2; + u16 motion_event_cntl; + int dmp_image_size; + int dmp_start_address; + bool step_counter_l_on; + bool step_counter_wake_l_on; + bool step_detector_l_on; + bool step_detector_wake_l_on; + bool gesture_only_on; + bool mag_start_flag; + int mag_divider; + bool special_mag_mode; + int prev_steps; + u32 curr_steps; + int aut_key_in; + int aut_key_out; + bool secondary_gyro_on; + bool secondary_mag_on; + bool secondary_prox_on; + bool secondary_switch; + bool send_calib_gyro; + bool send_raw_compass; + bool send_raw_gyro; + bool resume_state; + bool cycle_on; + int secondary_switch_data; + u8 raw_gyro_data[6]; + u32 raw_compass_data[3]; + wait_queue_head_t wait_queue; + u32 bac_drive_conf; + u32 bac_walk_conf; + u32 bac_smd_conf; + u32 bac_bike_conf; + u32 bac_run_conf; + u32 bac_still_conf; + u32 power_on_data; + u8 fifo_data_store[HARDWARE_FIFO_SIZE + LEFT_OVER_BYTES]; + u8 int_en; + u8 int_en_2; + u8 gesture_int_count; + u8 smplrt_div; +}; + +/** + * struct inv_mpu_slave - MPU slave structure. + * @st_upper: compass self test upper limit. + * @st_lower: compass self test lower limit. + * @scale: compass scale. + * @rate_scale: decide how fast a compass can read. + * @min_read_time: minimum time between each reading. + * @self_test: self test method of the slave. + * @set_scale: set scale of slave + * @get_scale: read scale back of the slave. + * @suspend: suspend operation. + * @resume: resume operation. + * @setup: setup chip. initialization. + * @combine_data: combine raw data into meaningful data. + * @read_data: read external sensor and output + * @get_mode: get current chip mode. + * @set_lpf: set low pass filter. + * @set_fs: set full scale + * @prev_ts: last time it is read. + */ +struct inv_mpu_slave { + const short *st_upper; + const short *st_lower; + int scale; + int rate_scale; + int min_read_time; + int (*self_test) (struct inv_mpu_state *); + int (*set_scale) (struct inv_mpu_state *, int scale); + int (*get_scale) (struct inv_mpu_state *, int *val); + int (*suspend) (struct inv_mpu_state *); + int (*resume) (struct inv_mpu_state *); + int (*setup) (struct inv_mpu_state *); + int (*combine_data) (u8 *in, short *out); + int (*read_data) (struct inv_mpu_state *, short *out); + int (*get_mode) (void); + int (*set_lpf) (struct inv_mpu_state *, int rate); + int (*set_fs) (struct inv_mpu_state *, int fs); + u64 prev_ts; +}; + +/* scan element definition */ +enum inv_mpu_scan { + INV_MPU_SCAN_TIMESTAMP, +}; + +/* IIO attribute address */ +enum MPU_IIO_ATTR_ADDR { + ATTR_DMP_GYRO_X_DMP_BIAS, + ATTR_DMP_GYRO_Y_DMP_BIAS, + ATTR_DMP_GYRO_Z_DMP_BIAS, + ATTR_DMP_GYRO_CAL_ENABLE, + ATTR_DMP_ACCEL_X_DMP_BIAS, + ATTR_DMP_ACCEL_Y_DMP_BIAS, + ATTR_DMP_ACCEL_Z_DMP_BIAS, + ATTR_DMP_MAGN_X_DMP_BIAS, + ATTR_DMP_MAGN_Y_DMP_BIAS, + ATTR_DMP_MAGN_Z_DMP_BIAS, + ATTR_DMP_MAGN_ACCURACY, + ATTR_GYRO_X_OFFSET, + ATTR_GYRO_Y_OFFSET, + ATTR_GYRO_Z_OFFSET, + ATTR_ACCEL_X_OFFSET, + ATTR_ACCEL_Y_OFFSET, + ATTR_ACCEL_Z_OFFSET, + ATTR_DMP_SC_AUTH, + ATTR_DMP_EIS_AUTH, + ATTR_DMP_ACCEL_CAL_ENABLE, + ATTR_DMP_PED_INT_ON, + ATTR_DMP_PED_STEP_THRESH, + ATTR_DMP_PED_INT_THRESH, + ATTR_DMP_PED_ON, + ATTR_DMP_SMD_ENABLE, + ATTR_DMP_TILT_ENABLE, + ATTR_DMP_PICK_UP_ENABLE, + ATTR_DMP_EIS_ENABLE, + ATTR_DMP_PEDOMETER_STEPS, + ATTR_DMP_PEDOMETER_TIME, + ATTR_DMP_PEDOMETER_COUNTER, + ATTR_DMP_LOW_POWER_GYRO_ON, + ATTR_DMP_LP_EN_OFF, + ATTR_DMP_CLK_SEL, + ATTR_DMP_DEBUG_MEM_READ, + ATTR_DMP_DEBUG_MEM_WRITE, + ATTR_DEBUG_REG_WRITE, + ATTR_DEBUG_WRITE_CFG, + ATTR_DEBUG_REG_ADDR, + ATTR_WOM_THLD, + /* *****above this line, are DMP features, power needs on/off */ + /* *****below this line, are DMP features, no power needed */ + ATTR_IN_POWER_ON, + ATTR_DMP_ON, + ATTR_DMP_EVENT_INT_ON, + ATTR_DMP_STEP_COUNTER_ON, + ATTR_DMP_STEP_COUNTER_WAKE_ON, + ATTR_DMP_BATCHMODE_TIMEOUT, + ATTR_DMP_BATCHMODE_WAKE_FIFO_FULL, + ATTR_DMP_STEP_DETECTOR_ON, + ATTR_DMP_STEP_DETECTOR_WAKE_ON, + ATTR_DMP_ACTIVITY_ON, + ATTR_DMP_IN_ANGLVEL_ACCURACY_ENABLE, + ATTR_DMP_IN_ACCEL_ACCURACY_ENABLE, + ATTR_DMP_DEBUG_DETERMINE_ENGINE_ON, + ATTR_DMP_MISC_GYRO_RECALIBRATION, + ATTR_DMP_MISC_ACCEL_RECALIBRATION, + ATTR_DMP_PARAMS_ACCEL_CALIBRATION_THRESHOLD, + ATTR_DMP_PARAMS_ACCEL_CALIBRATION_RATE, + ATTR_GYRO_SCALE, + ATTR_ACCEL_SCALE, + ATTR_COMPASS_SCALE, + ATTR_COMPASS_SENSITIVITY_X, + ATTR_COMPASS_SENSITIVITY_Y, + ATTR_COMPASS_SENSITIVITY_Z, + ATTR_GYRO_ENABLE, + ATTR_ACCEL_ENABLE, + ATTR_COMPASS_ENABLE, + ATTR_FIRMWARE_LOADED, + ATTR_POKE_MODE, + ATTR_ANGLVEL_X_CALIBBIAS, + ATTR_ANGLVEL_Y_CALIBBIAS, + ATTR_ANGLVEL_Z_CALIBBIAS, + ATTR_ACCEL_X_CALIBBIAS, + ATTR_ACCEL_Y_CALIBBIAS, + ATTR_ACCEL_Z_CALIBBIAS, + ATTR_ANGLVEL_X_ST_CALIBBIAS, + ATTR_ANGLVEL_Y_ST_CALIBBIAS, + ATTR_ANGLVEL_Z_ST_CALIBBIAS, + ATTR_ANGLVEL_X_OIS_ST_CALIBBIAS, + ATTR_ANGLVEL_Y_OIS_ST_CALIBBIAS, + ATTR_ANGLVEL_Z_OIS_ST_CALIBBIAS, + ATTR_ACCEL_X_ST_CALIBBIAS, + ATTR_ACCEL_Y_ST_CALIBBIAS, + ATTR_ACCEL_Z_ST_CALIBBIAS, + ATTR_ACCEL_X_OIS_ST_CALIBBIAS, + ATTR_ACCEL_Y_OIS_ST_CALIBBIAS, + ATTR_ACCEL_Z_OIS_ST_CALIBBIAS, + ATTR_GYRO_MATRIX, + ATTR_ACCEL_MATRIX, + ATTR_COMPASS_MATRIX, + ATTR_FSYNC_FRAME_COUNT, + ATTR_SECONDARY_NAME, + ATTR_GYRO_SF, + ATTR_BAC_DRIVE_CONFIDENCE, + ATTR_BAC_WALK_CONFIDENCE, + ATTR_BAC_SMD_CONFIDENCE, + ATTR_BAC_BIKE_CONFIDENCE, + ATTR_BAC_STILL_CONFIDENCE, + ATTR_BAC_RUN_CONFIDENCE, + IN_OIS_ACCEL_FS, + IN_OIS_GYRO_FS, + IN_OIS_ENABLE, +}; + +int inv_mpu_configure_ring(struct iio_dev *indio_dev); +int inv_mpu_probe_trigger(struct iio_dev *indio_dev); +void inv_mpu_unconfigure_ring(struct iio_dev *indio_dev); +void inv_mpu_remove_trigger(struct iio_dev *indio_dev); +#ifdef CONFIG_PM_SLEEP +int inv_mpu_suspend(struct iio_dev *indio_dev); +void inv_mpu_complete(struct iio_dev *indio_dev); +#endif + +int inv_get_pedometer_steps(struct inv_mpu_state *st, int *ped); +int inv_get_pedometer_time(struct inv_mpu_state *st, int *ped); +int inv_read_pedometer_counter(struct inv_mpu_state *st); + +int inv_dmp_read(struct inv_mpu_state *st, int off, int size, u8 *buf); +int inv_firmware_load(struct inv_mpu_state *st); + +int set_inv_enable(struct iio_dev *indio_dev); + +int inv_mpu_setup_compass_slave(struct inv_mpu_state *st); +int inv_mpu_setup_pressure_slave(struct inv_mpu_state *st); +int inv_mpu_setup_als_slave(struct inv_mpu_state *st); +int inv_mpu_initialize(struct inv_mpu_state *st); +int inv_set_accel_sf(struct inv_mpu_state *st); +int inv_set_gyro_sf(struct inv_mpu_state *st); +s64 get_time_ns(void); +int inv_i2c_read_base(struct inv_mpu_state *st, u16 i, u8 r, u16 l, u8 *d); +int inv_i2c_single_write_base(struct inv_mpu_state *st, u16 i, u8 r, u8 d); +int write_be32_to_mem(struct inv_mpu_state *st, u32 data, int addr); +int write_be16_to_mem(struct inv_mpu_state *st, u16 data, int addr); +int read_be32_from_mem(struct inv_mpu_state *st, u32 *o, int addr); +int read_be16_from_mem(struct inv_mpu_state *st, u16 *o, int addr); +u32 inv_get_cntr_diff(u32 curr_counter, u32 prev); +int inv_write_2bytes(struct inv_mpu_state *st, int k, int data); +int inv_set_bank(struct inv_mpu_state *st, u8 bank); +int inv_set_power(struct inv_mpu_state *st, bool power_on); +int inv_switch_power_in_lp(struct inv_mpu_state *st, bool on); +#ifndef CONFIG_INV_MPU_IIO_ICM20608D +int inv_set_accel_config2(struct inv_mpu_state *st, bool cycle_mode); +#endif +int inv_stop_dmp(struct inv_mpu_state *st); +int inv_reset_fifo(struct inv_mpu_state *st, bool turn_off); +int inv_create_dmp_sysfs(struct iio_dev *ind); +int inv_check_chip_type(struct iio_dev *indio_dev, const char *name); +int inv_write_compass_matrix(struct inv_mpu_state *st, int *adj); +irqreturn_t inv_read_fifo(int irq, void *dev_id); +#ifdef TIMER_BASED_BATCHING +void inv_batch_work(struct work_struct *work); +#endif +int inv_flush_batch_data(struct iio_dev *indio_dev, int data); +static inline int mpu_memory_write(struct inv_mpu_state *st, u8 mpu_addr, + u16 mem_addr, u32 len, u8 const *data) +{ + int ret = -1; + + if (st->mem_write) + ret = st->mem_write(st, mpu_addr, mem_addr, len, data); + + return ret; +} +static inline int mpu_memory_read(struct inv_mpu_state *st, u8 mpu_addr, + u16 mem_addr, u32 len, u8 *data) +{ + int ret = -1; + + if (st->mem_read) + ret = st->mem_read(st, mpu_addr, mem_addr, len, data); + + return ret; +} +int inv_read_secondary(struct inv_mpu_state *st, int ind, int addr, + int reg, int len); +int inv_write_secondary(struct inv_mpu_state *st, int ind, int addr, + int reg, int v); +int inv_execute_write_secondary(struct inv_mpu_state *st, int ind, int addr, + int reg, int v); +int inv_execute_read_secondary(struct inv_mpu_state *st, int ind, int addr, + int reg, int len, u8 *d); + +int inv_push_16bytes_buffer(struct inv_mpu_state *st, u16 hdr, + u64 t, int *q, s16 accur); +int inv_push_gyro_data(struct inv_mpu_state *st, s16 *raw, s32 *calib, u64 t); +int inv_push_8bytes_buffer(struct inv_mpu_state *st, u16 hdr, u64 t, s16 *d); +int inv_push_8bytes_kf(struct inv_mpu_state *st, u16 hdr, u64 t, s16 *d); + +void inv_push_step_indicator(struct inv_mpu_state *st, u64 t); +int inv_send_steps(struct inv_mpu_state *st, int step, u64 t); +int inv_push_marker_to_buffer(struct inv_mpu_state *st, u16 hdr, int data); + +int inv_check_sensor_on(struct inv_mpu_state *st); +int inv_write_cntl(struct inv_mpu_state *st, u16 wd, bool en, int cntl); + +int inv_get_packet_size(struct inv_mpu_state *st, u16 hdr, + u32 *pk_size, u8 *dptr); +int inv_parse_packet(struct inv_mpu_state *st, u16 hdr, u8 *dptr); +int inv_pre_parse_packet(struct inv_mpu_state *st, u16 hdr, u8 *dptr); +int inv_process_dmp_data(struct inv_mpu_state *st); + +int be32_to_int(u8 *d); +void inv_convert_and_push_16bytes(struct inv_mpu_state *st, u16 hdr, + u8 *d, u64 t, s8 *m); +void inv_convert_and_push_8bytes(struct inv_mpu_state *st, u16 hdr, + u8 *d, u64 t, s8 *m); +int inv_get_dmp_ts(struct inv_mpu_state *st, int i); +int inv_process_step_det(struct inv_mpu_state *st, u8 *dptr); +int inv_process_eis(struct inv_mpu_state *st, u16 delay); +int inv_rate_convert(struct inv_mpu_state *st, int ind, int data); + +int inv_setup_dmp_firmware(struct inv_mpu_state *st); +/* used to print i2c data using pr_debug */ +char *wr_pr_debug_begin(u8 const *data, u32 len, char *string); +char *wr_pr_debug_end(char *string); + +int inv_hw_self_test(struct inv_mpu_state *st); +int inv_q30_mult(int a, int b); +#ifdef ACCEL_BIAS_TEST +int inv_get_3axis_average(s16 src[], s16 dst[], s16 reset); +#endif + +static inline int inv_plat_single_write(struct inv_mpu_state *st, + u8 reg, u8 data) +{ + int ret = -1; + + if (st->write) + ret = st->write(st, reg, data); + + return ret; +} +static inline int inv_plat_read(struct inv_mpu_state *st, u8 reg, + int len, u8 *data) +{ + int ret = -1; + + if (st->read) + ret = st->read(st, reg, len, data); + + return ret; +} +irqreturn_t inv_read_fifo(int , void *); + +int inv_stop_interrupt(struct inv_mpu_state *st); +int inv_reenable_interrupt(struct inv_mpu_state *st); + +int inv_enable_pedometer_interrupt(struct inv_mpu_state *st, bool en); +int inv_dataout_control1(struct inv_mpu_state *st, u16 cntl1); +int inv_dataout_control2(struct inv_mpu_state *st, u16 cntl2); +int inv_motion_interrupt_control(struct inv_mpu_state *st, + u16 motion_event_cntl); + +int inv_bound_timestamp(struct inv_mpu_state *st); +int inv_update_dmp_ts(struct inv_mpu_state *st, int ind); +int inv_get_last_run_time_non_dmp_record_mode(struct inv_mpu_state *st); + +#define mem_w(a, b, c) mpu_memory_write(st, st->i2c_addr, a, b, c) +#define mem_r(a, b, c) mpu_memory_read(st, st->i2c_addr, a, b, c) + +#endif /* #ifndef _INV_MPU_IIO_H_ */ diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu/inv_mpu_ring.c new file mode 100644 index 000000000000..3e5bccbea0df --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_mpu_ring.c @@ -0,0 +1,643 @@ +/* +* 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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)) { + 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)); + sysfs_notify(&indio_dev->dev.kobj, NULL, "poll_tilt"); + } + + 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); +} + +/* + * inv_irq_handler() - Cache a timestamp at each data ready interrupt. + */ +static irqreturn_t inv_irq_handler(int irq, void *dev_id) +{ + return IRQ_WAKE_THREAD; +} + +#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 + +void inv_mpu_unconfigure_ring(struct iio_dev *indio_dev) +{ + struct inv_mpu_state *st = iio_priv(indio_dev); +#ifdef KERNEL_VERSION_4_X + devm_free_irq(st->dev, st->irq, st); + devm_iio_kfifo_free(st->dev, indio_dev->buffer); +#else + free_irq(st->irq, st); + iio_kfifo_free(indio_dev->buffer); +#endif +}; +EXPORT_SYMBOL_GPL(inv_mpu_unconfigure_ring); + +#ifndef KERNEL_VERSION_4_X +static int inv_predisable(struct iio_dev *indio_dev) +{ + return 0; +} + +static int inv_preenable(struct iio_dev *indio_dev) +{ + return 0; +} + +static const struct iio_buffer_setup_ops inv_mpu_ring_setup_ops = { + .preenable = &inv_preenable, + .predisable = &inv_predisable, +}; +#endif + +int inv_mpu_configure_ring(struct iio_dev *indio_dev) +{ + int ret; + struct inv_mpu_state *st = iio_priv(indio_dev); + struct iio_buffer *ring; + +#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 +#ifdef KERNEL_VERSION_4_X + ring = devm_iio_kfifo_allocate(st->dev); + if (!ring) + return -ENOMEM; + ring->scan_timestamp = true; + iio_device_attach_buffer(indio_dev, ring); + ret = devm_request_threaded_irq(st->dev, + st->irq, + inv_irq_handler, + inv_read_fifo, + IRQF_TRIGGER_RISING | IRQF_SHARED, + "inv_irq", + st); + if (ret) { + devm_iio_kfifo_free(st->dev, ring); + return ret; + } + + // this mode does not use ops + indio_dev->modes = INDIO_ALL_BUFFER_MODES; + + return ret; +#else + ring = iio_kfifo_allocate(indio_dev); + if (!ring) + return -ENOMEM; + indio_dev->buffer = ring; + /* setup ring buffer */ + ring->scan_timestamp = true; + indio_dev->setup_ops = &inv_mpu_ring_setup_ops; + ret = request_threaded_irq(st->irq, + inv_irq_handler, + inv_read_fifo, + IRQF_TRIGGER_RISING | IRQF_SHARED, + "inv_irq", + st); + if (ret) + goto error_iio_sw_rb_free; + + indio_dev->modes |= INDIO_BUFFER_HARDWARE; + + return 0; +error_iio_sw_rb_free: + iio_kfifo_free(indio_dev->buffer); + + return ret; +#endif +} +EXPORT_SYMBOL_GPL(inv_mpu_configure_ring); diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_spi.c b/drivers/iio/imu/inv_mpu/inv_mpu_spi.c new file mode 100644 index 000000000000..fb916788a9df --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_mpu_spi.c @@ -0,0 +1,410 @@ +/* +* 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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; + +} + +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; +} + +/* + * 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; + +#ifdef KERNEL_VERSION_4_X + indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st)); + if (indio_dev == NULL) { + pr_err("memory allocation failed\n"); + result = -ENOMEM; + goto out_no_free; + } +#else + indio_dev = iio_device_alloc(sizeof(*st)); + if (indio_dev == NULL) { + pr_err("memory allocation failed\n"); + result = -ENOMEM; + goto out_no_free; + } +#endif + st = iio_priv(indio_dev); + st->write = inv_spi_single_write; + st->read = inv_spi_read; + st->mem_write = inv_spi_mem_write; + st->mem_read = inv_spi_mem_read; + st->dev = &spi->dev; + st->irq = spi->irq; +#if !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) +# ifdef KERNEL_VERSION_4_X + return -ENODEV; +# else + goto out_free; +# 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); +# ifdef KERNEL_VERSION_4_X + return -ENODEV; +# else + goto out_free; +# endif + } + pr_info("%s: power on here.\n", __func__); + } + pr_info("%s: power on.\n", __func__); + + msleep(100); +#else + if (dev_get_platdata(st->dev) == NULL) +# ifdef KERNEL_VERSION_4_X + return -ENODEV; +# else + goto out_free; +# endif + st->plat_data = *(struct mpu_platform_data *)dev_get_platdata(st->dev); +#endif + + /* power is turned on inside check chip type */ + result = inv_check_chip_type(indio_dev, id->name); + if (result) +#ifdef KERNEL_VERSION_4_X + return -ENODEV; +#else + goto out_free; +#endif + + result = inv_mpu_configure_ring(indio_dev); + if (result) { + pr_err("configure ring buffer fail\n"); + goto out_free; + } +#ifdef KERNEL_VERSION_4_X + result = devm_iio_device_register(st->dev, indio_dev); + if (result) { + pr_err("IIO device register fail\n"); + goto out_unreg_ring; + } +#else + result = iio_buffer_register(indio_dev, indio_dev->channels, + indio_dev->num_channels); + if (result) { + pr_err("ring buffer register fail\n"); + goto out_unreg_ring; + } + + result = iio_device_register(indio_dev); + if (result) { + pr_err("IIO device register fail\n"); + goto out_remove_ring; + } +#endif + + 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; +#ifdef KERNEL_VERSION_4_X +out_unreg_iio: + devm_iio_device_unregister(st->dev, indio_dev); +out_unreg_ring: + inv_mpu_unconfigure_ring(indio_dev); +out_free: + devm_iio_device_free(st->dev, indio_dev); +out_no_free: +#else +out_unreg_iio: + iio_device_unregister(indio_dev); +out_remove_ring: + iio_buffer_unregister(indio_dev); +out_unreg_ring: + inv_mpu_unconfigure_ring(indio_dev); +out_free: + iio_device_free(indio_dev); +out_no_free: +#endif + dev_err(st->dev, "%s failed %d\n", __func__, result); + + return -EIO; +} + +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 */ + result = inv_plat_single_write(st, REG_PWR_MGMT_1, BIT_H_RESET); + 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); + +#ifdef KERNEL_VERSION_4_X + devm_iio_device_unregister(st->dev, indio_dev); +#else + iio_device_unregister(indio_dev); + iio_buffer_unregister(indio_dev); +#endif + inv_mpu_unconfigure_ring(indio_dev); +#ifdef KERNEL_VERSION_4_X + devm_iio_device_free(st->dev, indio_dev); +#else + iio_device_free(indio_dev); +#endif + 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}, +#endif + {} +}; + +MODULE_DEVICE_TABLE(spi, inv_mpu_id); + +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, + .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"); diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_timestamp.c b/drivers/iio/imu/inv_mpu/inv_mpu_timestamp.c new file mode 100644 index 000000000000..2cc721b18596 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_mpu_timestamp.c @@ -0,0 +1,280 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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% + 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; +#if defined(CONFIG_INV_MPU_IIO_ICM20602) || defined(CONFIG_INV_MPU_IIO_ICM20690) || defined(CONFIG_INV_MPU_IIO_IAM20680) + 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; +#endif + 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; +} diff --git a/drivers/iio/imu/inv_mpu/inv_test/Kconfig b/drivers/iio/imu/inv_mpu/inv_test/Kconfig new file mode 100644 index 000000000000..a4dfd95db886 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_test/Kconfig @@ -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. diff --git a/drivers/iio/imu/inv_mpu/inv_test/Makefile b/drivers/iio/imu/inv_mpu/inv_test/Makefile new file mode 100644 index 000000000000..4f0edd3de901 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_test/Makefile @@ -0,0 +1,6 @@ +# +# Makefile for Invensense IIO testing hooks. +# + +obj-$(CONFIG_INV_TESTING) += inv_counters.o + diff --git a/drivers/iio/imu/inv_mpu/inv_test/inv_counters.c b/drivers/iio/imu/inv_mpu/inv_test/inv_counters.c new file mode 100644 index 000000000000..f60337caeeed --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_test/inv_counters.c @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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"); + diff --git a/drivers/iio/imu/inv_mpu/inv_test/inv_counters.h b/drivers/iio/imu/inv_mpu/inv_test/inv_counters.h new file mode 100644 index 000000000000..62f76279e703 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_test/inv_counters.h @@ -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 +#include +#include +#include +#include +#include +#include + +#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_ */ + diff --git a/include/linux/iio/imu/mpu.h b/include/linux/iio/imu/mpu.h new file mode 100644 index 000000000000..4dbb86cad6d1 --- /dev/null +++ b/include/linux/iio/imu/mpu.h @@ -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 +#include +#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_ */