From 2c3c1048746a4622d8c89a29670120dc8fab93c4 Mon Sep 17 00:00:00 2001 From: Daniel Baumann Date: Sun, 7 Apr 2024 20:49:45 +0200 Subject: Adding upstream version 6.1.76. Signed-off-by: Daniel Baumann --- drivers/iio/imu/Kconfig | 113 + drivers/iio/imu/Makefile | 30 + drivers/iio/imu/adis.c | 545 +++++ drivers/iio/imu/adis16400.c | 1243 ++++++++++ drivers/iio/imu/adis16460.c | 431 ++++ drivers/iio/imu/adis16475.c | 1382 ++++++++++++ drivers/iio/imu/adis16480.c | 1536 +++++++++++++ drivers/iio/imu/adis_buffer.c | 216 ++ drivers/iio/imu/adis_trigger.c | 91 + drivers/iio/imu/bmi160/Kconfig | 33 + drivers/iio/imu/bmi160/Makefile | 7 + drivers/iio/imu/bmi160/bmi160.h | 31 + drivers/iio/imu/bmi160/bmi160_core.c | 891 ++++++++ drivers/iio/imu/bmi160/bmi160_i2c.c | 71 + drivers/iio/imu/bmi160/bmi160_spi.c | 68 + drivers/iio/imu/bno055/Kconfig | 27 + drivers/iio/imu/bno055/Makefile | 10 + drivers/iio/imu/bno055/bno055.c | 1685 ++++++++++++++ drivers/iio/imu/bno055/bno055.h | 13 + drivers/iio/imu/bno055/bno055_i2c.c | 57 + drivers/iio/imu/bno055/bno055_ser_core.c | 560 +++++ drivers/iio/imu/bno055/bno055_ser_trace.c | 14 + drivers/iio/imu/bno055/bno055_ser_trace.h | 104 + drivers/iio/imu/fxos8700.h | 10 + drivers/iio/imu/fxos8700_core.c | 715 ++++++ drivers/iio/imu/fxos8700_i2c.c | 70 + drivers/iio/imu/fxos8700_spi.c | 58 + drivers/iio/imu/inv_icm42600/Kconfig | 29 + drivers/iio/imu/inv_icm42600/Makefile | 15 + drivers/iio/imu/inv_icm42600/inv_icm42600.h | 396 ++++ drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c | 784 +++++++ drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c | 601 +++++ drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h | 98 + drivers/iio/imu/inv_icm42600/inv_icm42600_core.c | 786 +++++++ drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c | 796 +++++++ drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c | 104 + drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c | 100 + drivers/iio/imu/inv_icm42600/inv_icm42600_temp.c | 84 + drivers/iio/imu/inv_icm42600/inv_icm42600_temp.h | 30 + .../iio/imu/inv_icm42600/inv_icm42600_timestamp.c | 195 ++ .../iio/imu/inv_icm42600/inv_icm42600_timestamp.h | 85 + drivers/iio/imu/inv_mpu6050/Kconfig | 34 + drivers/iio/imu/inv_mpu6050/Makefile | 14 + drivers/iio/imu/inv_mpu6050/inv_mpu_acpi.c | 198 ++ drivers/iio/imu/inv_mpu6050/inv_mpu_aux.c | 204 ++ drivers/iio/imu/inv_mpu6050/inv_mpu_aux.h | 19 + drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 1778 +++++++++++++++ drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c | 278 +++ drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h | 474 ++++ drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c | 365 +++ drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h | 39 + drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 208 ++ drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c | 165 ++ drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 251 +++ drivers/iio/imu/kmx61.c | 1529 +++++++++++++ drivers/iio/imu/st_lsm6dsx/Kconfig | 34 + drivers/iio/imu/st_lsm6dsx/Makefile | 8 + drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h | 518 +++++ drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c | 756 +++++++ drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 2380 ++++++++++++++++++++ drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c | 154 ++ drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i3c.c | 57 + drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c | 919 ++++++++ drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c | 154 ++ drivers/iio/imu/st_lsm9ds0/Kconfig | 44 + drivers/iio/imu/st_lsm9ds0/Makefile | 5 + drivers/iio/imu/st_lsm9ds0/st_lsm9ds0.h | 22 + drivers/iio/imu/st_lsm9ds0/st_lsm9ds0_core.c | 150 ++ drivers/iio/imu/st_lsm9ds0/st_lsm9ds0_i2c.c | 80 + drivers/iio/imu/st_lsm9ds0/st_lsm9ds0_spi.c | 79 + 70 files changed, 25030 insertions(+) create mode 100644 drivers/iio/imu/Kconfig create mode 100644 drivers/iio/imu/Makefile create mode 100644 drivers/iio/imu/adis.c create mode 100644 drivers/iio/imu/adis16400.c create mode 100644 drivers/iio/imu/adis16460.c create mode 100644 drivers/iio/imu/adis16475.c create mode 100644 drivers/iio/imu/adis16480.c create mode 100644 drivers/iio/imu/adis_buffer.c create mode 100644 drivers/iio/imu/adis_trigger.c create mode 100644 drivers/iio/imu/bmi160/Kconfig create mode 100644 drivers/iio/imu/bmi160/Makefile create mode 100644 drivers/iio/imu/bmi160/bmi160.h create mode 100644 drivers/iio/imu/bmi160/bmi160_core.c create mode 100644 drivers/iio/imu/bmi160/bmi160_i2c.c create mode 100644 drivers/iio/imu/bmi160/bmi160_spi.c create mode 100644 drivers/iio/imu/bno055/Kconfig create mode 100644 drivers/iio/imu/bno055/Makefile create mode 100644 drivers/iio/imu/bno055/bno055.c create mode 100644 drivers/iio/imu/bno055/bno055.h create mode 100644 drivers/iio/imu/bno055/bno055_i2c.c create mode 100644 drivers/iio/imu/bno055/bno055_ser_core.c create mode 100644 drivers/iio/imu/bno055/bno055_ser_trace.c create mode 100644 drivers/iio/imu/bno055/bno055_ser_trace.h create mode 100644 drivers/iio/imu/fxos8700.h create mode 100644 drivers/iio/imu/fxos8700_core.c create mode 100644 drivers/iio/imu/fxos8700_i2c.c create mode 100644 drivers/iio/imu/fxos8700_spi.c create mode 100644 drivers/iio/imu/inv_icm42600/Kconfig create mode 100644 drivers/iio/imu/inv_icm42600/Makefile create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600.h create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_core.c create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_temp.c create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_temp.h create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h create mode 100644 drivers/iio/imu/inv_mpu6050/Kconfig create mode 100644 drivers/iio/imu/inv_mpu6050/Makefile create mode 100644 drivers/iio/imu/inv_mpu6050/inv_mpu_acpi.c create mode 100644 drivers/iio/imu/inv_mpu6050/inv_mpu_aux.c create mode 100644 drivers/iio/imu/inv_mpu6050/inv_mpu_aux.h create mode 100644 drivers/iio/imu/inv_mpu6050/inv_mpu_core.c create mode 100644 drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c create mode 100644 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h create mode 100644 drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c create mode 100644 drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h create mode 100644 drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c create mode 100644 drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c create mode 100644 drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c create mode 100644 drivers/iio/imu/kmx61.c create mode 100644 drivers/iio/imu/st_lsm6dsx/Kconfig create mode 100644 drivers/iio/imu/st_lsm6dsx/Makefile create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i3c.c create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c create mode 100644 drivers/iio/imu/st_lsm9ds0/Kconfig create mode 100644 drivers/iio/imu/st_lsm9ds0/Makefile create mode 100644 drivers/iio/imu/st_lsm9ds0/st_lsm9ds0.h create mode 100644 drivers/iio/imu/st_lsm9ds0/st_lsm9ds0_core.c create mode 100644 drivers/iio/imu/st_lsm9ds0/st_lsm9ds0_i2c.c create mode 100644 drivers/iio/imu/st_lsm9ds0/st_lsm9ds0_spi.c (limited to 'drivers/iio/imu') diff --git a/drivers/iio/imu/Kconfig b/drivers/iio/imu/Kconfig new file mode 100644 index 000000000..c2f97629e --- /dev/null +++ b/drivers/iio/imu/Kconfig @@ -0,0 +1,113 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# IIO imu drivers configuration +# +# When adding new entries keep the list in alphabetical order + +menu "Inertial measurement units" + +config ADIS16400 + tristate "Analog Devices ADIS16400 and similar IMU SPI driver" + depends on SPI + select IIO_ADIS_LIB + select IIO_ADIS_LIB_BUFFER if IIO_BUFFER + help + Say yes here to build support for Analog Devices adis16300, adis16344, + adis16350, adis16354, adis16355, adis16360, adis16362, adis16364, + adis16365, adis16400 and adis16405 triaxial inertial sensors + (adis16400 series also have magnetometers). + +config ADIS16460 + tristate "Analog Devices ADIS16460 and similar IMU driver" + depends on SPI + select IIO_ADIS_LIB + select IIO_ADIS_LIB_BUFFER if IIO_BUFFER + help + Say yes here to build support for Analog Devices ADIS16460 inertial + sensor. + + To compile this driver as a module, choose M here: the module will be + called adis16460. + +config ADIS16475 + tristate "Analog Devices ADIS16475 and similar IMU driver" + depends on SPI + select IIO_ADIS_LIB + select IIO_ADIS_LIB_BUFFER if IIO_BUFFER + help + Say yes here to build support for Analog Devices ADIS16470, ADIS16475, + ADIS16477, ADIS16465, ADIS16467, ADIS16500, ADIS16505, ADIS16507 inertial + sensors. + + To compile this driver as a module, choose M here: the module will be + called adis16475. + +config ADIS16480 + tristate "Analog Devices ADIS16480 and similar IMU driver" + depends on SPI + select IIO_ADIS_LIB + select IIO_ADIS_LIB_BUFFER if IIO_BUFFER + select CRC32 + help + Say yes here to build support for Analog Devices ADIS16375, ADIS16480, + ADIS16485, ADIS16488 inertial sensors. + +source "drivers/iio/imu/bmi160/Kconfig" +source "drivers/iio/imu/bno055/Kconfig" + +config FXOS8700 + tristate + +config FXOS8700_I2C + tristate "NXP FXOS8700 I2C driver" + depends on I2C + select FXOS8700 + select REGMAP_I2C + help + Say yes here to build support for the NXP FXOS8700 m+g combo + sensor on I2C. + + This driver can also be built as a module. If so, the module will be + called fxos8700_i2c. + +config FXOS8700_SPI + tristate "NXP FXOS8700 SPI driver" + depends on SPI + select FXOS8700 + select REGMAP_SPI + help + Say yes here to build support for the NXP FXOS8700 m+g combo + sensor on SPI. + + This driver can also be built as a module. If so, the module will be + called fxos8700_spi. + +config KMX61 + tristate "Kionix KMX61 6-axis accelerometer and magnetometer" + depends on I2C + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + help + Say Y here if you want to build a driver for Kionix KMX61 6-axis + accelerometer and magnetometer. + To compile this driver as module, choose M here: the module will + be called kmx61. + +source "drivers/iio/imu/inv_icm42600/Kconfig" +source "drivers/iio/imu/inv_mpu6050/Kconfig" +source "drivers/iio/imu/st_lsm6dsx/Kconfig" +source "drivers/iio/imu/st_lsm9ds0/Kconfig" + +endmenu + +config IIO_ADIS_LIB + tristate + help + A set of IO helper functions for the Analog Devices ADIS* device family. + +config IIO_ADIS_LIB_BUFFER + bool + select IIO_TRIGGERED_BUFFER + help + A set of buffer helper functions for the Analog Devices ADIS* device + family. diff --git a/drivers/iio/imu/Makefile b/drivers/iio/imu/Makefile new file mode 100644 index 000000000..6eb612034 --- /dev/null +++ b/drivers/iio/imu/Makefile @@ -0,0 +1,30 @@ +# SPDX-License-Identifier: GPL-2.0 +# +# Makefile for Inertial Measurement Units +# + +# When adding new entries keep the list in alphabetical order +obj-$(CONFIG_ADIS16400) += adis16400.o +obj-$(CONFIG_ADIS16460) += adis16460.o +obj-$(CONFIG_ADIS16475) += adis16475.o +obj-$(CONFIG_ADIS16480) += adis16480.o + +adis_lib-y += adis.o +adis_lib-$(CONFIG_IIO_ADIS_LIB_BUFFER) += adis_trigger.o +adis_lib-$(CONFIG_IIO_ADIS_LIB_BUFFER) += adis_buffer.o +obj-$(CONFIG_IIO_ADIS_LIB) += adis_lib.o + +obj-y += bmi160/ +obj-y += bno055/ + +obj-$(CONFIG_FXOS8700) += fxos8700_core.o +obj-$(CONFIG_FXOS8700_I2C) += fxos8700_i2c.o +obj-$(CONFIG_FXOS8700_SPI) += fxos8700_spi.o + +obj-y += inv_icm42600/ +obj-y += inv_mpu6050/ + +obj-$(CONFIG_KMX61) += kmx61.o + +obj-y += st_lsm6dsx/ +obj-y += st_lsm9ds0/ diff --git a/drivers/iio/imu/adis.c b/drivers/iio/imu/adis.c new file mode 100644 index 000000000..bc40240b2 --- /dev/null +++ b/drivers/iio/imu/adis.c @@ -0,0 +1,545 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * Common library for ADIS16XXX devices + * + * Copyright 2012 Analog Devices Inc. + * Author: Lars-Peter Clausen + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#define ADIS_MSC_CTRL_DATA_RDY_EN BIT(2) +#define ADIS_MSC_CTRL_DATA_RDY_POL_HIGH BIT(1) +#define ADIS_MSC_CTRL_DATA_RDY_DIO2 BIT(0) +#define ADIS_GLOB_CMD_SW_RESET BIT(7) + +/** + * __adis_write_reg() - write N bytes to register (unlocked version) + * @adis: The adis device + * @reg: The address of the lower of the two registers + * @value: The value to write to device (up to 4 bytes) + * @size: The size of the @value (in bytes) + */ +int __adis_write_reg(struct adis *adis, unsigned int reg, unsigned int value, + unsigned int size) +{ + unsigned int page = reg / ADIS_PAGE_SIZE; + int ret, i; + struct spi_message msg; + struct spi_transfer xfers[] = { + { + .tx_buf = adis->tx, + .bits_per_word = 8, + .len = 2, + .cs_change = 1, + .delay.value = adis->data->write_delay, + .delay.unit = SPI_DELAY_UNIT_USECS, + .cs_change_delay.value = adis->data->cs_change_delay, + .cs_change_delay.unit = SPI_DELAY_UNIT_USECS, + }, { + .tx_buf = adis->tx + 2, + .bits_per_word = 8, + .len = 2, + .cs_change = 1, + .delay.value = adis->data->write_delay, + .delay.unit = SPI_DELAY_UNIT_USECS, + .cs_change_delay.value = adis->data->cs_change_delay, + .cs_change_delay.unit = SPI_DELAY_UNIT_USECS, + }, { + .tx_buf = adis->tx + 4, + .bits_per_word = 8, + .len = 2, + .cs_change = 1, + .delay.value = adis->data->write_delay, + .delay.unit = SPI_DELAY_UNIT_USECS, + .cs_change_delay.value = adis->data->cs_change_delay, + .cs_change_delay.unit = SPI_DELAY_UNIT_USECS, + }, { + .tx_buf = adis->tx + 6, + .bits_per_word = 8, + .len = 2, + .delay.value = adis->data->write_delay, + .delay.unit = SPI_DELAY_UNIT_USECS, + }, { + .tx_buf = adis->tx + 8, + .bits_per_word = 8, + .len = 2, + .delay.value = adis->data->write_delay, + .delay.unit = SPI_DELAY_UNIT_USECS, + }, + }; + + spi_message_init(&msg); + + if (adis->current_page != page) { + adis->tx[0] = ADIS_WRITE_REG(ADIS_REG_PAGE_ID); + adis->tx[1] = page; + spi_message_add_tail(&xfers[0], &msg); + } + + switch (size) { + case 4: + adis->tx[8] = ADIS_WRITE_REG(reg + 3); + adis->tx[9] = (value >> 24) & 0xff; + adis->tx[6] = ADIS_WRITE_REG(reg + 2); + adis->tx[7] = (value >> 16) & 0xff; + fallthrough; + case 2: + adis->tx[4] = ADIS_WRITE_REG(reg + 1); + adis->tx[5] = (value >> 8) & 0xff; + fallthrough; + case 1: + adis->tx[2] = ADIS_WRITE_REG(reg); + adis->tx[3] = value & 0xff; + break; + default: + return -EINVAL; + } + + xfers[size].cs_change = 0; + + for (i = 1; i <= size; i++) + spi_message_add_tail(&xfers[i], &msg); + + ret = spi_sync(adis->spi, &msg); + if (ret) { + dev_err(&adis->spi->dev, "Failed to write register 0x%02X: %d\n", + reg, ret); + } else { + adis->current_page = page; + } + + return ret; +} +EXPORT_SYMBOL_NS_GPL(__adis_write_reg, IIO_ADISLIB); + +/** + * __adis_read_reg() - read N bytes from register (unlocked version) + * @adis: The adis device + * @reg: The address of the lower of the two registers + * @val: The value read back from the device + * @size: The size of the @val buffer + */ +int __adis_read_reg(struct adis *adis, unsigned int reg, unsigned int *val, + unsigned int size) +{ + unsigned int page = reg / ADIS_PAGE_SIZE; + struct spi_message msg; + int ret; + struct spi_transfer xfers[] = { + { + .tx_buf = adis->tx, + .bits_per_word = 8, + .len = 2, + .cs_change = 1, + .delay.value = adis->data->write_delay, + .delay.unit = SPI_DELAY_UNIT_USECS, + .cs_change_delay.value = adis->data->cs_change_delay, + .cs_change_delay.unit = SPI_DELAY_UNIT_USECS, + }, { + .tx_buf = adis->tx + 2, + .bits_per_word = 8, + .len = 2, + .cs_change = 1, + .delay.value = adis->data->read_delay, + .delay.unit = SPI_DELAY_UNIT_USECS, + .cs_change_delay.value = adis->data->cs_change_delay, + .cs_change_delay.unit = SPI_DELAY_UNIT_USECS, + }, { + .tx_buf = adis->tx + 4, + .rx_buf = adis->rx, + .bits_per_word = 8, + .len = 2, + .cs_change = 1, + .delay.value = adis->data->read_delay, + .delay.unit = SPI_DELAY_UNIT_USECS, + .cs_change_delay.value = adis->data->cs_change_delay, + .cs_change_delay.unit = SPI_DELAY_UNIT_USECS, + }, { + .rx_buf = adis->rx + 2, + .bits_per_word = 8, + .len = 2, + .delay.value = adis->data->read_delay, + .delay.unit = SPI_DELAY_UNIT_USECS, + }, + }; + + spi_message_init(&msg); + + if (adis->current_page != page) { + adis->tx[0] = ADIS_WRITE_REG(ADIS_REG_PAGE_ID); + adis->tx[1] = page; + spi_message_add_tail(&xfers[0], &msg); + } + + switch (size) { + case 4: + adis->tx[2] = ADIS_READ_REG(reg + 2); + adis->tx[3] = 0; + spi_message_add_tail(&xfers[1], &msg); + fallthrough; + case 2: + adis->tx[4] = ADIS_READ_REG(reg); + adis->tx[5] = 0; + spi_message_add_tail(&xfers[2], &msg); + spi_message_add_tail(&xfers[3], &msg); + break; + default: + return -EINVAL; + } + + ret = spi_sync(adis->spi, &msg); + if (ret) { + dev_err(&adis->spi->dev, "Failed to read register 0x%02X: %d\n", + reg, ret); + return ret; + } + + adis->current_page = page; + + switch (size) { + case 4: + *val = get_unaligned_be32(adis->rx); + break; + case 2: + *val = get_unaligned_be16(adis->rx + 2); + break; + } + + return ret; +} +EXPORT_SYMBOL_NS_GPL(__adis_read_reg, IIO_ADISLIB); +/** + * __adis_update_bits_base() - ADIS Update bits function - Unlocked version + * @adis: The adis device + * @reg: The address of the lower of the two registers + * @mask: Bitmask to change + * @val: Value to be written + * @size: Size of the register to update + * + * Updates the desired bits of @reg in accordance with @mask and @val. + */ +int __adis_update_bits_base(struct adis *adis, unsigned int reg, const u32 mask, + const u32 val, u8 size) +{ + int ret; + u32 __val; + + ret = __adis_read_reg(adis, reg, &__val, size); + if (ret) + return ret; + + __val = (__val & ~mask) | (val & mask); + + return __adis_write_reg(adis, reg, __val, size); +} +EXPORT_SYMBOL_NS_GPL(__adis_update_bits_base, IIO_ADISLIB); + +#ifdef CONFIG_DEBUG_FS + +int adis_debugfs_reg_access(struct iio_dev *indio_dev, unsigned int reg, + unsigned int writeval, unsigned int *readval) +{ + struct adis *adis = iio_device_get_drvdata(indio_dev); + + if (readval) { + u16 val16; + int ret; + + ret = adis_read_reg_16(adis, reg, &val16); + if (ret == 0) + *readval = val16; + + return ret; + } + + return adis_write_reg_16(adis, reg, writeval); +} +EXPORT_SYMBOL_NS(adis_debugfs_reg_access, IIO_ADISLIB); + +#endif + +/** + * __adis_enable_irq() - Enable or disable data ready IRQ (unlocked) + * @adis: The adis device + * @enable: Whether to enable the IRQ + * + * Returns 0 on success, negative error code otherwise + */ +int __adis_enable_irq(struct adis *adis, bool enable) +{ + int ret; + u16 msc; + + if (adis->data->enable_irq) + return adis->data->enable_irq(adis, enable); + + if (adis->data->unmasked_drdy) { + if (enable) + enable_irq(adis->spi->irq); + else + disable_irq(adis->spi->irq); + + return 0; + } + + ret = __adis_read_reg_16(adis, adis->data->msc_ctrl_reg, &msc); + if (ret) + return ret; + + msc |= ADIS_MSC_CTRL_DATA_RDY_POL_HIGH; + msc &= ~ADIS_MSC_CTRL_DATA_RDY_DIO2; + if (enable) + msc |= ADIS_MSC_CTRL_DATA_RDY_EN; + else + msc &= ~ADIS_MSC_CTRL_DATA_RDY_EN; + + return __adis_write_reg_16(adis, adis->data->msc_ctrl_reg, msc); +} +EXPORT_SYMBOL_NS(__adis_enable_irq, IIO_ADISLIB); + +/** + * __adis_check_status() - Check the device for error conditions (unlocked) + * @adis: The adis device + * + * Returns 0 on success, a negative error code otherwise + */ +int __adis_check_status(struct adis *adis) +{ + u16 status; + int ret; + int i; + + ret = __adis_read_reg_16(adis, adis->data->diag_stat_reg, &status); + if (ret) + return ret; + + status &= adis->data->status_error_mask; + + if (status == 0) + return 0; + + for (i = 0; i < 16; ++i) { + if (status & BIT(i)) { + dev_err(&adis->spi->dev, "%s.\n", + adis->data->status_error_msgs[i]); + } + } + + return -EIO; +} +EXPORT_SYMBOL_NS_GPL(__adis_check_status, IIO_ADISLIB); + +/** + * __adis_reset() - Reset the device (unlocked version) + * @adis: The adis device + * + * Returns 0 on success, a negative error code otherwise + */ +int __adis_reset(struct adis *adis) +{ + int ret; + const struct adis_timeout *timeouts = adis->data->timeouts; + + ret = __adis_write_reg_8(adis, adis->data->glob_cmd_reg, + ADIS_GLOB_CMD_SW_RESET); + if (ret) { + dev_err(&adis->spi->dev, "Failed to reset device: %d\n", ret); + return ret; + } + + msleep(timeouts->sw_reset_ms); + + return 0; +} +EXPORT_SYMBOL_NS_GPL(__adis_reset, IIO_ADIS_LIB); + +static int adis_self_test(struct adis *adis) +{ + int ret; + const struct adis_timeout *timeouts = adis->data->timeouts; + + ret = __adis_write_reg_16(adis, adis->data->self_test_reg, + adis->data->self_test_mask); + if (ret) { + dev_err(&adis->spi->dev, "Failed to initiate self test: %d\n", + ret); + return ret; + } + + msleep(timeouts->self_test_ms); + + ret = __adis_check_status(adis); + + if (adis->data->self_test_no_autoclear) + __adis_write_reg_16(adis, adis->data->self_test_reg, 0x00); + + return ret; +} + +/** + * __adis_initial_startup() - Device initial setup + * @adis: The adis device + * + * The function performs a HW reset via a reset pin that should be specified + * via GPIOLIB. If no pin is configured a SW reset will be performed. + * The RST pin for the ADIS devices should be configured as ACTIVE_LOW. + * + * After the self-test operation is performed, the function will also check + * that the product ID is as expected. This assumes that drivers providing + * 'prod_id_reg' will also provide the 'prod_id'. + * + * Returns 0 if the device is operational, a negative error code otherwise. + * + * This function should be called early on in the device initialization sequence + * to ensure that the device is in a sane and known state and that it is usable. + */ +int __adis_initial_startup(struct adis *adis) +{ + const struct adis_timeout *timeouts = adis->data->timeouts; + struct gpio_desc *gpio; + u16 prod_id; + int ret; + + /* check if the device has rst pin low */ + gpio = devm_gpiod_get_optional(&adis->spi->dev, "reset", GPIOD_OUT_HIGH); + if (IS_ERR(gpio)) + return PTR_ERR(gpio); + + if (gpio) { + usleep_range(10, 12); + /* bring device out of reset */ + gpiod_set_value_cansleep(gpio, 0); + msleep(timeouts->reset_ms); + } else { + ret = __adis_reset(adis); + if (ret) + return ret; + } + + ret = adis_self_test(adis); + if (ret) + return ret; + + /* + * don't bother calling this if we can't unmask the IRQ as in this case + * the IRQ is most likely not yet requested and we will request it + * with 'IRQF_NO_AUTOEN' anyways. + */ + if (!adis->data->unmasked_drdy) + __adis_enable_irq(adis, false); + + if (!adis->data->prod_id_reg) + return 0; + + ret = adis_read_reg_16(adis, adis->data->prod_id_reg, &prod_id); + if (ret) + return ret; + + if (prod_id != adis->data->prod_id) + dev_warn(&adis->spi->dev, + "Device ID(%u) and product ID(%u) do not match.\n", + adis->data->prod_id, prod_id); + + return 0; +} +EXPORT_SYMBOL_NS_GPL(__adis_initial_startup, IIO_ADISLIB); + +/** + * adis_single_conversion() - Performs a single sample conversion + * @indio_dev: The IIO device + * @chan: The IIO channel + * @error_mask: Mask for the error bit + * @val: Result of the conversion + * + * Returns IIO_VAL_INT on success, a negative error code otherwise. + * + * The function performs a single conversion on a given channel and post + * processes the value accordingly to the channel spec. If a error_mask is given + * the function will check if the mask is set in the returned raw value. If it + * is set the function will perform a self-check. If the device does not report + * a error bit in the channels raw value set error_mask to 0. + */ +int adis_single_conversion(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, + unsigned int error_mask, int *val) +{ + struct adis *adis = iio_device_get_drvdata(indio_dev); + unsigned int uval; + int ret; + + mutex_lock(&adis->state_lock); + + ret = __adis_read_reg(adis, chan->address, &uval, + chan->scan_type.storagebits / 8); + if (ret) + goto err_unlock; + + if (uval & error_mask) { + ret = __adis_check_status(adis); + if (ret) + goto err_unlock; + } + + if (chan->scan_type.sign == 's') + *val = sign_extend32(uval, chan->scan_type.realbits - 1); + else + *val = uval & ((1 << chan->scan_type.realbits) - 1); + + ret = IIO_VAL_INT; +err_unlock: + mutex_unlock(&adis->state_lock); + return ret; +} +EXPORT_SYMBOL_NS_GPL(adis_single_conversion, IIO_ADISLIB); + +/** + * adis_init() - Initialize adis device structure + * @adis: The adis device + * @indio_dev: The iio device + * @spi: The spi device + * @data: Chip specific data + * + * Returns 0 on success, a negative error code otherwise. + * + * This function must be called, before any other adis helper function may be + * called. + */ +int adis_init(struct adis *adis, struct iio_dev *indio_dev, + struct spi_device *spi, const struct adis_data *data) +{ + if (!data || !data->timeouts) { + dev_err(&spi->dev, "No config data or timeouts not defined!\n"); + return -EINVAL; + } + + mutex_init(&adis->state_lock); + adis->spi = spi; + adis->data = data; + iio_device_set_drvdata(indio_dev, adis); + + if (data->has_paging) { + /* Need to set the page before first read/write */ + adis->current_page = -1; + } else { + /* Page will always be 0 */ + adis->current_page = 0; + } + + return 0; +} +EXPORT_SYMBOL_NS_GPL(adis_init, IIO_ADISLIB); + +MODULE_LICENSE("GPL"); +MODULE_AUTHOR("Lars-Peter Clausen "); +MODULE_DESCRIPTION("Common library code for ADIS16XXX devices"); diff --git a/drivers/iio/imu/adis16400.c b/drivers/iio/imu/adis16400.c new file mode 100644 index 000000000..17bb0c40a --- /dev/null +++ b/drivers/iio/imu/adis16400.c @@ -0,0 +1,1243 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * adis16400.c support Analog Devices ADIS16400/5 + * 3d 2g Linear Accelerometers, + * 3d Gyroscopes, + * 3d Magnetometers via SPI + * + * Copyright (c) 2009 Manuel Stahl + * Copyright (c) 2007 Jonathan Cameron + * Copyright (c) 2011 Analog Devices Inc. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#define ADIS16400_STARTUP_DELAY 290 /* ms */ +#define ADIS16400_MTEST_DELAY 90 /* ms */ + +#define ADIS16400_FLASH_CNT 0x00 /* Flash memory write count */ +#define ADIS16400_SUPPLY_OUT 0x02 /* Power supply measurement */ +#define ADIS16400_XGYRO_OUT 0x04 /* X-axis gyroscope output */ +#define ADIS16400_YGYRO_OUT 0x06 /* Y-axis gyroscope output */ +#define ADIS16400_ZGYRO_OUT 0x08 /* Z-axis gyroscope output */ +#define ADIS16400_XACCL_OUT 0x0A /* X-axis accelerometer output */ +#define ADIS16400_YACCL_OUT 0x0C /* Y-axis accelerometer output */ +#define ADIS16400_ZACCL_OUT 0x0E /* Z-axis accelerometer output */ +#define ADIS16400_XMAGN_OUT 0x10 /* X-axis magnetometer measurement */ +#define ADIS16400_YMAGN_OUT 0x12 /* Y-axis magnetometer measurement */ +#define ADIS16400_ZMAGN_OUT 0x14 /* Z-axis magnetometer measurement */ +#define ADIS16400_TEMP_OUT 0x16 /* Temperature output */ +#define ADIS16400_AUX_ADC 0x18 /* Auxiliary ADC measurement */ + +#define ADIS16350_XTEMP_OUT 0x10 /* X-axis gyroscope temperature measurement */ +#define ADIS16350_YTEMP_OUT 0x12 /* Y-axis gyroscope temperature measurement */ +#define ADIS16350_ZTEMP_OUT 0x14 /* Z-axis gyroscope temperature measurement */ + +#define ADIS16300_PITCH_OUT 0x12 /* X axis inclinometer output measurement */ +#define ADIS16300_ROLL_OUT 0x14 /* Y axis inclinometer output measurement */ +#define ADIS16300_AUX_ADC 0x16 /* Auxiliary ADC measurement */ + +#define ADIS16448_BARO_OUT 0x16 /* Barometric pressure output */ +#define ADIS16448_TEMP_OUT 0x18 /* Temperature output */ + +/* Calibration parameters */ +#define ADIS16400_XGYRO_OFF 0x1A /* X-axis gyroscope bias offset factor */ +#define ADIS16400_YGYRO_OFF 0x1C /* Y-axis gyroscope bias offset factor */ +#define ADIS16400_ZGYRO_OFF 0x1E /* Z-axis gyroscope bias offset factor */ +#define ADIS16400_XACCL_OFF 0x20 /* X-axis acceleration bias offset factor */ +#define ADIS16400_YACCL_OFF 0x22 /* Y-axis acceleration bias offset factor */ +#define ADIS16400_ZACCL_OFF 0x24 /* Z-axis acceleration bias offset factor */ +#define ADIS16400_XMAGN_HIF 0x26 /* X-axis magnetometer, hard-iron factor */ +#define ADIS16400_YMAGN_HIF 0x28 /* Y-axis magnetometer, hard-iron factor */ +#define ADIS16400_ZMAGN_HIF 0x2A /* Z-axis magnetometer, hard-iron factor */ +#define ADIS16400_XMAGN_SIF 0x2C /* X-axis magnetometer, soft-iron factor */ +#define ADIS16400_YMAGN_SIF 0x2E /* Y-axis magnetometer, soft-iron factor */ +#define ADIS16400_ZMAGN_SIF 0x30 /* Z-axis magnetometer, soft-iron factor */ + +#define ADIS16400_GPIO_CTRL 0x32 /* Auxiliary digital input/output control */ +#define ADIS16400_MSC_CTRL 0x34 /* Miscellaneous control */ +#define ADIS16400_SMPL_PRD 0x36 /* Internal sample period (rate) control */ +#define ADIS16400_SENS_AVG 0x38 /* Dynamic range and digital filter control */ +#define ADIS16400_SLP_CNT 0x3A /* Sleep mode control */ +#define ADIS16400_DIAG_STAT 0x3C /* System status */ + +/* Alarm functions */ +#define ADIS16400_GLOB_CMD 0x3E /* System command */ +#define ADIS16400_ALM_MAG1 0x40 /* Alarm 1 amplitude threshold */ +#define ADIS16400_ALM_MAG2 0x42 /* Alarm 2 amplitude threshold */ +#define ADIS16400_ALM_SMPL1 0x44 /* Alarm 1 sample size */ +#define ADIS16400_ALM_SMPL2 0x46 /* Alarm 2 sample size */ +#define ADIS16400_ALM_CTRL 0x48 /* Alarm control */ +#define ADIS16400_AUX_DAC 0x4A /* Auxiliary DAC data */ + +#define ADIS16334_LOT_ID1 0x52 /* Lot identification code 1 */ +#define ADIS16334_LOT_ID2 0x54 /* Lot identification code 2 */ +#define ADIS16400_PRODUCT_ID 0x56 /* Product identifier */ +#define ADIS16334_SERIAL_NUMBER 0x58 /* Serial number, lot specific */ + +#define ADIS16400_ERROR_ACTIVE (1<<14) +#define ADIS16400_NEW_DATA (1<<14) + +/* MSC_CTRL */ +#define ADIS16400_MSC_CTRL_MEM_TEST (1<<11) +#define ADIS16400_MSC_CTRL_INT_SELF_TEST (1<<10) +#define ADIS16400_MSC_CTRL_NEG_SELF_TEST (1<<9) +#define ADIS16400_MSC_CTRL_POS_SELF_TEST (1<<8) +#define ADIS16400_MSC_CTRL_GYRO_BIAS (1<<7) +#define ADIS16400_MSC_CTRL_ACCL_ALIGN (1<<6) +#define ADIS16400_MSC_CTRL_DATA_RDY_EN (1<<2) +#define ADIS16400_MSC_CTRL_DATA_RDY_POL_HIGH (1<<1) +#define ADIS16400_MSC_CTRL_DATA_RDY_DIO2 (1<<0) + +/* SMPL_PRD */ +#define ADIS16400_SMPL_PRD_TIME_BASE (1<<7) +#define ADIS16400_SMPL_PRD_DIV_MASK 0x7F + +/* DIAG_STAT */ +#define ADIS16400_DIAG_STAT_ZACCL_FAIL 15 +#define ADIS16400_DIAG_STAT_YACCL_FAIL 14 +#define ADIS16400_DIAG_STAT_XACCL_FAIL 13 +#define ADIS16400_DIAG_STAT_XGYRO_FAIL 12 +#define ADIS16400_DIAG_STAT_YGYRO_FAIL 11 +#define ADIS16400_DIAG_STAT_ZGYRO_FAIL 10 +#define ADIS16400_DIAG_STAT_ALARM2 9 +#define ADIS16400_DIAG_STAT_ALARM1 8 +#define ADIS16400_DIAG_STAT_FLASH_CHK 6 +#define ADIS16400_DIAG_STAT_SELF_TEST 5 +#define ADIS16400_DIAG_STAT_OVERFLOW 4 +#define ADIS16400_DIAG_STAT_SPI_FAIL 3 +#define ADIS16400_DIAG_STAT_FLASH_UPT 2 +#define ADIS16400_DIAG_STAT_POWER_HIGH 1 +#define ADIS16400_DIAG_STAT_POWER_LOW 0 + +/* GLOB_CMD */ +#define ADIS16400_GLOB_CMD_SW_RESET (1<<7) +#define ADIS16400_GLOB_CMD_P_AUTO_NULL (1<<4) +#define ADIS16400_GLOB_CMD_FLASH_UPD (1<<3) +#define ADIS16400_GLOB_CMD_DAC_LATCH (1<<2) +#define ADIS16400_GLOB_CMD_FAC_CALIB (1<<1) +#define ADIS16400_GLOB_CMD_AUTO_NULL (1<<0) + +/* SLP_CNT */ +#define ADIS16400_SLP_CNT_POWER_OFF (1<<8) + +#define ADIS16334_RATE_DIV_SHIFT 8 +#define ADIS16334_RATE_INT_CLK BIT(0) + +#define ADIS16400_SPI_SLOW (u32)(300 * 1000) +#define ADIS16400_SPI_BURST (u32)(1000 * 1000) +#define ADIS16400_SPI_FAST (u32)(2000 * 1000) + +#define ADIS16400_HAS_PROD_ID BIT(0) +#define ADIS16400_NO_BURST BIT(1) +#define ADIS16400_HAS_SLOW_MODE BIT(2) +#define ADIS16400_HAS_SERIAL_NUMBER BIT(3) +#define ADIS16400_BURST_DIAG_STAT BIT(4) + +struct adis16400_state; + +struct adis16400_chip_info { + const struct iio_chan_spec *channels; + const struct adis_data adis_data; + const int num_channels; + const long flags; + unsigned int gyro_scale_micro; + unsigned int accel_scale_micro; + int temp_scale_nano; + int temp_offset; + /* set_freq() & get_freq() need to avoid using ADIS lib's state lock */ + int (*set_freq)(struct adis16400_state *st, unsigned int freq); + int (*get_freq)(struct adis16400_state *st); +}; + +/** + * struct adis16400_state - device instance specific data + * @variant: chip variant info + * @filt_int: integer part of requested filter frequency + * @adis: adis device + * @avail_scan_mask: NULL terminated array of bitmaps of channels + * that must be enabled together + **/ +struct adis16400_state { + struct adis16400_chip_info *variant; + int filt_int; + + struct adis adis; + unsigned long avail_scan_mask[2]; +}; + +/* At the moment triggers are only used for ring buffer + * filling. This may change! + */ + +enum { + ADIS16400_SCAN_SUPPLY, + ADIS16400_SCAN_GYRO_X, + ADIS16400_SCAN_GYRO_Y, + ADIS16400_SCAN_GYRO_Z, + ADIS16400_SCAN_ACC_X, + ADIS16400_SCAN_ACC_Y, + ADIS16400_SCAN_ACC_Z, + ADIS16400_SCAN_MAGN_X, + ADIS16400_SCAN_MAGN_Y, + ADIS16400_SCAN_MAGN_Z, + ADIS16400_SCAN_BARO, + ADIS16350_SCAN_TEMP_X, + ADIS16350_SCAN_TEMP_Y, + ADIS16350_SCAN_TEMP_Z, + ADIS16300_SCAN_INCLI_X, + ADIS16300_SCAN_INCLI_Y, + ADIS16400_SCAN_ADC, + ADIS16400_SCAN_TIMESTAMP, +}; + +#ifdef CONFIG_DEBUG_FS + +static ssize_t adis16400_show_serial_number(struct file *file, + char __user *userbuf, size_t count, loff_t *ppos) +{ + struct adis16400_state *st = file->private_data; + u16 lot1, lot2, serial_number; + char buf[16]; + size_t len; + int ret; + + ret = adis_read_reg_16(&st->adis, ADIS16334_LOT_ID1, &lot1); + if (ret) + return ret; + + ret = adis_read_reg_16(&st->adis, ADIS16334_LOT_ID2, &lot2); + if (ret) + return ret; + + ret = adis_read_reg_16(&st->adis, ADIS16334_SERIAL_NUMBER, + &serial_number); + if (ret) + return ret; + + len = snprintf(buf, sizeof(buf), "%.4x-%.4x-%.4x\n", lot1, lot2, + serial_number); + + return simple_read_from_buffer(userbuf, count, ppos, buf, len); +} + +static const struct file_operations adis16400_serial_number_fops = { + .open = simple_open, + .read = adis16400_show_serial_number, + .llseek = default_llseek, + .owner = THIS_MODULE, +}; + +static int adis16400_show_product_id(void *arg, u64 *val) +{ + struct adis16400_state *st = arg; + uint16_t prod_id; + int ret; + + ret = adis_read_reg_16(&st->adis, ADIS16400_PRODUCT_ID, &prod_id); + if (ret) + return ret; + + *val = prod_id; + + return 0; +} +DEFINE_DEBUGFS_ATTRIBUTE(adis16400_product_id_fops, + adis16400_show_product_id, NULL, "%lld\n"); + +static int adis16400_show_flash_count(void *arg, u64 *val) +{ + struct adis16400_state *st = arg; + uint16_t flash_count; + int ret; + + ret = adis_read_reg_16(&st->adis, ADIS16400_FLASH_CNT, &flash_count); + if (ret) + return ret; + + *val = flash_count; + + return 0; +} +DEFINE_DEBUGFS_ATTRIBUTE(adis16400_flash_count_fops, + adis16400_show_flash_count, NULL, "%lld\n"); + +static int adis16400_debugfs_init(struct iio_dev *indio_dev) +{ + struct adis16400_state *st = iio_priv(indio_dev); + struct dentry *d = iio_get_debugfs_dentry(indio_dev); + + if (st->variant->flags & ADIS16400_HAS_SERIAL_NUMBER) + debugfs_create_file_unsafe("serial_number", 0400, + d, st, &adis16400_serial_number_fops); + if (st->variant->flags & ADIS16400_HAS_PROD_ID) + debugfs_create_file_unsafe("product_id", 0400, + d, st, &adis16400_product_id_fops); + debugfs_create_file_unsafe("flash_count", 0400, + d, st, &adis16400_flash_count_fops); + + return 0; +} + +#else + +static int adis16400_debugfs_init(struct iio_dev *indio_dev) +{ + return 0; +} + +#endif + +enum adis16400_chip_variant { + ADIS16300, + ADIS16334, + ADIS16350, + ADIS16360, + ADIS16362, + ADIS16364, + ADIS16367, + ADIS16400, + ADIS16445, + ADIS16448, +}; + +static int adis16334_get_freq(struct adis16400_state *st) +{ + int ret; + uint16_t t; + + ret = __adis_read_reg_16(&st->adis, ADIS16400_SMPL_PRD, &t); + if (ret) + return ret; + + t >>= ADIS16334_RATE_DIV_SHIFT; + + return 819200 >> t; +} + +static int adis16334_set_freq(struct adis16400_state *st, unsigned int freq) +{ + unsigned int t; + + if (freq < 819200) + t = ilog2(819200 / freq); + else + t = 0; + + if (t > 0x31) + t = 0x31; + + t <<= ADIS16334_RATE_DIV_SHIFT; + t |= ADIS16334_RATE_INT_CLK; + + return __adis_write_reg_16(&st->adis, ADIS16400_SMPL_PRD, t); +} + +static int adis16400_get_freq(struct adis16400_state *st) +{ + int sps, ret; + uint16_t t; + + ret = __adis_read_reg_16(&st->adis, ADIS16400_SMPL_PRD, &t); + if (ret) + return ret; + + sps = (t & ADIS16400_SMPL_PRD_TIME_BASE) ? 52851 : 1638404; + sps /= (t & ADIS16400_SMPL_PRD_DIV_MASK) + 1; + + return sps; +} + +static int adis16400_set_freq(struct adis16400_state *st, unsigned int freq) +{ + unsigned int t; + uint8_t val = 0; + + t = 1638404 / freq; + if (t >= 128) { + val |= ADIS16400_SMPL_PRD_TIME_BASE; + t = 52851 / freq; + if (t >= 128) + t = 127; + } else if (t != 0) { + t--; + } + + val |= t; + + if (t >= 0x0A || (val & ADIS16400_SMPL_PRD_TIME_BASE)) + st->adis.spi->max_speed_hz = ADIS16400_SPI_SLOW; + else + st->adis.spi->max_speed_hz = ADIS16400_SPI_FAST; + + return __adis_write_reg_8(&st->adis, ADIS16400_SMPL_PRD, val); +} + +static const unsigned int adis16400_3db_divisors[] = { + [0] = 2, /* Special case */ + [1] = 6, + [2] = 12, + [3] = 25, + [4] = 50, + [5] = 100, + [6] = 200, + [7] = 200, /* Not a valid setting */ +}; + +static int __adis16400_set_filter(struct iio_dev *indio_dev, int sps, int val) +{ + struct adis16400_state *st = iio_priv(indio_dev); + uint16_t val16; + int i, ret; + + for (i = ARRAY_SIZE(adis16400_3db_divisors) - 1; i >= 1; i--) { + if (sps / adis16400_3db_divisors[i] >= val) + break; + } + + ret = __adis_read_reg_16(&st->adis, ADIS16400_SENS_AVG, &val16); + if (ret) + return ret; + + ret = __adis_write_reg_16(&st->adis, ADIS16400_SENS_AVG, + (val16 & ~0x07) | i); + return ret; +} + +/* Power down the device */ +static int adis16400_stop_device(struct iio_dev *indio_dev) +{ + struct adis16400_state *st = iio_priv(indio_dev); + int ret; + + ret = adis_write_reg_16(&st->adis, ADIS16400_SLP_CNT, + ADIS16400_SLP_CNT_POWER_OFF); + if (ret) + dev_err(&indio_dev->dev, + "problem with turning device off: SLP_CNT"); + + return ret; +} + +static int adis16400_initial_setup(struct iio_dev *indio_dev) +{ + struct adis16400_state *st = iio_priv(indio_dev); + uint16_t prod_id, smp_prd; + unsigned int device_id; + int ret; + + /* use low spi speed for init if the device has a slow mode */ + if (st->variant->flags & ADIS16400_HAS_SLOW_MODE) + st->adis.spi->max_speed_hz = ADIS16400_SPI_SLOW; + else + st->adis.spi->max_speed_hz = ADIS16400_SPI_FAST; + st->adis.spi->mode = SPI_MODE_3; + spi_setup(st->adis.spi); + + ret = adis_initial_startup(&st->adis); + if (ret) + return ret; + + if (st->variant->flags & ADIS16400_HAS_PROD_ID) { + ret = adis_read_reg_16(&st->adis, + ADIS16400_PRODUCT_ID, &prod_id); + if (ret) + goto err_ret; + + if (sscanf(indio_dev->name, "adis%u\n", &device_id) != 1) { + ret = -EINVAL; + goto err_ret; + } + + if (prod_id != device_id) + dev_warn(&indio_dev->dev, "Device ID(%u) and product ID(%u) do not match.", + device_id, prod_id); + + dev_info(&indio_dev->dev, "%s: prod_id 0x%04x at CS%d (irq %d)\n", + indio_dev->name, prod_id, + st->adis.spi->chip_select, st->adis.spi->irq); + } + /* use high spi speed if possible */ + if (st->variant->flags & ADIS16400_HAS_SLOW_MODE) { + ret = adis_read_reg_16(&st->adis, ADIS16400_SMPL_PRD, &smp_prd); + if (ret) + goto err_ret; + + if ((smp_prd & ADIS16400_SMPL_PRD_DIV_MASK) < 0x0A) { + st->adis.spi->max_speed_hz = ADIS16400_SPI_FAST; + spi_setup(st->adis.spi); + } + } + +err_ret: + return ret; +} + +static const uint8_t adis16400_addresses[] = { + [ADIS16400_SCAN_GYRO_X] = ADIS16400_XGYRO_OFF, + [ADIS16400_SCAN_GYRO_Y] = ADIS16400_YGYRO_OFF, + [ADIS16400_SCAN_GYRO_Z] = ADIS16400_ZGYRO_OFF, + [ADIS16400_SCAN_ACC_X] = ADIS16400_XACCL_OFF, + [ADIS16400_SCAN_ACC_Y] = ADIS16400_YACCL_OFF, + [ADIS16400_SCAN_ACC_Z] = ADIS16400_ZACCL_OFF, +}; + +static int adis16400_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, int val, int val2, long info) +{ + struct adis16400_state *st = iio_priv(indio_dev); + int ret, sps; + + switch (info) { + case IIO_CHAN_INFO_CALIBBIAS: + ret = adis_write_reg_16(&st->adis, + adis16400_addresses[chan->scan_index], val); + return ret; + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: + /* + * Need to cache values so we can update if the frequency + * changes. + */ + adis_dev_lock(&st->adis); + st->filt_int = val; + /* Work out update to current value */ + sps = st->variant->get_freq(st); + if (sps < 0) { + adis_dev_unlock(&st->adis); + return sps; + } + + ret = __adis16400_set_filter(indio_dev, sps, + val * 1000 + val2 / 1000); + adis_dev_unlock(&st->adis); + return ret; + case IIO_CHAN_INFO_SAMP_FREQ: + sps = val * 1000 + val2 / 1000; + + if (sps <= 0) + return -EINVAL; + + adis_dev_lock(&st->adis); + ret = st->variant->set_freq(st, sps); + adis_dev_unlock(&st->adis); + return ret; + default: + return -EINVAL; + } +} + +static int adis16400_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, int *val, int *val2, long info) +{ + struct adis16400_state *st = iio_priv(indio_dev); + int16_t val16; + int ret; + + switch (info) { + case IIO_CHAN_INFO_RAW: + return adis_single_conversion(indio_dev, chan, 0, val); + case IIO_CHAN_INFO_SCALE: + switch (chan->type) { + case IIO_ANGL_VEL: + *val = 0; + *val2 = st->variant->gyro_scale_micro; + return IIO_VAL_INT_PLUS_MICRO; + case IIO_VOLTAGE: + *val = 0; + if (chan->channel == 0) { + *val = 2; + *val2 = 418000; /* 2.418 mV */ + } else { + *val = 0; + *val2 = 805800; /* 805.8 uV */ + } + return IIO_VAL_INT_PLUS_MICRO; + case IIO_ACCEL: + *val = 0; + *val2 = st->variant->accel_scale_micro; + return IIO_VAL_INT_PLUS_MICRO; + case IIO_MAGN: + *val = 0; + *val2 = 500; /* 0.5 mgauss */ + return IIO_VAL_INT_PLUS_MICRO; + case IIO_TEMP: + *val = st->variant->temp_scale_nano / 1000000; + *val2 = (st->variant->temp_scale_nano % 1000000); + return IIO_VAL_INT_PLUS_MICRO; + case IIO_PRESSURE: + /* 20 uBar = 0.002kPascal */ + *val = 0; + *val2 = 2000; + return IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } + case IIO_CHAN_INFO_CALIBBIAS: + ret = adis_read_reg_16(&st->adis, + adis16400_addresses[chan->scan_index], &val16); + if (ret) + return ret; + val16 = sign_extend32(val16, 11); + *val = val16; + return IIO_VAL_INT; + case IIO_CHAN_INFO_OFFSET: + /* currently only temperature */ + *val = st->variant->temp_offset; + return IIO_VAL_INT; + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: + adis_dev_lock(&st->adis); + /* Need both the number of taps and the sampling frequency */ + ret = __adis_read_reg_16(&st->adis, + ADIS16400_SENS_AVG, + &val16); + if (ret) { + adis_dev_unlock(&st->adis); + return ret; + } + ret = st->variant->get_freq(st); + adis_dev_unlock(&st->adis); + if (ret) + return ret; + ret /= adis16400_3db_divisors[val16 & 0x07]; + *val = ret / 1000; + *val2 = (ret % 1000) * 1000; + return IIO_VAL_INT_PLUS_MICRO; + case IIO_CHAN_INFO_SAMP_FREQ: + adis_dev_lock(&st->adis); + ret = st->variant->get_freq(st); + adis_dev_unlock(&st->adis); + if (ret) + return ret; + *val = ret / 1000; + *val2 = (ret % 1000) * 1000; + return IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } +} + +#if IS_ENABLED(CONFIG_IIO_BUFFER) +static irqreturn_t adis16400_trigger_handler(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + struct adis16400_state *st = iio_priv(indio_dev); + struct adis *adis = &st->adis; + void *buffer; + int ret; + + ret = spi_sync(adis->spi, &adis->msg); + if (ret) + dev_err(&adis->spi->dev, "Failed to read data: %d\n", ret); + + if (st->variant->flags & ADIS16400_BURST_DIAG_STAT) { + buffer = adis->buffer + sizeof(u16); + /* + * The size here is always larger than, or equal to the true + * size of the channel data. This may result in a larger copy + * than necessary, but as the target buffer will be + * buffer->scan_bytes this will be safe. + */ + iio_push_to_buffers_with_ts_unaligned(indio_dev, buffer, + indio_dev->scan_bytes - sizeof(pf->timestamp), + pf->timestamp); + } else { + iio_push_to_buffers_with_timestamp(indio_dev, + adis->buffer, + pf->timestamp); + } + + + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; +} +#else +#define adis16400_trigger_handler NULL +#endif /* IS_ENABLED(CONFIG_IIO_BUFFER) */ + +#define ADIS16400_VOLTAGE_CHAN(addr, bits, name, si, chn) { \ + .type = IIO_VOLTAGE, \ + .indexed = 1, \ + .channel = chn, \ + .extend_name = name, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .address = (addr), \ + .scan_index = (si), \ + .scan_type = { \ + .sign = 'u', \ + .realbits = (bits), \ + .storagebits = 16, \ + .shift = 0, \ + .endianness = IIO_BE, \ + }, \ +} + +#define ADIS16400_SUPPLY_CHAN(addr, bits) \ + ADIS16400_VOLTAGE_CHAN(addr, bits, "supply", ADIS16400_SCAN_SUPPLY, 0) + +#define ADIS16400_AUX_ADC_CHAN(addr, bits) \ + ADIS16400_VOLTAGE_CHAN(addr, bits, NULL, ADIS16400_SCAN_ADC, 1) + +#define ADIS16400_GYRO_CHAN(mod, addr, bits) { \ + .type = IIO_ANGL_VEL, \ + .modified = 1, \ + .channel2 = IIO_MOD_ ## mod, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_CALIBBIAS), \ + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) | \ + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .address = addr, \ + .scan_index = ADIS16400_SCAN_GYRO_ ## mod, \ + .scan_type = { \ + .sign = 's', \ + .realbits = (bits), \ + .storagebits = 16, \ + .shift = 0, \ + .endianness = IIO_BE, \ + }, \ +} + +#define ADIS16400_ACCEL_CHAN(mod, addr, bits) { \ + .type = IIO_ACCEL, \ + .modified = 1, \ + .channel2 = IIO_MOD_ ## mod, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_CALIBBIAS), \ + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) | \ + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .address = (addr), \ + .scan_index = ADIS16400_SCAN_ACC_ ## mod, \ + .scan_type = { \ + .sign = 's', \ + .realbits = (bits), \ + .storagebits = 16, \ + .shift = 0, \ + .endianness = IIO_BE, \ + }, \ +} + +#define ADIS16400_MAGN_CHAN(mod, addr, bits) { \ + .type = IIO_MAGN, \ + .modified = 1, \ + .channel2 = IIO_MOD_ ## mod, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \ + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) | \ + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .address = (addr), \ + .scan_index = ADIS16400_SCAN_MAGN_ ## mod, \ + .scan_type = { \ + .sign = 's', \ + .realbits = (bits), \ + .storagebits = 16, \ + .shift = 0, \ + .endianness = IIO_BE, \ + }, \ +} + +#define ADIS16400_MOD_TEMP_NAME_X "x" +#define ADIS16400_MOD_TEMP_NAME_Y "y" +#define ADIS16400_MOD_TEMP_NAME_Z "z" + +#define ADIS16400_MOD_TEMP_CHAN(mod, addr, bits) { \ + .type = IIO_TEMP, \ + .indexed = 1, \ + .channel = 0, \ + .extend_name = ADIS16400_MOD_TEMP_NAME_ ## mod, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_OFFSET) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_shared_by_type = \ + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .address = (addr), \ + .scan_index = ADIS16350_SCAN_TEMP_ ## mod, \ + .scan_type = { \ + .sign = 's', \ + .realbits = (bits), \ + .storagebits = 16, \ + .shift = 0, \ + .endianness = IIO_BE, \ + }, \ +} + +#define ADIS16400_TEMP_CHAN(addr, bits) { \ + .type = IIO_TEMP, \ + .indexed = 1, \ + .channel = 0, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_OFFSET) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .address = (addr), \ + .scan_index = ADIS16350_SCAN_TEMP_X, \ + .scan_type = { \ + .sign = 's', \ + .realbits = (bits), \ + .storagebits = 16, \ + .shift = 0, \ + .endianness = IIO_BE, \ + }, \ +} + +#define ADIS16400_INCLI_CHAN(mod, addr, bits) { \ + .type = IIO_INCLI, \ + .modified = 1, \ + .channel2 = IIO_MOD_ ## mod, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \ + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .address = (addr), \ + .scan_index = ADIS16300_SCAN_INCLI_ ## mod, \ + .scan_type = { \ + .sign = 's', \ + .realbits = (bits), \ + .storagebits = 16, \ + .shift = 0, \ + .endianness = IIO_BE, \ + }, \ +} + +static const struct iio_chan_spec adis16400_channels[] = { + ADIS16400_SUPPLY_CHAN(ADIS16400_SUPPLY_OUT, 14), + ADIS16400_GYRO_CHAN(X, ADIS16400_XGYRO_OUT, 14), + ADIS16400_GYRO_CHAN(Y, ADIS16400_YGYRO_OUT, 14), + ADIS16400_GYRO_CHAN(Z, ADIS16400_ZGYRO_OUT, 14), + ADIS16400_ACCEL_CHAN(X, ADIS16400_XACCL_OUT, 14), + ADIS16400_ACCEL_CHAN(Y, ADIS16400_YACCL_OUT, 14), + ADIS16400_ACCEL_CHAN(Z, ADIS16400_ZACCL_OUT, 14), + ADIS16400_MAGN_CHAN(X, ADIS16400_XMAGN_OUT, 14), + ADIS16400_MAGN_CHAN(Y, ADIS16400_YMAGN_OUT, 14), + ADIS16400_MAGN_CHAN(Z, ADIS16400_ZMAGN_OUT, 14), + ADIS16400_TEMP_CHAN(ADIS16400_TEMP_OUT, 12), + ADIS16400_AUX_ADC_CHAN(ADIS16400_AUX_ADC, 12), + IIO_CHAN_SOFT_TIMESTAMP(ADIS16400_SCAN_TIMESTAMP), +}; + +static const struct iio_chan_spec adis16445_channels[] = { + ADIS16400_GYRO_CHAN(X, ADIS16400_XGYRO_OUT, 16), + ADIS16400_GYRO_CHAN(Y, ADIS16400_YGYRO_OUT, 16), + ADIS16400_GYRO_CHAN(Z, ADIS16400_ZGYRO_OUT, 16), + ADIS16400_ACCEL_CHAN(X, ADIS16400_XACCL_OUT, 16), + ADIS16400_ACCEL_CHAN(Y, ADIS16400_YACCL_OUT, 16), + ADIS16400_ACCEL_CHAN(Z, ADIS16400_ZACCL_OUT, 16), + ADIS16400_TEMP_CHAN(ADIS16448_TEMP_OUT, 12), + IIO_CHAN_SOFT_TIMESTAMP(ADIS16400_SCAN_TIMESTAMP), +}; + +static const struct iio_chan_spec adis16448_channels[] = { + ADIS16400_GYRO_CHAN(X, ADIS16400_XGYRO_OUT, 16), + ADIS16400_GYRO_CHAN(Y, ADIS16400_YGYRO_OUT, 16), + ADIS16400_GYRO_CHAN(Z, ADIS16400_ZGYRO_OUT, 16), + ADIS16400_ACCEL_CHAN(X, ADIS16400_XACCL_OUT, 16), + ADIS16400_ACCEL_CHAN(Y, ADIS16400_YACCL_OUT, 16), + ADIS16400_ACCEL_CHAN(Z, ADIS16400_ZACCL_OUT, 16), + ADIS16400_MAGN_CHAN(X, ADIS16400_XMAGN_OUT, 16), + ADIS16400_MAGN_CHAN(Y, ADIS16400_YMAGN_OUT, 16), + ADIS16400_MAGN_CHAN(Z, ADIS16400_ZMAGN_OUT, 16), + { + .type = IIO_PRESSURE, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), + .address = ADIS16448_BARO_OUT, + .scan_index = ADIS16400_SCAN_BARO, + .scan_type = { + .sign = 's', + .realbits = 16, + .storagebits = 16, + .endianness = IIO_BE, + }, + }, + ADIS16400_TEMP_CHAN(ADIS16448_TEMP_OUT, 12), + IIO_CHAN_SOFT_TIMESTAMP(ADIS16400_SCAN_TIMESTAMP), +}; + +static const struct iio_chan_spec adis16350_channels[] = { + ADIS16400_SUPPLY_CHAN(ADIS16400_SUPPLY_OUT, 12), + ADIS16400_GYRO_CHAN(X, ADIS16400_XGYRO_OUT, 14), + ADIS16400_GYRO_CHAN(Y, ADIS16400_YGYRO_OUT, 14), + ADIS16400_GYRO_CHAN(Z, ADIS16400_ZGYRO_OUT, 14), + ADIS16400_ACCEL_CHAN(X, ADIS16400_XACCL_OUT, 14), + ADIS16400_ACCEL_CHAN(Y, ADIS16400_YACCL_OUT, 14), + ADIS16400_ACCEL_CHAN(Z, ADIS16400_ZACCL_OUT, 14), + ADIS16400_MAGN_CHAN(X, ADIS16400_XMAGN_OUT, 14), + ADIS16400_MAGN_CHAN(Y, ADIS16400_YMAGN_OUT, 14), + ADIS16400_MAGN_CHAN(Z, ADIS16400_ZMAGN_OUT, 14), + ADIS16400_AUX_ADC_CHAN(ADIS16300_AUX_ADC, 12), + ADIS16400_MOD_TEMP_CHAN(X, ADIS16350_XTEMP_OUT, 12), + ADIS16400_MOD_TEMP_CHAN(Y, ADIS16350_YTEMP_OUT, 12), + ADIS16400_MOD_TEMP_CHAN(Z, ADIS16350_ZTEMP_OUT, 12), + IIO_CHAN_SOFT_TIMESTAMP(ADIS16400_SCAN_TIMESTAMP), +}; + +static const struct iio_chan_spec adis16300_channels[] = { + ADIS16400_SUPPLY_CHAN(ADIS16400_SUPPLY_OUT, 12), + ADIS16400_GYRO_CHAN(X, ADIS16400_XGYRO_OUT, 14), + ADIS16400_ACCEL_CHAN(X, ADIS16400_XACCL_OUT, 14), + ADIS16400_ACCEL_CHAN(Y, ADIS16400_YACCL_OUT, 14), + ADIS16400_ACCEL_CHAN(Z, ADIS16400_ZACCL_OUT, 14), + ADIS16400_TEMP_CHAN(ADIS16350_XTEMP_OUT, 12), + ADIS16400_AUX_ADC_CHAN(ADIS16300_AUX_ADC, 12), + ADIS16400_INCLI_CHAN(X, ADIS16300_PITCH_OUT, 13), + ADIS16400_INCLI_CHAN(Y, ADIS16300_ROLL_OUT, 13), + IIO_CHAN_SOFT_TIMESTAMP(ADIS16400_SCAN_TIMESTAMP), +}; + +static const struct iio_chan_spec adis16334_channels[] = { + ADIS16400_GYRO_CHAN(X, ADIS16400_XGYRO_OUT, 14), + ADIS16400_GYRO_CHAN(Y, ADIS16400_YGYRO_OUT, 14), + ADIS16400_GYRO_CHAN(Z, ADIS16400_ZGYRO_OUT, 14), + ADIS16400_ACCEL_CHAN(X, ADIS16400_XACCL_OUT, 14), + ADIS16400_ACCEL_CHAN(Y, ADIS16400_YACCL_OUT, 14), + ADIS16400_ACCEL_CHAN(Z, ADIS16400_ZACCL_OUT, 14), + ADIS16400_TEMP_CHAN(ADIS16350_XTEMP_OUT, 12), + IIO_CHAN_SOFT_TIMESTAMP(ADIS16400_SCAN_TIMESTAMP), +}; + +static const char * const adis16400_status_error_msgs[] = { + [ADIS16400_DIAG_STAT_ZACCL_FAIL] = "Z-axis accelerometer self-test failure", + [ADIS16400_DIAG_STAT_YACCL_FAIL] = "Y-axis accelerometer self-test failure", + [ADIS16400_DIAG_STAT_XACCL_FAIL] = "X-axis accelerometer self-test failure", + [ADIS16400_DIAG_STAT_XGYRO_FAIL] = "X-axis gyroscope self-test failure", + [ADIS16400_DIAG_STAT_YGYRO_FAIL] = "Y-axis gyroscope self-test failure", + [ADIS16400_DIAG_STAT_ZGYRO_FAIL] = "Z-axis gyroscope self-test failure", + [ADIS16400_DIAG_STAT_ALARM2] = "Alarm 2 active", + [ADIS16400_DIAG_STAT_ALARM1] = "Alarm 1 active", + [ADIS16400_DIAG_STAT_FLASH_CHK] = "Flash checksum error", + [ADIS16400_DIAG_STAT_SELF_TEST] = "Self test error", + [ADIS16400_DIAG_STAT_OVERFLOW] = "Sensor overrange", + [ADIS16400_DIAG_STAT_SPI_FAIL] = "SPI failure", + [ADIS16400_DIAG_STAT_FLASH_UPT] = "Flash update failed", + [ADIS16400_DIAG_STAT_POWER_HIGH] = "Power supply above 5.25V", + [ADIS16400_DIAG_STAT_POWER_LOW] = "Power supply below 4.75V", +}; + +#define ADIS16400_DATA(_timeouts, _burst_len) \ +{ \ + .msc_ctrl_reg = ADIS16400_MSC_CTRL, \ + .glob_cmd_reg = ADIS16400_GLOB_CMD, \ + .diag_stat_reg = ADIS16400_DIAG_STAT, \ + .read_delay = 50, \ + .write_delay = 50, \ + .self_test_mask = ADIS16400_MSC_CTRL_MEM_TEST, \ + .self_test_reg = ADIS16400_MSC_CTRL, \ + .status_error_msgs = adis16400_status_error_msgs, \ + .status_error_mask = BIT(ADIS16400_DIAG_STAT_ZACCL_FAIL) | \ + BIT(ADIS16400_DIAG_STAT_YACCL_FAIL) | \ + BIT(ADIS16400_DIAG_STAT_XACCL_FAIL) | \ + BIT(ADIS16400_DIAG_STAT_XGYRO_FAIL) | \ + BIT(ADIS16400_DIAG_STAT_YGYRO_FAIL) | \ + BIT(ADIS16400_DIAG_STAT_ZGYRO_FAIL) | \ + BIT(ADIS16400_DIAG_STAT_ALARM2) | \ + BIT(ADIS16400_DIAG_STAT_ALARM1) | \ + BIT(ADIS16400_DIAG_STAT_FLASH_CHK) | \ + BIT(ADIS16400_DIAG_STAT_SELF_TEST) | \ + BIT(ADIS16400_DIAG_STAT_OVERFLOW) | \ + BIT(ADIS16400_DIAG_STAT_SPI_FAIL) | \ + BIT(ADIS16400_DIAG_STAT_FLASH_UPT) | \ + BIT(ADIS16400_DIAG_STAT_POWER_HIGH) | \ + BIT(ADIS16400_DIAG_STAT_POWER_LOW), \ + .timeouts = (_timeouts), \ + .burst_reg_cmd = ADIS16400_GLOB_CMD, \ + .burst_len = (_burst_len), \ + .burst_max_speed_hz = ADIS16400_SPI_BURST \ +} + +static const struct adis_timeout adis16300_timeouts = { + .reset_ms = ADIS16400_STARTUP_DELAY, + .sw_reset_ms = ADIS16400_STARTUP_DELAY, + .self_test_ms = ADIS16400_STARTUP_DELAY, +}; + +static const struct adis_timeout adis16334_timeouts = { + .reset_ms = 60, + .sw_reset_ms = 60, + .self_test_ms = 14, +}; + +static const struct adis_timeout adis16362_timeouts = { + .reset_ms = 130, + .sw_reset_ms = 130, + .self_test_ms = 12, +}; + +static const struct adis_timeout adis16400_timeouts = { + .reset_ms = 170, + .sw_reset_ms = 170, + .self_test_ms = 12, +}; + +static const struct adis_timeout adis16445_timeouts = { + .reset_ms = 55, + .sw_reset_ms = 55, + .self_test_ms = 16, +}; + +static const struct adis_timeout adis16448_timeouts = { + .reset_ms = 90, + .sw_reset_ms = 90, + .self_test_ms = 45, +}; + +static struct adis16400_chip_info adis16400_chips[] = { + [ADIS16300] = { + .channels = adis16300_channels, + .num_channels = ARRAY_SIZE(adis16300_channels), + .flags = ADIS16400_HAS_PROD_ID | ADIS16400_HAS_SLOW_MODE | + ADIS16400_HAS_SERIAL_NUMBER, + .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */ + .accel_scale_micro = 5884, + .temp_scale_nano = 140000000, /* 0.14 C */ + .temp_offset = 25000000 / 140000, /* 25 C = 0x00 */ + .set_freq = adis16400_set_freq, + .get_freq = adis16400_get_freq, + .adis_data = ADIS16400_DATA(&adis16300_timeouts, 18), + }, + [ADIS16334] = { + .channels = adis16334_channels, + .num_channels = ARRAY_SIZE(adis16334_channels), + .flags = ADIS16400_HAS_PROD_ID | ADIS16400_NO_BURST | + ADIS16400_HAS_SERIAL_NUMBER, + .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */ + .accel_scale_micro = IIO_G_TO_M_S_2(1000), /* 1 mg */ + .temp_scale_nano = 67850000, /* 0.06785 C */ + .temp_offset = 25000000 / 67850, /* 25 C = 0x00 */ + .set_freq = adis16334_set_freq, + .get_freq = adis16334_get_freq, + .adis_data = ADIS16400_DATA(&adis16334_timeouts, 0), + }, + [ADIS16350] = { + .channels = adis16350_channels, + .num_channels = ARRAY_SIZE(adis16350_channels), + .gyro_scale_micro = IIO_DEGREE_TO_RAD(73260), /* 0.07326 deg/s */ + .accel_scale_micro = IIO_G_TO_M_S_2(2522), /* 0.002522 g */ + .temp_scale_nano = 145300000, /* 0.1453 C */ + .temp_offset = 25000000 / 145300, /* 25 C = 0x00 */ + .flags = ADIS16400_NO_BURST | ADIS16400_HAS_SLOW_MODE, + .set_freq = adis16400_set_freq, + .get_freq = adis16400_get_freq, + .adis_data = ADIS16400_DATA(&adis16300_timeouts, 0), + }, + [ADIS16360] = { + .channels = adis16350_channels, + .num_channels = ARRAY_SIZE(adis16350_channels), + .flags = ADIS16400_HAS_PROD_ID | ADIS16400_HAS_SLOW_MODE | + ADIS16400_HAS_SERIAL_NUMBER, + .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */ + .accel_scale_micro = IIO_G_TO_M_S_2(3333), /* 3.333 mg */ + .temp_scale_nano = 136000000, /* 0.136 C */ + .temp_offset = 25000000 / 136000, /* 25 C = 0x00 */ + .set_freq = adis16400_set_freq, + .get_freq = adis16400_get_freq, + .adis_data = ADIS16400_DATA(&adis16300_timeouts, 28), + }, + [ADIS16362] = { + .channels = adis16350_channels, + .num_channels = ARRAY_SIZE(adis16350_channels), + .flags = ADIS16400_HAS_PROD_ID | ADIS16400_HAS_SLOW_MODE | + ADIS16400_HAS_SERIAL_NUMBER, + .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */ + .accel_scale_micro = IIO_G_TO_M_S_2(333), /* 0.333 mg */ + .temp_scale_nano = 136000000, /* 0.136 C */ + .temp_offset = 25000000 / 136000, /* 25 C = 0x00 */ + .set_freq = adis16400_set_freq, + .get_freq = adis16400_get_freq, + .adis_data = ADIS16400_DATA(&adis16362_timeouts, 28), + }, + [ADIS16364] = { + .channels = adis16350_channels, + .num_channels = ARRAY_SIZE(adis16350_channels), + .flags = ADIS16400_HAS_PROD_ID | ADIS16400_HAS_SLOW_MODE | + ADIS16400_HAS_SERIAL_NUMBER, + .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */ + .accel_scale_micro = IIO_G_TO_M_S_2(1000), /* 1 mg */ + .temp_scale_nano = 136000000, /* 0.136 C */ + .temp_offset = 25000000 / 136000, /* 25 C = 0x00 */ + .set_freq = adis16400_set_freq, + .get_freq = adis16400_get_freq, + .adis_data = ADIS16400_DATA(&adis16362_timeouts, 28), + }, + [ADIS16367] = { + .channels = adis16350_channels, + .num_channels = ARRAY_SIZE(adis16350_channels), + .flags = ADIS16400_HAS_PROD_ID | ADIS16400_HAS_SLOW_MODE | + ADIS16400_HAS_SERIAL_NUMBER, + .gyro_scale_micro = IIO_DEGREE_TO_RAD(2000), /* 0.2 deg/s */ + .accel_scale_micro = IIO_G_TO_M_S_2(3333), /* 3.333 mg */ + .temp_scale_nano = 136000000, /* 0.136 C */ + .temp_offset = 25000000 / 136000, /* 25 C = 0x00 */ + .set_freq = adis16400_set_freq, + .get_freq = adis16400_get_freq, + .adis_data = ADIS16400_DATA(&adis16300_timeouts, 28), + }, + [ADIS16400] = { + .channels = adis16400_channels, + .num_channels = ARRAY_SIZE(adis16400_channels), + .flags = ADIS16400_HAS_PROD_ID | ADIS16400_HAS_SLOW_MODE, + .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */ + .accel_scale_micro = IIO_G_TO_M_S_2(3333), /* 3.333 mg */ + .temp_scale_nano = 140000000, /* 0.14 C */ + .temp_offset = 25000000 / 140000, /* 25 C = 0x00 */ + .set_freq = adis16400_set_freq, + .get_freq = adis16400_get_freq, + .adis_data = ADIS16400_DATA(&adis16400_timeouts, 24), + }, + [ADIS16445] = { + .channels = adis16445_channels, + .num_channels = ARRAY_SIZE(adis16445_channels), + .flags = ADIS16400_HAS_PROD_ID | + ADIS16400_HAS_SERIAL_NUMBER | + ADIS16400_BURST_DIAG_STAT, + .gyro_scale_micro = IIO_DEGREE_TO_RAD(10000), /* 0.01 deg/s */ + .accel_scale_micro = IIO_G_TO_M_S_2(250), /* 1/4000 g */ + .temp_scale_nano = 73860000, /* 0.07386 C */ + .temp_offset = 31000000 / 73860, /* 31 C = 0x00 */ + .set_freq = adis16334_set_freq, + .get_freq = adis16334_get_freq, + .adis_data = ADIS16400_DATA(&adis16445_timeouts, 16), + }, + [ADIS16448] = { + .channels = adis16448_channels, + .num_channels = ARRAY_SIZE(adis16448_channels), + .flags = ADIS16400_HAS_PROD_ID | + ADIS16400_HAS_SERIAL_NUMBER | + ADIS16400_BURST_DIAG_STAT, + .gyro_scale_micro = IIO_DEGREE_TO_RAD(40000), /* 0.04 deg/s */ + .accel_scale_micro = IIO_G_TO_M_S_2(833), /* 1/1200 g */ + .temp_scale_nano = 73860000, /* 0.07386 C */ + .temp_offset = 31000000 / 73860, /* 31 C = 0x00 */ + .set_freq = adis16334_set_freq, + .get_freq = adis16334_get_freq, + .adis_data = ADIS16400_DATA(&adis16448_timeouts, 24), + } +}; + +static const struct iio_info adis16400_info = { + .read_raw = &adis16400_read_raw, + .write_raw = &adis16400_write_raw, + .update_scan_mode = adis_update_scan_mode, + .debugfs_reg_access = adis_debugfs_reg_access, +}; + +static void adis16400_setup_chan_mask(struct adis16400_state *st) +{ + const struct adis16400_chip_info *chip_info = st->variant; + unsigned int i; + + for (i = 0; i < chip_info->num_channels; i++) { + const struct iio_chan_spec *ch = &chip_info->channels[i]; + + if (ch->scan_index >= 0 && + ch->scan_index != ADIS16400_SCAN_TIMESTAMP) + st->avail_scan_mask[0] |= BIT(ch->scan_index); + } +} + +static void adis16400_stop(void *data) +{ + adis16400_stop_device(data); +} + +static int adis16400_probe(struct spi_device *spi) +{ + struct adis16400_state *st; + struct iio_dev *indio_dev; + int ret; + const struct adis_data *adis16400_data; + + indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st)); + if (indio_dev == NULL) + return -ENOMEM; + + st = iio_priv(indio_dev); + + /* setup the industrialio driver allocated elements */ + st->variant = &adis16400_chips[spi_get_device_id(spi)->driver_data]; + indio_dev->name = spi_get_device_id(spi)->name; + indio_dev->channels = st->variant->channels; + indio_dev->num_channels = st->variant->num_channels; + indio_dev->info = &adis16400_info; + indio_dev->modes = INDIO_DIRECT_MODE; + + if (!(st->variant->flags & ADIS16400_NO_BURST)) { + adis16400_setup_chan_mask(st); + indio_dev->available_scan_masks = st->avail_scan_mask; + } + + adis16400_data = &st->variant->adis_data; + + ret = adis_init(&st->adis, indio_dev, spi, adis16400_data); + if (ret) + return ret; + + ret = devm_adis_setup_buffer_and_trigger(&st->adis, indio_dev, adis16400_trigger_handler); + if (ret) + return ret; + + /* Get the device into a sane initial state */ + ret = adis16400_initial_setup(indio_dev); + if (ret) + return ret; + + ret = devm_add_action_or_reset(&spi->dev, adis16400_stop, indio_dev); + if (ret) + return ret; + + ret = devm_iio_device_register(&spi->dev, indio_dev); + if (ret) + return ret; + + adis16400_debugfs_init(indio_dev); + return 0; +} + +static const struct spi_device_id adis16400_id[] = { + {"adis16300", ADIS16300}, + {"adis16305", ADIS16300}, + {"adis16334", ADIS16334}, + {"adis16350", ADIS16350}, + {"adis16354", ADIS16350}, + {"adis16355", ADIS16350}, + {"adis16360", ADIS16360}, + {"adis16362", ADIS16362}, + {"adis16364", ADIS16364}, + {"adis16365", ADIS16360}, + {"adis16367", ADIS16367}, + {"adis16400", ADIS16400}, + {"adis16405", ADIS16400}, + {"adis16445", ADIS16445}, + {"adis16448", ADIS16448}, + {} +}; +MODULE_DEVICE_TABLE(spi, adis16400_id); + +static struct spi_driver adis16400_driver = { + .driver = { + .name = "adis16400", + }, + .id_table = adis16400_id, + .probe = adis16400_probe, +}; +module_spi_driver(adis16400_driver); + +MODULE_AUTHOR("Manuel Stahl "); +MODULE_DESCRIPTION("Analog Devices ADIS16400/5 IMU SPI driver"); +MODULE_LICENSE("GPL v2"); +MODULE_IMPORT_NS(IIO_ADISLIB); diff --git a/drivers/iio/imu/adis16460.c b/drivers/iio/imu/adis16460.c new file mode 100644 index 000000000..69facd72b --- /dev/null +++ b/drivers/iio/imu/adis16460.c @@ -0,0 +1,431 @@ +// SPDX-License-Identifier: GPL-2.0+ +/* + * ADIS16460 IMU driver + * + * Copyright 2019 Analog Devices Inc. + */ + +#include +#include + +#include +#include + +#include + +#define ADIS16460_REG_FLASH_CNT 0x00 +#define ADIS16460_REG_DIAG_STAT 0x02 +#define ADIS16460_REG_X_GYRO_LOW 0x04 +#define ADIS16460_REG_X_GYRO_OUT 0x06 +#define ADIS16460_REG_Y_GYRO_LOW 0x08 +#define ADIS16460_REG_Y_GYRO_OUT 0x0A +#define ADIS16460_REG_Z_GYRO_LOW 0x0C +#define ADIS16460_REG_Z_GYRO_OUT 0x0E +#define ADIS16460_REG_X_ACCL_LOW 0x10 +#define ADIS16460_REG_X_ACCL_OUT 0x12 +#define ADIS16460_REG_Y_ACCL_LOW 0x14 +#define ADIS16460_REG_Y_ACCL_OUT 0x16 +#define ADIS16460_REG_Z_ACCL_LOW 0x18 +#define ADIS16460_REG_Z_ACCL_OUT 0x1A +#define ADIS16460_REG_SMPL_CNTR 0x1C +#define ADIS16460_REG_TEMP_OUT 0x1E +#define ADIS16460_REG_X_DELT_ANG 0x24 +#define ADIS16460_REG_Y_DELT_ANG 0x26 +#define ADIS16460_REG_Z_DELT_ANG 0x28 +#define ADIS16460_REG_X_DELT_VEL 0x2A +#define ADIS16460_REG_Y_DELT_VEL 0x2C +#define ADIS16460_REG_Z_DELT_VEL 0x2E +#define ADIS16460_REG_MSC_CTRL 0x32 +#define ADIS16460_REG_SYNC_SCAL 0x34 +#define ADIS16460_REG_DEC_RATE 0x36 +#define ADIS16460_REG_FLTR_CTRL 0x38 +#define ADIS16460_REG_GLOB_CMD 0x3E +#define ADIS16460_REG_X_GYRO_OFF 0x40 +#define ADIS16460_REG_Y_GYRO_OFF 0x42 +#define ADIS16460_REG_Z_GYRO_OFF 0x44 +#define ADIS16460_REG_X_ACCL_OFF 0x46 +#define ADIS16460_REG_Y_ACCL_OFF 0x48 +#define ADIS16460_REG_Z_ACCL_OFF 0x4A +#define ADIS16460_REG_LOT_ID1 0x52 +#define ADIS16460_REG_LOT_ID2 0x54 +#define ADIS16460_REG_PROD_ID 0x56 +#define ADIS16460_REG_SERIAL_NUM 0x58 +#define ADIS16460_REG_CAL_SGNTR 0x60 +#define ADIS16460_REG_CAL_CRC 0x62 +#define ADIS16460_REG_CODE_SGNTR 0x64 +#define ADIS16460_REG_CODE_CRC 0x66 + +struct adis16460_chip_info { + unsigned int num_channels; + const struct iio_chan_spec *channels; + unsigned int gyro_max_val; + unsigned int gyro_max_scale; + unsigned int accel_max_val; + unsigned int accel_max_scale; +}; + +struct adis16460 { + const struct adis16460_chip_info *chip_info; + struct adis adis; +}; + +#ifdef CONFIG_DEBUG_FS + +static int adis16460_show_serial_number(void *arg, u64 *val) +{ + struct adis16460 *adis16460 = arg; + u16 serial; + int ret; + + ret = adis_read_reg_16(&adis16460->adis, ADIS16460_REG_SERIAL_NUM, + &serial); + if (ret) + return ret; + + *val = serial; + + return 0; +} +DEFINE_DEBUGFS_ATTRIBUTE(adis16460_serial_number_fops, + adis16460_show_serial_number, NULL, "0x%.4llx\n"); + +static int adis16460_show_product_id(void *arg, u64 *val) +{ + struct adis16460 *adis16460 = arg; + u16 prod_id; + int ret; + + ret = adis_read_reg_16(&adis16460->adis, ADIS16460_REG_PROD_ID, + &prod_id); + if (ret) + return ret; + + *val = prod_id; + + return 0; +} +DEFINE_DEBUGFS_ATTRIBUTE(adis16460_product_id_fops, + adis16460_show_product_id, NULL, "%llu\n"); + +static int adis16460_show_flash_count(void *arg, u64 *val) +{ + struct adis16460 *adis16460 = arg; + u32 flash_count; + int ret; + + ret = adis_read_reg_32(&adis16460->adis, ADIS16460_REG_FLASH_CNT, + &flash_count); + if (ret) + return ret; + + *val = flash_count; + + return 0; +} +DEFINE_DEBUGFS_ATTRIBUTE(adis16460_flash_count_fops, + adis16460_show_flash_count, NULL, "%lld\n"); + +static int adis16460_debugfs_init(struct iio_dev *indio_dev) +{ + struct adis16460 *adis16460 = iio_priv(indio_dev); + struct dentry *d = iio_get_debugfs_dentry(indio_dev); + + debugfs_create_file_unsafe("serial_number", 0400, + d, adis16460, &adis16460_serial_number_fops); + debugfs_create_file_unsafe("product_id", 0400, + d, adis16460, &adis16460_product_id_fops); + debugfs_create_file_unsafe("flash_count", 0400, + d, adis16460, &adis16460_flash_count_fops); + + return 0; +} + +#else + +static int adis16460_debugfs_init(struct iio_dev *indio_dev) +{ + return 0; +} + +#endif + +static int adis16460_set_freq(struct iio_dev *indio_dev, int val, int val2) +{ + struct adis16460 *st = iio_priv(indio_dev); + int t; + + t = val * 1000 + val2 / 1000; + if (t <= 0) + return -EINVAL; + + t = 2048000 / t; + if (t > 2048) + t = 2048; + + if (t != 0) + t--; + + return adis_write_reg_16(&st->adis, ADIS16460_REG_DEC_RATE, t); +} + +static int adis16460_get_freq(struct iio_dev *indio_dev, int *val, int *val2) +{ + struct adis16460 *st = iio_priv(indio_dev); + uint16_t t; + int ret; + unsigned int freq; + + ret = adis_read_reg_16(&st->adis, ADIS16460_REG_DEC_RATE, &t); + if (ret) + return ret; + + freq = 2048000 / (t + 1); + *val = freq / 1000; + *val2 = (freq % 1000) * 1000; + + return IIO_VAL_INT_PLUS_MICRO; +} + +static int adis16460_read_raw(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, int *val, int *val2, long info) +{ + struct adis16460 *st = iio_priv(indio_dev); + + switch (info) { + case IIO_CHAN_INFO_RAW: + return adis_single_conversion(indio_dev, chan, 0, val); + case IIO_CHAN_INFO_SCALE: + switch (chan->type) { + case IIO_ANGL_VEL: + *val = st->chip_info->gyro_max_scale; + *val2 = st->chip_info->gyro_max_val; + return IIO_VAL_FRACTIONAL; + case IIO_ACCEL: + *val = st->chip_info->accel_max_scale; + *val2 = st->chip_info->accel_max_val; + return IIO_VAL_FRACTIONAL; + case IIO_TEMP: + *val = 50; /* 50 milli degrees Celsius/LSB */ + return IIO_VAL_INT; + default: + return -EINVAL; + } + case IIO_CHAN_INFO_OFFSET: + *val = 500; /* 25 degrees Celsius = 0x0000 */ + return IIO_VAL_INT; + case IIO_CHAN_INFO_SAMP_FREQ: + return adis16460_get_freq(indio_dev, val, val2); + default: + return -EINVAL; + } +} + +static int adis16460_write_raw(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, int val, int val2, long info) +{ + switch (info) { + case IIO_CHAN_INFO_SAMP_FREQ: + return adis16460_set_freq(indio_dev, val, val2); + default: + return -EINVAL; + } +} + +enum { + ADIS16460_SCAN_GYRO_X, + ADIS16460_SCAN_GYRO_Y, + ADIS16460_SCAN_GYRO_Z, + ADIS16460_SCAN_ACCEL_X, + ADIS16460_SCAN_ACCEL_Y, + ADIS16460_SCAN_ACCEL_Z, + ADIS16460_SCAN_TEMP, +}; + +#define ADIS16460_MOD_CHANNEL(_type, _mod, _address, _si, _bits) \ + { \ + .type = (_type), \ + .modified = 1, \ + .channel2 = (_mod), \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \ + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .address = (_address), \ + .scan_index = (_si), \ + .scan_type = { \ + .sign = 's', \ + .realbits = (_bits), \ + .storagebits = (_bits), \ + .endianness = IIO_BE, \ + }, \ + } + +#define ADIS16460_GYRO_CHANNEL(_mod) \ + ADIS16460_MOD_CHANNEL(IIO_ANGL_VEL, IIO_MOD_ ## _mod, \ + ADIS16460_REG_ ## _mod ## _GYRO_LOW, ADIS16460_SCAN_GYRO_ ## _mod, \ + 32) + +#define ADIS16460_ACCEL_CHANNEL(_mod) \ + ADIS16460_MOD_CHANNEL(IIO_ACCEL, IIO_MOD_ ## _mod, \ + ADIS16460_REG_ ## _mod ## _ACCL_LOW, ADIS16460_SCAN_ACCEL_ ## _mod, \ + 32) + +#define ADIS16460_TEMP_CHANNEL() { \ + .type = IIO_TEMP, \ + .indexed = 1, \ + .channel = 0, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE) | \ + BIT(IIO_CHAN_INFO_OFFSET), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .address = ADIS16460_REG_TEMP_OUT, \ + .scan_index = ADIS16460_SCAN_TEMP, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_BE, \ + }, \ + } + +static const struct iio_chan_spec adis16460_channels[] = { + ADIS16460_GYRO_CHANNEL(X), + ADIS16460_GYRO_CHANNEL(Y), + ADIS16460_GYRO_CHANNEL(Z), + ADIS16460_ACCEL_CHANNEL(X), + ADIS16460_ACCEL_CHANNEL(Y), + ADIS16460_ACCEL_CHANNEL(Z), + ADIS16460_TEMP_CHANNEL(), + IIO_CHAN_SOFT_TIMESTAMP(7) +}; + +static const struct adis16460_chip_info adis16460_chip_info = { + .channels = adis16460_channels, + .num_channels = ARRAY_SIZE(adis16460_channels), + /* + * storing the value in rad/degree and the scale in degree + * gives us the result in rad and better precession than + * storing the scale directly in rad. + */ + .gyro_max_val = IIO_RAD_TO_DEGREE(200 << 16), + .gyro_max_scale = 1, + .accel_max_val = IIO_M_S_2_TO_G(20000 << 16), + .accel_max_scale = 5, +}; + +static const struct iio_info adis16460_info = { + .read_raw = &adis16460_read_raw, + .write_raw = &adis16460_write_raw, + .update_scan_mode = adis_update_scan_mode, + .debugfs_reg_access = adis_debugfs_reg_access, +}; + +#define ADIS16460_DIAG_STAT_IN_CLK_OOS 7 +#define ADIS16460_DIAG_STAT_FLASH_MEM 6 +#define ADIS16460_DIAG_STAT_SELF_TEST 5 +#define ADIS16460_DIAG_STAT_OVERRANGE 4 +#define ADIS16460_DIAG_STAT_SPI_COMM 3 +#define ADIS16460_DIAG_STAT_FLASH_UPT 2 + +static const char * const adis16460_status_error_msgs[] = { + [ADIS16460_DIAG_STAT_IN_CLK_OOS] = "Input clock out of sync", + [ADIS16460_DIAG_STAT_FLASH_MEM] = "Flash memory failure", + [ADIS16460_DIAG_STAT_SELF_TEST] = "Self test diagnostic failure", + [ADIS16460_DIAG_STAT_OVERRANGE] = "Sensor overrange", + [ADIS16460_DIAG_STAT_SPI_COMM] = "SPI communication failure", + [ADIS16460_DIAG_STAT_FLASH_UPT] = "Flash update failure", +}; + +static const struct adis_timeout adis16460_timeouts = { + .reset_ms = 225, + .sw_reset_ms = 225, + .self_test_ms = 10, +}; + +static const struct adis_data adis16460_data = { + .diag_stat_reg = ADIS16460_REG_DIAG_STAT, + .glob_cmd_reg = ADIS16460_REG_GLOB_CMD, + .prod_id_reg = ADIS16460_REG_PROD_ID, + .prod_id = 16460, + .self_test_mask = BIT(2), + .self_test_reg = ADIS16460_REG_GLOB_CMD, + .has_paging = false, + .read_delay = 5, + .write_delay = 5, + .cs_change_delay = 16, + .status_error_msgs = adis16460_status_error_msgs, + .status_error_mask = BIT(ADIS16460_DIAG_STAT_IN_CLK_OOS) | + BIT(ADIS16460_DIAG_STAT_FLASH_MEM) | + BIT(ADIS16460_DIAG_STAT_SELF_TEST) | + BIT(ADIS16460_DIAG_STAT_OVERRANGE) | + BIT(ADIS16460_DIAG_STAT_SPI_COMM) | + BIT(ADIS16460_DIAG_STAT_FLASH_UPT), + .unmasked_drdy = true, + .timeouts = &adis16460_timeouts, +}; + +static int adis16460_probe(struct spi_device *spi) +{ + struct iio_dev *indio_dev; + struct adis16460 *st; + int ret; + + indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st)); + if (indio_dev == NULL) + return -ENOMEM; + + st = iio_priv(indio_dev); + + st->chip_info = &adis16460_chip_info; + indio_dev->name = spi_get_device_id(spi)->name; + indio_dev->channels = st->chip_info->channels; + indio_dev->num_channels = st->chip_info->num_channels; + indio_dev->info = &adis16460_info; + indio_dev->modes = INDIO_DIRECT_MODE; + + ret = adis_init(&st->adis, indio_dev, spi, &adis16460_data); + if (ret) + return ret; + + ret = devm_adis_setup_buffer_and_trigger(&st->adis, indio_dev, NULL); + if (ret) + return ret; + + ret = __adis_initial_startup(&st->adis); + if (ret) + return ret; + + ret = devm_iio_device_register(&spi->dev, indio_dev); + if (ret) + return ret; + + adis16460_debugfs_init(indio_dev); + + return 0; +} + +static const struct spi_device_id adis16460_ids[] = { + { "adis16460", 0 }, + {} +}; +MODULE_DEVICE_TABLE(spi, adis16460_ids); + +static const struct of_device_id adis16460_of_match[] = { + { .compatible = "adi,adis16460" }, + {} +}; +MODULE_DEVICE_TABLE(of, adis16460_of_match); + +static struct spi_driver adis16460_driver = { + .driver = { + .name = "adis16460", + .of_match_table = adis16460_of_match, + }, + .id_table = adis16460_ids, + .probe = adis16460_probe, +}; +module_spi_driver(adis16460_driver); + +MODULE_AUTHOR("Dragos Bogdan "); +MODULE_DESCRIPTION("Analog Devices ADIS16460 IMU driver"); +MODULE_LICENSE("GPL"); +MODULE_IMPORT_NS(IIO_ADISLIB); diff --git a/drivers/iio/imu/adis16475.c b/drivers/iio/imu/adis16475.c new file mode 100644 index 000000000..2d9397734 --- /dev/null +++ b/drivers/iio/imu/adis16475.c @@ -0,0 +1,1382 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * ADIS16475 IMU driver + * + * Copyright 2019 Analog Devices Inc. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define ADIS16475_REG_DIAG_STAT 0x02 +#define ADIS16475_REG_X_GYRO_L 0x04 +#define ADIS16475_REG_Y_GYRO_L 0x08 +#define ADIS16475_REG_Z_GYRO_L 0x0C +#define ADIS16475_REG_X_ACCEL_L 0x10 +#define ADIS16475_REG_Y_ACCEL_L 0x14 +#define ADIS16475_REG_Z_ACCEL_L 0x18 +#define ADIS16475_REG_TEMP_OUT 0x1c +#define ADIS16475_REG_X_GYRO_BIAS_L 0x40 +#define ADIS16475_REG_Y_GYRO_BIAS_L 0x44 +#define ADIS16475_REG_Z_GYRO_BIAS_L 0x48 +#define ADIS16475_REG_X_ACCEL_BIAS_L 0x4c +#define ADIS16475_REG_Y_ACCEL_BIAS_L 0x50 +#define ADIS16475_REG_Z_ACCEL_BIAS_L 0x54 +#define ADIS16475_REG_FILT_CTRL 0x5c +#define ADIS16475_FILT_CTRL_MASK GENMASK(2, 0) +#define ADIS16475_FILT_CTRL(x) FIELD_PREP(ADIS16475_FILT_CTRL_MASK, x) +#define ADIS16475_REG_MSG_CTRL 0x60 +#define ADIS16475_MSG_CTRL_DR_POL_MASK BIT(0) +#define ADIS16475_MSG_CTRL_DR_POL(x) \ + FIELD_PREP(ADIS16475_MSG_CTRL_DR_POL_MASK, x) +#define ADIS16475_SYNC_MODE_MASK GENMASK(4, 2) +#define ADIS16475_SYNC_MODE(x) FIELD_PREP(ADIS16475_SYNC_MODE_MASK, x) +#define ADIS16475_REG_UP_SCALE 0x62 +#define ADIS16475_REG_DEC_RATE 0x64 +#define ADIS16475_REG_GLOB_CMD 0x68 +#define ADIS16475_REG_FIRM_REV 0x6c +#define ADIS16475_REG_FIRM_DM 0x6e +#define ADIS16475_REG_FIRM_Y 0x70 +#define ADIS16475_REG_PROD_ID 0x72 +#define ADIS16475_REG_SERIAL_NUM 0x74 +#define ADIS16475_REG_FLASH_CNT 0x7c +#define ADIS16500_BURST32_MASK BIT(9) +#define ADIS16500_BURST32(x) FIELD_PREP(ADIS16500_BURST32_MASK, x) +/* number of data elements in burst mode */ +#define ADIS16475_BURST32_MAX_DATA 32 +#define ADIS16475_BURST_MAX_DATA 20 +#define ADIS16475_MAX_SCAN_DATA 20 +/* spi max speed in brust mode */ +#define ADIS16475_BURST_MAX_SPEED 1000000 +#define ADIS16475_LSB_DEC_MASK BIT(0) +#define ADIS16475_LSB_FIR_MASK BIT(1) + +enum { + ADIS16475_SYNC_DIRECT = 1, + ADIS16475_SYNC_SCALED, + ADIS16475_SYNC_OUTPUT, + ADIS16475_SYNC_PULSE = 5, +}; + +struct adis16475_sync { + u16 sync_mode; + u16 min_rate; + u16 max_rate; +}; + +struct adis16475_chip_info { + const struct iio_chan_spec *channels; + const struct adis16475_sync *sync; + const struct adis_data adis_data; + const char *name; + u32 num_channels; + u32 gyro_max_val; + u32 gyro_max_scale; + u32 accel_max_val; + u32 accel_max_scale; + u32 temp_scale; + u32 int_clk; + u16 max_dec; + u8 num_sync; + bool has_burst32; +}; + +struct adis16475 { + const struct adis16475_chip_info *info; + struct adis adis; + u32 clk_freq; + bool burst32; + unsigned long lsb_flag; + u16 sync_mode; + /* Alignment needed for the timestamp */ + __be16 data[ADIS16475_MAX_SCAN_DATA] __aligned(8); +}; + +enum { + ADIS16475_SCAN_GYRO_X, + ADIS16475_SCAN_GYRO_Y, + ADIS16475_SCAN_GYRO_Z, + ADIS16475_SCAN_ACCEL_X, + ADIS16475_SCAN_ACCEL_Y, + ADIS16475_SCAN_ACCEL_Z, + ADIS16475_SCAN_TEMP, + ADIS16475_SCAN_DIAG_S_FLAGS, + ADIS16475_SCAN_CRC_FAILURE, +}; + +static bool low_rate_allow; +module_param(low_rate_allow, bool, 0444); +MODULE_PARM_DESC(low_rate_allow, + "Allow IMU rates below the minimum advisable when external clk is used in SCALED mode (default: N)"); + +#ifdef CONFIG_DEBUG_FS +static ssize_t adis16475_show_firmware_revision(struct file *file, + char __user *userbuf, + size_t count, loff_t *ppos) +{ + struct adis16475 *st = file->private_data; + char buf[7]; + size_t len; + u16 rev; + int ret; + + ret = adis_read_reg_16(&st->adis, ADIS16475_REG_FIRM_REV, &rev); + if (ret) + return ret; + + len = scnprintf(buf, sizeof(buf), "%x.%x\n", rev >> 8, rev & 0xff); + + return simple_read_from_buffer(userbuf, count, ppos, buf, len); +} + +static const struct file_operations adis16475_firmware_revision_fops = { + .open = simple_open, + .read = adis16475_show_firmware_revision, + .llseek = default_llseek, + .owner = THIS_MODULE, +}; + +static ssize_t adis16475_show_firmware_date(struct file *file, + char __user *userbuf, + size_t count, loff_t *ppos) +{ + struct adis16475 *st = file->private_data; + u16 md, year; + char buf[12]; + size_t len; + int ret; + + ret = adis_read_reg_16(&st->adis, ADIS16475_REG_FIRM_Y, &year); + if (ret) + return ret; + + ret = adis_read_reg_16(&st->adis, ADIS16475_REG_FIRM_DM, &md); + if (ret) + return ret; + + len = snprintf(buf, sizeof(buf), "%.2x-%.2x-%.4x\n", md >> 8, md & 0xff, + year); + + return simple_read_from_buffer(userbuf, count, ppos, buf, len); +} + +static const struct file_operations adis16475_firmware_date_fops = { + .open = simple_open, + .read = adis16475_show_firmware_date, + .llseek = default_llseek, + .owner = THIS_MODULE, +}; + +static int adis16475_show_serial_number(void *arg, u64 *val) +{ + struct adis16475 *st = arg; + u16 serial; + int ret; + + ret = adis_read_reg_16(&st->adis, ADIS16475_REG_SERIAL_NUM, &serial); + if (ret) + return ret; + + *val = serial; + + return 0; +} +DEFINE_DEBUGFS_ATTRIBUTE(adis16475_serial_number_fops, + adis16475_show_serial_number, NULL, "0x%.4llx\n"); + +static int adis16475_show_product_id(void *arg, u64 *val) +{ + struct adis16475 *st = arg; + u16 prod_id; + int ret; + + ret = adis_read_reg_16(&st->adis, ADIS16475_REG_PROD_ID, &prod_id); + if (ret) + return ret; + + *val = prod_id; + + return 0; +} +DEFINE_DEBUGFS_ATTRIBUTE(adis16475_product_id_fops, + adis16475_show_product_id, NULL, "%llu\n"); + +static int adis16475_show_flash_count(void *arg, u64 *val) +{ + struct adis16475 *st = arg; + u32 flash_count; + int ret; + + ret = adis_read_reg_32(&st->adis, ADIS16475_REG_FLASH_CNT, + &flash_count); + if (ret) + return ret; + + *val = flash_count; + + return 0; +} +DEFINE_DEBUGFS_ATTRIBUTE(adis16475_flash_count_fops, + adis16475_show_flash_count, NULL, "%lld\n"); + +static void adis16475_debugfs_init(struct iio_dev *indio_dev) +{ + struct adis16475 *st = iio_priv(indio_dev); + struct dentry *d = iio_get_debugfs_dentry(indio_dev); + + debugfs_create_file_unsafe("serial_number", 0400, + d, st, &adis16475_serial_number_fops); + debugfs_create_file_unsafe("product_id", 0400, + d, st, &adis16475_product_id_fops); + debugfs_create_file_unsafe("flash_count", 0400, + d, st, &adis16475_flash_count_fops); + debugfs_create_file("firmware_revision", 0400, + d, st, &adis16475_firmware_revision_fops); + debugfs_create_file("firmware_date", 0400, d, + st, &adis16475_firmware_date_fops); +} +#else +static void adis16475_debugfs_init(struct iio_dev *indio_dev) +{ +} +#endif + +static int adis16475_get_freq(struct adis16475 *st, u32 *freq) +{ + int ret; + u16 dec; + u32 sample_rate = st->clk_freq; + + adis_dev_lock(&st->adis); + + if (st->sync_mode == ADIS16475_SYNC_SCALED) { + u16 sync_scale; + + ret = __adis_read_reg_16(&st->adis, ADIS16475_REG_UP_SCALE, &sync_scale); + if (ret) + goto error; + + sample_rate = st->clk_freq * sync_scale; + } + + ret = __adis_read_reg_16(&st->adis, ADIS16475_REG_DEC_RATE, &dec); + if (ret) + goto error; + + adis_dev_unlock(&st->adis); + + *freq = DIV_ROUND_CLOSEST(sample_rate, dec + 1); + + return 0; +error: + adis_dev_unlock(&st->adis); + return ret; +} + +static int adis16475_set_freq(struct adis16475 *st, const u32 freq) +{ + u16 dec; + int ret; + u32 sample_rate = st->clk_freq; + + if (!freq) + return -EINVAL; + + adis_dev_lock(&st->adis); + /* + * When using sync scaled mode, the input clock needs to be scaled so that we have + * an IMU sample rate between (optimally) 1900 and 2100. After this, we can use the + * decimation filter to lower the sampling rate in order to get what the user wants. + * Optimally, the user sample rate is a multiple of both the IMU sample rate and + * the input clock. Hence, calculating the sync_scale dynamically gives us better + * chances of achieving a perfect/integer value for DEC_RATE. The math here is: + * 1. lcm of the input clock and the desired output rate. + * 2. get the highest multiple of the previous result lower than the adis max rate. + * 3. The last result becomes the IMU sample rate. Use that to calculate SYNC_SCALE + * and DEC_RATE (to get the user output rate) + */ + if (st->sync_mode == ADIS16475_SYNC_SCALED) { + unsigned long scaled_rate = lcm(st->clk_freq, freq); + int sync_scale; + + /* + * If lcm is bigger than the IMU maximum sampling rate there's no perfect + * solution. In this case, we get the highest multiple of the input clock + * lower than the IMU max sample rate. + */ + if (scaled_rate > 2100000) + scaled_rate = 2100000 / st->clk_freq * st->clk_freq; + else + scaled_rate = 2100000 / scaled_rate * scaled_rate; + + /* + * This is not an hard requirement but it's not advised to run the IMU + * with a sample rate lower than 4000Hz due to possible undersampling + * issues. However, there are users that might really want to take the risk. + * Hence, we provide a module parameter for them. If set, we allow sample + * rates lower than 4KHz. By default, we won't allow this and we just roundup + * the rate to the next multiple of the input clock bigger than 4KHz. This + * is done like this as in some cases (when DEC_RATE is 0) might give + * us the closest value to the one desired by the user... + */ + if (scaled_rate < 1900000 && !low_rate_allow) + scaled_rate = roundup(1900000, st->clk_freq); + + sync_scale = scaled_rate / st->clk_freq; + ret = __adis_write_reg_16(&st->adis, ADIS16475_REG_UP_SCALE, sync_scale); + if (ret) + goto error; + + sample_rate = scaled_rate; + } + + dec = DIV_ROUND_CLOSEST(sample_rate, freq); + + if (dec) + dec--; + + if (dec > st->info->max_dec) + dec = st->info->max_dec; + + ret = __adis_write_reg_16(&st->adis, ADIS16475_REG_DEC_RATE, dec); + if (ret) + goto error; + + adis_dev_unlock(&st->adis); + /* + * If decimation is used, then gyro and accel data will have meaningful + * bits on the LSB registers. This info is used on the trigger handler. + */ + assign_bit(ADIS16475_LSB_DEC_MASK, &st->lsb_flag, dec); + + return 0; +error: + adis_dev_unlock(&st->adis); + return ret; +} + +/* The values are approximated. */ +static const u32 adis16475_3db_freqs[] = { + [0] = 720, /* Filter disabled, full BW (~720Hz) */ + [1] = 360, + [2] = 164, + [3] = 80, + [4] = 40, + [5] = 20, + [6] = 10, +}; + +static int adis16475_get_filter(struct adis16475 *st, u32 *filter) +{ + u16 filter_sz; + int ret; + const int mask = ADIS16475_FILT_CTRL_MASK; + + ret = adis_read_reg_16(&st->adis, ADIS16475_REG_FILT_CTRL, &filter_sz); + if (ret) + return ret; + + *filter = adis16475_3db_freqs[filter_sz & mask]; + + return 0; +} + +static int adis16475_set_filter(struct adis16475 *st, const u32 filter) +{ + int i = ARRAY_SIZE(adis16475_3db_freqs); + int ret; + + while (--i) { + if (adis16475_3db_freqs[i] >= filter) + break; + } + + ret = adis_write_reg_16(&st->adis, ADIS16475_REG_FILT_CTRL, + ADIS16475_FILT_CTRL(i)); + if (ret) + return ret; + + /* + * If FIR is used, then gyro and accel data will have meaningful + * bits on the LSB registers. This info is used on the trigger handler. + */ + assign_bit(ADIS16475_LSB_FIR_MASK, &st->lsb_flag, i); + + return 0; +} + +static const u32 adis16475_calib_regs[] = { + [ADIS16475_SCAN_GYRO_X] = ADIS16475_REG_X_GYRO_BIAS_L, + [ADIS16475_SCAN_GYRO_Y] = ADIS16475_REG_Y_GYRO_BIAS_L, + [ADIS16475_SCAN_GYRO_Z] = ADIS16475_REG_Z_GYRO_BIAS_L, + [ADIS16475_SCAN_ACCEL_X] = ADIS16475_REG_X_ACCEL_BIAS_L, + [ADIS16475_SCAN_ACCEL_Y] = ADIS16475_REG_Y_ACCEL_BIAS_L, + [ADIS16475_SCAN_ACCEL_Z] = ADIS16475_REG_Z_ACCEL_BIAS_L, +}; + +static int adis16475_read_raw(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, + int *val, int *val2, long info) +{ + struct adis16475 *st = iio_priv(indio_dev); + int ret; + u32 tmp; + + switch (info) { + case IIO_CHAN_INFO_RAW: + return adis_single_conversion(indio_dev, chan, 0, val); + case IIO_CHAN_INFO_SCALE: + switch (chan->type) { + case IIO_ANGL_VEL: + *val = st->info->gyro_max_val; + *val2 = st->info->gyro_max_scale; + return IIO_VAL_FRACTIONAL; + case IIO_ACCEL: + *val = st->info->accel_max_val; + *val2 = st->info->accel_max_scale; + return IIO_VAL_FRACTIONAL; + case IIO_TEMP: + *val = st->info->temp_scale; + return IIO_VAL_INT; + default: + return -EINVAL; + } + case IIO_CHAN_INFO_CALIBBIAS: + ret = adis_read_reg_32(&st->adis, + adis16475_calib_regs[chan->scan_index], + val); + if (ret) + return ret; + + return IIO_VAL_INT; + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: + ret = adis16475_get_filter(st, val); + if (ret) + return ret; + + return IIO_VAL_INT; + case IIO_CHAN_INFO_SAMP_FREQ: + ret = adis16475_get_freq(st, &tmp); + if (ret) + return ret; + + *val = tmp / 1000; + *val2 = (tmp % 1000) * 1000; + return IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } +} + +static int adis16475_write_raw(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, + int val, int val2, long info) +{ + struct adis16475 *st = iio_priv(indio_dev); + u32 tmp; + + switch (info) { + case IIO_CHAN_INFO_SAMP_FREQ: + tmp = val * 1000 + val2 / 1000; + return adis16475_set_freq(st, tmp); + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: + return adis16475_set_filter(st, val); + case IIO_CHAN_INFO_CALIBBIAS: + return adis_write_reg_32(&st->adis, + adis16475_calib_regs[chan->scan_index], + val); + default: + return -EINVAL; + } +} + +#define ADIS16475_MOD_CHAN(_type, _mod, _address, _si, _r_bits, _s_bits) \ + { \ + .type = (_type), \ + .modified = 1, \ + .channel2 = (_mod), \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_CALIBBIAS), \ + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ) | \ + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), \ + .address = (_address), \ + .scan_index = (_si), \ + .scan_type = { \ + .sign = 's', \ + .realbits = (_r_bits), \ + .storagebits = (_s_bits), \ + .endianness = IIO_BE, \ + }, \ + } + +#define ADIS16475_GYRO_CHANNEL(_mod) \ + ADIS16475_MOD_CHAN(IIO_ANGL_VEL, IIO_MOD_ ## _mod, \ + ADIS16475_REG_ ## _mod ## _GYRO_L, \ + ADIS16475_SCAN_GYRO_ ## _mod, 32, 32) + +#define ADIS16475_ACCEL_CHANNEL(_mod) \ + ADIS16475_MOD_CHAN(IIO_ACCEL, IIO_MOD_ ## _mod, \ + ADIS16475_REG_ ## _mod ## _ACCEL_L, \ + ADIS16475_SCAN_ACCEL_ ## _mod, 32, 32) + +#define ADIS16475_TEMP_CHANNEL() { \ + .type = IIO_TEMP, \ + .indexed = 1, \ + .channel = 0, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ) | \ + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), \ + .address = ADIS16475_REG_TEMP_OUT, \ + .scan_index = ADIS16475_SCAN_TEMP, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_BE, \ + }, \ + } + +static const struct iio_chan_spec adis16475_channels[] = { + ADIS16475_GYRO_CHANNEL(X), + ADIS16475_GYRO_CHANNEL(Y), + ADIS16475_GYRO_CHANNEL(Z), + ADIS16475_ACCEL_CHANNEL(X), + ADIS16475_ACCEL_CHANNEL(Y), + ADIS16475_ACCEL_CHANNEL(Z), + ADIS16475_TEMP_CHANNEL(), + IIO_CHAN_SOFT_TIMESTAMP(7) +}; + +enum adis16475_variant { + ADIS16470, + ADIS16475_1, + ADIS16475_2, + ADIS16475_3, + ADIS16477_1, + ADIS16477_2, + ADIS16477_3, + ADIS16465_1, + ADIS16465_2, + ADIS16465_3, + ADIS16467_1, + ADIS16467_2, + ADIS16467_3, + ADIS16500, + ADIS16505_1, + ADIS16505_2, + ADIS16505_3, + ADIS16507_1, + ADIS16507_2, + ADIS16507_3, +}; + +enum { + ADIS16475_DIAG_STAT_DATA_PATH = 1, + ADIS16475_DIAG_STAT_FLASH_MEM, + ADIS16475_DIAG_STAT_SPI, + ADIS16475_DIAG_STAT_STANDBY, + ADIS16475_DIAG_STAT_SENSOR, + ADIS16475_DIAG_STAT_MEMORY, + ADIS16475_DIAG_STAT_CLK, +}; + +static const char * const adis16475_status_error_msgs[] = { + [ADIS16475_DIAG_STAT_DATA_PATH] = "Data Path Overrun", + [ADIS16475_DIAG_STAT_FLASH_MEM] = "Flash memory update failure", + [ADIS16475_DIAG_STAT_SPI] = "SPI communication error", + [ADIS16475_DIAG_STAT_STANDBY] = "Standby mode", + [ADIS16475_DIAG_STAT_SENSOR] = "Sensor failure", + [ADIS16475_DIAG_STAT_MEMORY] = "Memory failure", + [ADIS16475_DIAG_STAT_CLK] = "Clock error", +}; + +#define ADIS16475_DATA(_prod_id, _timeouts) \ +{ \ + .msc_ctrl_reg = ADIS16475_REG_MSG_CTRL, \ + .glob_cmd_reg = ADIS16475_REG_GLOB_CMD, \ + .diag_stat_reg = ADIS16475_REG_DIAG_STAT, \ + .prod_id_reg = ADIS16475_REG_PROD_ID, \ + .prod_id = (_prod_id), \ + .self_test_mask = BIT(2), \ + .self_test_reg = ADIS16475_REG_GLOB_CMD, \ + .cs_change_delay = 16, \ + .read_delay = 5, \ + .write_delay = 5, \ + .status_error_msgs = adis16475_status_error_msgs, \ + .status_error_mask = BIT(ADIS16475_DIAG_STAT_DATA_PATH) | \ + BIT(ADIS16475_DIAG_STAT_FLASH_MEM) | \ + BIT(ADIS16475_DIAG_STAT_SPI) | \ + BIT(ADIS16475_DIAG_STAT_STANDBY) | \ + BIT(ADIS16475_DIAG_STAT_SENSOR) | \ + BIT(ADIS16475_DIAG_STAT_MEMORY) | \ + BIT(ADIS16475_DIAG_STAT_CLK), \ + .unmasked_drdy = true, \ + .timeouts = (_timeouts), \ + .burst_reg_cmd = ADIS16475_REG_GLOB_CMD, \ + .burst_len = ADIS16475_BURST_MAX_DATA, \ + .burst_max_len = ADIS16475_BURST32_MAX_DATA, \ + .burst_max_speed_hz = ADIS16475_BURST_MAX_SPEED \ +} + +static const struct adis16475_sync adis16475_sync_mode[] = { + { ADIS16475_SYNC_OUTPUT }, + { ADIS16475_SYNC_DIRECT, 1900, 2100 }, + { ADIS16475_SYNC_SCALED, 1, 128 }, + { ADIS16475_SYNC_PULSE, 1000, 2100 }, +}; + +static const struct adis_timeout adis16475_timeouts = { + .reset_ms = 200, + .sw_reset_ms = 200, + .self_test_ms = 20, +}; + +static const struct adis_timeout adis1650x_timeouts = { + .reset_ms = 260, + .sw_reset_ms = 260, + .self_test_ms = 30, +}; + +static const struct adis16475_chip_info adis16475_chip_info[] = { + [ADIS16470] = { + .name = "adis16470", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(10 << 16), + .accel_max_val = 1, + .accel_max_scale = IIO_M_S_2_TO_G(800 << 16), + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + .num_sync = ARRAY_SIZE(adis16475_sync_mode), + .adis_data = ADIS16475_DATA(16470, &adis16475_timeouts), + }, + [ADIS16475_1] = { + .name = "adis16475-1", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(160 << 16), + .accel_max_val = 1, + .accel_max_scale = IIO_M_S_2_TO_G(4000 << 16), + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + .num_sync = ARRAY_SIZE(adis16475_sync_mode), + .adis_data = ADIS16475_DATA(16475, &adis16475_timeouts), + }, + [ADIS16475_2] = { + .name = "adis16475-2", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(40 << 16), + .accel_max_val = 1, + .accel_max_scale = IIO_M_S_2_TO_G(4000 << 16), + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + .num_sync = ARRAY_SIZE(adis16475_sync_mode), + .adis_data = ADIS16475_DATA(16475, &adis16475_timeouts), + }, + [ADIS16475_3] = { + .name = "adis16475-3", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(10 << 16), + .accel_max_val = 1, + .accel_max_scale = IIO_M_S_2_TO_G(4000 << 16), + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + .num_sync = ARRAY_SIZE(adis16475_sync_mode), + .adis_data = ADIS16475_DATA(16475, &adis16475_timeouts), + }, + [ADIS16477_1] = { + .name = "adis16477-1", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(160 << 16), + .accel_max_val = 1, + .accel_max_scale = IIO_M_S_2_TO_G(800 << 16), + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + .num_sync = ARRAY_SIZE(adis16475_sync_mode), + .adis_data = ADIS16475_DATA(16477, &adis16475_timeouts), + }, + [ADIS16477_2] = { + .name = "adis16477-2", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(40 << 16), + .accel_max_val = 1, + .accel_max_scale = IIO_M_S_2_TO_G(800 << 16), + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + .num_sync = ARRAY_SIZE(adis16475_sync_mode), + .adis_data = ADIS16475_DATA(16477, &adis16475_timeouts), + }, + [ADIS16477_3] = { + .name = "adis16477-3", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(10 << 16), + .accel_max_val = 1, + .accel_max_scale = IIO_M_S_2_TO_G(800 << 16), + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + .num_sync = ARRAY_SIZE(adis16475_sync_mode), + .adis_data = ADIS16475_DATA(16477, &adis16475_timeouts), + }, + [ADIS16465_1] = { + .name = "adis16465-1", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(160 << 16), + .accel_max_val = 1, + .accel_max_scale = IIO_M_S_2_TO_G(4000 << 16), + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + .num_sync = ARRAY_SIZE(adis16475_sync_mode), + .adis_data = ADIS16475_DATA(16465, &adis16475_timeouts), + }, + [ADIS16465_2] = { + .name = "adis16465-2", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(40 << 16), + .accel_max_val = 1, + .accel_max_scale = IIO_M_S_2_TO_G(4000 << 16), + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + .num_sync = ARRAY_SIZE(adis16475_sync_mode), + .adis_data = ADIS16475_DATA(16465, &adis16475_timeouts), + }, + [ADIS16465_3] = { + .name = "adis16465-3", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(10 << 16), + .accel_max_val = 1, + .accel_max_scale = IIO_M_S_2_TO_G(4000 << 16), + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + .num_sync = ARRAY_SIZE(adis16475_sync_mode), + .adis_data = ADIS16475_DATA(16465, &adis16475_timeouts), + }, + [ADIS16467_1] = { + .name = "adis16467-1", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(160 << 16), + .accel_max_val = 1, + .accel_max_scale = IIO_M_S_2_TO_G(800 << 16), + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + .num_sync = ARRAY_SIZE(adis16475_sync_mode), + .adis_data = ADIS16475_DATA(16467, &adis16475_timeouts), + }, + [ADIS16467_2] = { + .name = "adis16467-2", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(40 << 16), + .accel_max_val = 1, + .accel_max_scale = IIO_M_S_2_TO_G(800 << 16), + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + .num_sync = ARRAY_SIZE(adis16475_sync_mode), + .adis_data = ADIS16475_DATA(16467, &adis16475_timeouts), + }, + [ADIS16467_3] = { + .name = "adis16467-3", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(10 << 16), + .accel_max_val = 1, + .accel_max_scale = IIO_M_S_2_TO_G(800 << 16), + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + .num_sync = ARRAY_SIZE(adis16475_sync_mode), + .adis_data = ADIS16475_DATA(16467, &adis16475_timeouts), + }, + [ADIS16500] = { + .name = "adis16500", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(10 << 16), + .accel_max_val = 392, + .accel_max_scale = 32000 << 16, + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + /* pulse sync not supported */ + .num_sync = ARRAY_SIZE(adis16475_sync_mode) - 1, + .has_burst32 = true, + .adis_data = ADIS16475_DATA(16500, &adis1650x_timeouts), + }, + [ADIS16505_1] = { + .name = "adis16505-1", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(160 << 16), + .accel_max_val = 78, + .accel_max_scale = 32000 << 16, + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + /* pulse sync not supported */ + .num_sync = ARRAY_SIZE(adis16475_sync_mode) - 1, + .has_burst32 = true, + .adis_data = ADIS16475_DATA(16505, &adis1650x_timeouts), + }, + [ADIS16505_2] = { + .name = "adis16505-2", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(40 << 16), + .accel_max_val = 78, + .accel_max_scale = 32000 << 16, + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + /* pulse sync not supported */ + .num_sync = ARRAY_SIZE(adis16475_sync_mode) - 1, + .has_burst32 = true, + .adis_data = ADIS16475_DATA(16505, &adis1650x_timeouts), + }, + [ADIS16505_3] = { + .name = "adis16505-3", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(10 << 16), + .accel_max_val = 78, + .accel_max_scale = 32000 << 16, + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + /* pulse sync not supported */ + .num_sync = ARRAY_SIZE(adis16475_sync_mode) - 1, + .has_burst32 = true, + .adis_data = ADIS16475_DATA(16505, &adis1650x_timeouts), + }, + [ADIS16507_1] = { + .name = "adis16507-1", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(160 << 16), + .accel_max_val = 392, + .accel_max_scale = 32000 << 16, + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + /* pulse sync not supported */ + .num_sync = ARRAY_SIZE(adis16475_sync_mode) - 1, + .has_burst32 = true, + .adis_data = ADIS16475_DATA(16507, &adis1650x_timeouts), + }, + [ADIS16507_2] = { + .name = "adis16507-2", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(40 << 16), + .accel_max_val = 392, + .accel_max_scale = 32000 << 16, + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + /* pulse sync not supported */ + .num_sync = ARRAY_SIZE(adis16475_sync_mode) - 1, + .has_burst32 = true, + .adis_data = ADIS16475_DATA(16507, &adis1650x_timeouts), + }, + [ADIS16507_3] = { + .name = "adis16507-3", + .num_channels = ARRAY_SIZE(adis16475_channels), + .channels = adis16475_channels, + .gyro_max_val = 1, + .gyro_max_scale = IIO_RAD_TO_DEGREE(10 << 16), + .accel_max_val = 392, + .accel_max_scale = 32000 << 16, + .temp_scale = 100, + .int_clk = 2000, + .max_dec = 1999, + .sync = adis16475_sync_mode, + /* pulse sync not supported */ + .num_sync = ARRAY_SIZE(adis16475_sync_mode) - 1, + .has_burst32 = true, + .adis_data = ADIS16475_DATA(16507, &adis1650x_timeouts), + }, +}; + +static const struct iio_info adis16475_info = { + .read_raw = &adis16475_read_raw, + .write_raw = &adis16475_write_raw, + .update_scan_mode = adis_update_scan_mode, + .debugfs_reg_access = adis_debugfs_reg_access, +}; + +static bool adis16475_validate_crc(const u8 *buffer, u16 crc, + const bool burst32) +{ + int i; + /* extra 6 elements for low gyro and accel */ + const u16 sz = burst32 ? ADIS16475_BURST32_MAX_DATA : + ADIS16475_BURST_MAX_DATA; + + for (i = 0; i < sz - 2; i++) + crc -= buffer[i]; + + return crc == 0; +} + +static void adis16475_burst32_check(struct adis16475 *st) +{ + int ret; + struct adis *adis = &st->adis; + + if (!st->info->has_burst32) + return; + + if (st->lsb_flag && !st->burst32) { + const u16 en = ADIS16500_BURST32(1); + + ret = __adis_update_bits(&st->adis, ADIS16475_REG_MSG_CTRL, + ADIS16500_BURST32_MASK, en); + if (ret) + return; + + st->burst32 = true; + + /* + * In 32-bit mode we need extra 2 bytes for all gyro + * and accel channels. + */ + adis->burst_extra_len = 6 * sizeof(u16); + adis->xfer[1].len += 6 * sizeof(u16); + dev_dbg(&adis->spi->dev, "Enable burst32 mode, xfer:%d", + adis->xfer[1].len); + + } else if (!st->lsb_flag && st->burst32) { + const u16 en = ADIS16500_BURST32(0); + + ret = __adis_update_bits(&st->adis, ADIS16475_REG_MSG_CTRL, + ADIS16500_BURST32_MASK, en); + if (ret) + return; + + st->burst32 = false; + + /* Remove the extra bits */ + adis->burst_extra_len = 0; + adis->xfer[1].len -= 6 * sizeof(u16); + dev_dbg(&adis->spi->dev, "Disable burst32 mode, xfer:%d\n", + adis->xfer[1].len); + } +} + +static irqreturn_t adis16475_trigger_handler(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + struct adis16475 *st = iio_priv(indio_dev); + struct adis *adis = &st->adis; + int ret, bit, i = 0; + __be16 *buffer; + u16 crc; + bool valid; + /* offset until the first element after gyro and accel */ + const u8 offset = st->burst32 ? 13 : 7; + + ret = spi_sync(adis->spi, &adis->msg); + if (ret) + goto check_burst32; + + buffer = adis->buffer; + + crc = be16_to_cpu(buffer[offset + 2]); + valid = adis16475_validate_crc(adis->buffer, crc, st->burst32); + if (!valid) { + dev_err(&adis->spi->dev, "Invalid crc\n"); + goto check_burst32; + } + + for_each_set_bit(bit, indio_dev->active_scan_mask, + indio_dev->masklength) { + /* + * When burst mode is used, system flags is the first data + * channel in the sequence, but the scan index is 7. + */ + switch (bit) { + case ADIS16475_SCAN_TEMP: + st->data[i++] = buffer[offset]; + break; + case ADIS16475_SCAN_GYRO_X ... ADIS16475_SCAN_ACCEL_Z: + /* + * The first 2 bytes on the received data are the + * DIAG_STAT reg, hence the +1 offset here... + */ + if (st->burst32) { + /* upper 16 */ + st->data[i++] = buffer[bit * 2 + 2]; + /* lower 16 */ + st->data[i++] = buffer[bit * 2 + 1]; + } else { + st->data[i++] = buffer[bit + 1]; + /* + * Don't bother in doing the manual read if the + * device supports burst32. burst32 will be + * enabled in the next call to + * adis16475_burst32_check()... + */ + if (st->lsb_flag && !st->info->has_burst32) { + u16 val = 0; + const u32 reg = ADIS16475_REG_X_GYRO_L + + bit * 4; + + adis_read_reg_16(adis, reg, &val); + st->data[i++] = cpu_to_be16(val); + } else { + /* lower not used */ + st->data[i++] = 0; + } + } + break; + } + } + + iio_push_to_buffers_with_timestamp(indio_dev, st->data, pf->timestamp); +check_burst32: + /* + * We only check the burst mode at the end of the current capture since + * it takes a full data ready cycle for the device to update the burst + * array. + */ + adis16475_burst32_check(st); + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; +} + +static int adis16475_config_sync_mode(struct adis16475 *st) +{ + int ret; + struct device *dev = &st->adis.spi->dev; + const struct adis16475_sync *sync; + u32 sync_mode; + + /* default to internal clk */ + st->clk_freq = st->info->int_clk * 1000; + + ret = device_property_read_u32(dev, "adi,sync-mode", &sync_mode); + if (ret) + return 0; + + if (sync_mode >= st->info->num_sync) { + dev_err(dev, "Invalid sync mode: %u for %s\n", sync_mode, + st->info->name); + return -EINVAL; + } + + sync = &st->info->sync[sync_mode]; + st->sync_mode = sync->sync_mode; + + /* All the other modes require external input signal */ + if (sync->sync_mode != ADIS16475_SYNC_OUTPUT) { + struct clk *clk = devm_clk_get_enabled(dev, NULL); + + if (IS_ERR(clk)) + return PTR_ERR(clk); + + st->clk_freq = clk_get_rate(clk); + if (st->clk_freq < sync->min_rate || + st->clk_freq > sync->max_rate) { + dev_err(dev, + "Clk rate:%u not in a valid range:[%u %u]\n", + st->clk_freq, sync->min_rate, sync->max_rate); + return -EINVAL; + } + + if (sync->sync_mode == ADIS16475_SYNC_SCALED) { + u16 up_scale; + + /* + * In sync scaled mode, the IMU sample rate is the clk_freq * sync_scale. + * Hence, default the IMU sample rate to the highest multiple of the input + * clock lower than the IMU max sample rate. The optimal range is + * 1900-2100 sps... + */ + up_scale = 2100 / st->clk_freq; + + ret = __adis_write_reg_16(&st->adis, + ADIS16475_REG_UP_SCALE, + up_scale); + if (ret) + return ret; + } + + st->clk_freq *= 1000; + } + /* + * Keep in mind that the mask for the clk modes in adis1650* + * chips is different (1100 instead of 11100). However, we + * are not configuring BIT(4) in these chips and the default + * value is 0, so we are fine in doing the below operations. + * I'm keeping this for simplicity and avoiding extra variables + * in chip_info. + */ + ret = __adis_update_bits(&st->adis, ADIS16475_REG_MSG_CTRL, + ADIS16475_SYNC_MODE_MASK, sync->sync_mode); + if (ret) + return ret; + + usleep_range(250, 260); + + return 0; +} + +static int adis16475_config_irq_pin(struct adis16475 *st) +{ + int ret; + struct irq_data *desc; + u32 irq_type; + u16 val = 0; + u8 polarity; + struct spi_device *spi = st->adis.spi; + + desc = irq_get_irq_data(spi->irq); + if (!desc) { + dev_err(&spi->dev, "Could not find IRQ %d\n", spi->irq); + return -EINVAL; + } + /* + * It is possible to configure the data ready polarity. Furthermore, we + * need to update the adis struct if we want data ready as active low. + */ + irq_type = irqd_get_trigger_type(desc); + if (irq_type == IRQ_TYPE_EDGE_RISING) { + polarity = 1; + st->adis.irq_flag = IRQF_TRIGGER_RISING; + } else if (irq_type == IRQ_TYPE_EDGE_FALLING) { + polarity = 0; + st->adis.irq_flag = IRQF_TRIGGER_FALLING; + } else { + dev_err(&spi->dev, "Invalid interrupt type 0x%x specified\n", + irq_type); + return -EINVAL; + } + + val = ADIS16475_MSG_CTRL_DR_POL(polarity); + ret = __adis_update_bits(&st->adis, ADIS16475_REG_MSG_CTRL, + ADIS16475_MSG_CTRL_DR_POL_MASK, val); + if (ret) + return ret; + /* + * There is a delay writing to any bits written to the MSC_CTRL + * register. It should not be bigger than 200us, so 250 should be more + * than enough! + */ + usleep_range(250, 260); + + return 0; +} + + +static int adis16475_probe(struct spi_device *spi) +{ + struct iio_dev *indio_dev; + struct adis16475 *st; + int ret; + + indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st)); + if (!indio_dev) + return -ENOMEM; + + st = iio_priv(indio_dev); + + st->info = spi_get_device_match_data(spi); + if (!st->info) + return -EINVAL; + + ret = adis_init(&st->adis, indio_dev, spi, &st->info->adis_data); + if (ret) + return ret; + + indio_dev->name = st->info->name; + indio_dev->channels = st->info->channels; + indio_dev->num_channels = st->info->num_channels; + indio_dev->info = &adis16475_info; + indio_dev->modes = INDIO_DIRECT_MODE; + + ret = __adis_initial_startup(&st->adis); + if (ret) + return ret; + + ret = adis16475_config_irq_pin(st); + if (ret) + return ret; + + ret = adis16475_config_sync_mode(st); + if (ret) + return ret; + + ret = devm_adis_setup_buffer_and_trigger(&st->adis, indio_dev, + adis16475_trigger_handler); + if (ret) + return ret; + + ret = devm_iio_device_register(&spi->dev, indio_dev); + if (ret) + return ret; + + adis16475_debugfs_init(indio_dev); + + return 0; +} + +static const struct of_device_id adis16475_of_match[] = { + { .compatible = "adi,adis16470", + .data = &adis16475_chip_info[ADIS16470] }, + { .compatible = "adi,adis16475-1", + .data = &adis16475_chip_info[ADIS16475_1] }, + { .compatible = "adi,adis16475-2", + .data = &adis16475_chip_info[ADIS16475_2] }, + { .compatible = "adi,adis16475-3", + .data = &adis16475_chip_info[ADIS16475_3] }, + { .compatible = "adi,adis16477-1", + .data = &adis16475_chip_info[ADIS16477_1] }, + { .compatible = "adi,adis16477-2", + .data = &adis16475_chip_info[ADIS16477_2] }, + { .compatible = "adi,adis16477-3", + .data = &adis16475_chip_info[ADIS16477_3] }, + { .compatible = "adi,adis16465-1", + .data = &adis16475_chip_info[ADIS16465_1] }, + { .compatible = "adi,adis16465-2", + .data = &adis16475_chip_info[ADIS16465_2] }, + { .compatible = "adi,adis16465-3", + .data = &adis16475_chip_info[ADIS16465_3] }, + { .compatible = "adi,adis16467-1", + .data = &adis16475_chip_info[ADIS16467_1] }, + { .compatible = "adi,adis16467-2", + .data = &adis16475_chip_info[ADIS16467_2] }, + { .compatible = "adi,adis16467-3", + .data = &adis16475_chip_info[ADIS16467_3] }, + { .compatible = "adi,adis16500", + .data = &adis16475_chip_info[ADIS16500] }, + { .compatible = "adi,adis16505-1", + .data = &adis16475_chip_info[ADIS16505_1] }, + { .compatible = "adi,adis16505-2", + .data = &adis16475_chip_info[ADIS16505_2] }, + { .compatible = "adi,adis16505-3", + .data = &adis16475_chip_info[ADIS16505_3] }, + { .compatible = "adi,adis16507-1", + .data = &adis16475_chip_info[ADIS16507_1] }, + { .compatible = "adi,adis16507-2", + .data = &adis16475_chip_info[ADIS16507_2] }, + { .compatible = "adi,adis16507-3", + .data = &adis16475_chip_info[ADIS16507_3] }, + { }, +}; +MODULE_DEVICE_TABLE(of, adis16475_of_match); + +static const struct spi_device_id adis16475_ids[] = { + { "adis16470", (kernel_ulong_t)&adis16475_chip_info[ADIS16470] }, + { "adis16475-1", (kernel_ulong_t)&adis16475_chip_info[ADIS16475_1] }, + { "adis16475-2", (kernel_ulong_t)&adis16475_chip_info[ADIS16475_2] }, + { "adis16475-3", (kernel_ulong_t)&adis16475_chip_info[ADIS16475_3] }, + { "adis16477-1", (kernel_ulong_t)&adis16475_chip_info[ADIS16477_1] }, + { "adis16477-2", (kernel_ulong_t)&adis16475_chip_info[ADIS16477_2] }, + { "adis16477-3", (kernel_ulong_t)&adis16475_chip_info[ADIS16477_3] }, + { "adis16465-1", (kernel_ulong_t)&adis16475_chip_info[ADIS16465_1] }, + { "adis16465-2", (kernel_ulong_t)&adis16475_chip_info[ADIS16465_2] }, + { "adis16465-3", (kernel_ulong_t)&adis16475_chip_info[ADIS16465_3] }, + { "adis16467-1", (kernel_ulong_t)&adis16475_chip_info[ADIS16467_1] }, + { "adis16467-2", (kernel_ulong_t)&adis16475_chip_info[ADIS16467_2] }, + { "adis16467-3", (kernel_ulong_t)&adis16475_chip_info[ADIS16467_3] }, + { "adis16500", (kernel_ulong_t)&adis16475_chip_info[ADIS16500] }, + { "adis16505-1", (kernel_ulong_t)&adis16475_chip_info[ADIS16505_1] }, + { "adis16505-2", (kernel_ulong_t)&adis16475_chip_info[ADIS16505_2] }, + { "adis16505-3", (kernel_ulong_t)&adis16475_chip_info[ADIS16505_3] }, + { "adis16507-1", (kernel_ulong_t)&adis16475_chip_info[ADIS16507_1] }, + { "adis16507-2", (kernel_ulong_t)&adis16475_chip_info[ADIS16507_2] }, + { "adis16507-3", (kernel_ulong_t)&adis16475_chip_info[ADIS16507_3] }, + { } +}; +MODULE_DEVICE_TABLE(spi, adis16475_ids); + +static struct spi_driver adis16475_driver = { + .driver = { + .name = "adis16475", + .of_match_table = adis16475_of_match, + }, + .probe = adis16475_probe, + .id_table = adis16475_ids, +}; +module_spi_driver(adis16475_driver); + +MODULE_AUTHOR("Nuno Sa "); +MODULE_DESCRIPTION("Analog Devices ADIS16475 IMU driver"); +MODULE_LICENSE("GPL"); +MODULE_IMPORT_NS(IIO_ADISLIB); diff --git a/drivers/iio/imu/adis16480.c b/drivers/iio/imu/adis16480.c new file mode 100644 index 000000000..fe520194a --- /dev/null +++ b/drivers/iio/imu/adis16480.c @@ -0,0 +1,1536 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * ADIS16480 and similar IMUs driver + * + * Copyright 2012 Analog Devices Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#define ADIS16480_PAGE_SIZE 0x80 + +#define ADIS16480_REG(page, reg) ((page) * ADIS16480_PAGE_SIZE + (reg)) + +#define ADIS16480_REG_PAGE_ID 0x00 /* Same address on each page */ +#define ADIS16480_REG_SEQ_CNT ADIS16480_REG(0x00, 0x06) +#define ADIS16480_REG_SYS_E_FLA ADIS16480_REG(0x00, 0x08) +#define ADIS16480_REG_DIAG_STS ADIS16480_REG(0x00, 0x0A) +#define ADIS16480_REG_ALM_STS ADIS16480_REG(0x00, 0x0C) +#define ADIS16480_REG_TEMP_OUT ADIS16480_REG(0x00, 0x0E) +#define ADIS16480_REG_X_GYRO_OUT ADIS16480_REG(0x00, 0x10) +#define ADIS16480_REG_Y_GYRO_OUT ADIS16480_REG(0x00, 0x14) +#define ADIS16480_REG_Z_GYRO_OUT ADIS16480_REG(0x00, 0x18) +#define ADIS16480_REG_X_ACCEL_OUT ADIS16480_REG(0x00, 0x1C) +#define ADIS16480_REG_Y_ACCEL_OUT ADIS16480_REG(0x00, 0x20) +#define ADIS16480_REG_Z_ACCEL_OUT ADIS16480_REG(0x00, 0x24) +#define ADIS16480_REG_X_MAGN_OUT ADIS16480_REG(0x00, 0x28) +#define ADIS16480_REG_Y_MAGN_OUT ADIS16480_REG(0x00, 0x2A) +#define ADIS16480_REG_Z_MAGN_OUT ADIS16480_REG(0x00, 0x2C) +#define ADIS16480_REG_BAROM_OUT ADIS16480_REG(0x00, 0x2E) +#define ADIS16480_REG_X_DELTAANG_OUT ADIS16480_REG(0x00, 0x40) +#define ADIS16480_REG_Y_DELTAANG_OUT ADIS16480_REG(0x00, 0x44) +#define ADIS16480_REG_Z_DELTAANG_OUT ADIS16480_REG(0x00, 0x48) +#define ADIS16480_REG_X_DELTAVEL_OUT ADIS16480_REG(0x00, 0x4C) +#define ADIS16480_REG_Y_DELTAVEL_OUT ADIS16480_REG(0x00, 0x50) +#define ADIS16480_REG_Z_DELTAVEL_OUT ADIS16480_REG(0x00, 0x54) +#define ADIS16480_REG_PROD_ID ADIS16480_REG(0x00, 0x7E) + +#define ADIS16480_REG_X_GYRO_SCALE ADIS16480_REG(0x02, 0x04) +#define ADIS16480_REG_Y_GYRO_SCALE ADIS16480_REG(0x02, 0x06) +#define ADIS16480_REG_Z_GYRO_SCALE ADIS16480_REG(0x02, 0x08) +#define ADIS16480_REG_X_ACCEL_SCALE ADIS16480_REG(0x02, 0x0A) +#define ADIS16480_REG_Y_ACCEL_SCALE ADIS16480_REG(0x02, 0x0C) +#define ADIS16480_REG_Z_ACCEL_SCALE ADIS16480_REG(0x02, 0x0E) +#define ADIS16480_REG_X_GYRO_BIAS ADIS16480_REG(0x02, 0x10) +#define ADIS16480_REG_Y_GYRO_BIAS ADIS16480_REG(0x02, 0x14) +#define ADIS16480_REG_Z_GYRO_BIAS ADIS16480_REG(0x02, 0x18) +#define ADIS16480_REG_X_ACCEL_BIAS ADIS16480_REG(0x02, 0x1C) +#define ADIS16480_REG_Y_ACCEL_BIAS ADIS16480_REG(0x02, 0x20) +#define ADIS16480_REG_Z_ACCEL_BIAS ADIS16480_REG(0x02, 0x24) +#define ADIS16480_REG_X_HARD_IRON ADIS16480_REG(0x02, 0x28) +#define ADIS16480_REG_Y_HARD_IRON ADIS16480_REG(0x02, 0x2A) +#define ADIS16480_REG_Z_HARD_IRON ADIS16480_REG(0x02, 0x2C) +#define ADIS16480_REG_BAROM_BIAS ADIS16480_REG(0x02, 0x40) +#define ADIS16480_REG_FLASH_CNT ADIS16480_REG(0x02, 0x7C) + +#define ADIS16480_REG_GLOB_CMD ADIS16480_REG(0x03, 0x02) +#define ADIS16480_REG_FNCTIO_CTRL ADIS16480_REG(0x03, 0x06) +#define ADIS16480_REG_GPIO_CTRL ADIS16480_REG(0x03, 0x08) +#define ADIS16480_REG_CONFIG ADIS16480_REG(0x03, 0x0A) +#define ADIS16480_REG_DEC_RATE ADIS16480_REG(0x03, 0x0C) +#define ADIS16480_REG_SLP_CNT ADIS16480_REG(0x03, 0x10) +#define ADIS16480_REG_FILTER_BNK0 ADIS16480_REG(0x03, 0x16) +#define ADIS16480_REG_FILTER_BNK1 ADIS16480_REG(0x03, 0x18) +#define ADIS16480_REG_ALM_CNFG0 ADIS16480_REG(0x03, 0x20) +#define ADIS16480_REG_ALM_CNFG1 ADIS16480_REG(0x03, 0x22) +#define ADIS16480_REG_ALM_CNFG2 ADIS16480_REG(0x03, 0x24) +#define ADIS16480_REG_XG_ALM_MAGN ADIS16480_REG(0x03, 0x28) +#define ADIS16480_REG_YG_ALM_MAGN ADIS16480_REG(0x03, 0x2A) +#define ADIS16480_REG_ZG_ALM_MAGN ADIS16480_REG(0x03, 0x2C) +#define ADIS16480_REG_XA_ALM_MAGN ADIS16480_REG(0x03, 0x2E) +#define ADIS16480_REG_YA_ALM_MAGN ADIS16480_REG(0x03, 0x30) +#define ADIS16480_REG_ZA_ALM_MAGN ADIS16480_REG(0x03, 0x32) +#define ADIS16480_REG_XM_ALM_MAGN ADIS16480_REG(0x03, 0x34) +#define ADIS16480_REG_YM_ALM_MAGN ADIS16480_REG(0x03, 0x36) +#define ADIS16480_REG_ZM_ALM_MAGN ADIS16480_REG(0x03, 0x38) +#define ADIS16480_REG_BR_ALM_MAGN ADIS16480_REG(0x03, 0x3A) +#define ADIS16480_REG_FIRM_REV ADIS16480_REG(0x03, 0x78) +#define ADIS16480_REG_FIRM_DM ADIS16480_REG(0x03, 0x7A) +#define ADIS16480_REG_FIRM_Y ADIS16480_REG(0x03, 0x7C) + +/* + * External clock scaling in PPS mode. + * Available only for ADIS1649x devices + */ +#define ADIS16495_REG_SYNC_SCALE ADIS16480_REG(0x03, 0x10) +#define ADIS16495_REG_BURST_CMD ADIS16480_REG(0x00, 0x7C) +#define ADIS16495_BURST_ID 0xA5A5 +/* total number of segments in burst */ +#define ADIS16495_BURST_MAX_DATA 20 +/* spi max speed in burst mode */ +#define ADIS16495_BURST_MAX_SPEED 6000000 + +#define ADIS16480_REG_SERIAL_NUM ADIS16480_REG(0x04, 0x20) + +/* Each filter coefficent bank spans two pages */ +#define ADIS16480_FIR_COEF(page) (x < 60 ? ADIS16480_REG(page, (x) + 8) : \ + ADIS16480_REG((page) + 1, (x) - 60 + 8)) +#define ADIS16480_FIR_COEF_A(x) ADIS16480_FIR_COEF(0x05, (x)) +#define ADIS16480_FIR_COEF_B(x) ADIS16480_FIR_COEF(0x07, (x)) +#define ADIS16480_FIR_COEF_C(x) ADIS16480_FIR_COEF(0x09, (x)) +#define ADIS16480_FIR_COEF_D(x) ADIS16480_FIR_COEF(0x0B, (x)) + +/* ADIS16480_REG_FNCTIO_CTRL */ +#define ADIS16480_DRDY_SEL_MSK GENMASK(1, 0) +#define ADIS16480_DRDY_SEL(x) FIELD_PREP(ADIS16480_DRDY_SEL_MSK, x) +#define ADIS16480_DRDY_POL_MSK BIT(2) +#define ADIS16480_DRDY_POL(x) FIELD_PREP(ADIS16480_DRDY_POL_MSK, x) +#define ADIS16480_DRDY_EN_MSK BIT(3) +#define ADIS16480_DRDY_EN(x) FIELD_PREP(ADIS16480_DRDY_EN_MSK, x) +#define ADIS16480_SYNC_SEL_MSK GENMASK(5, 4) +#define ADIS16480_SYNC_SEL(x) FIELD_PREP(ADIS16480_SYNC_SEL_MSK, x) +#define ADIS16480_SYNC_EN_MSK BIT(7) +#define ADIS16480_SYNC_EN(x) FIELD_PREP(ADIS16480_SYNC_EN_MSK, x) +#define ADIS16480_SYNC_MODE_MSK BIT(8) +#define ADIS16480_SYNC_MODE(x) FIELD_PREP(ADIS16480_SYNC_MODE_MSK, x) + +struct adis16480_chip_info { + unsigned int num_channels; + const struct iio_chan_spec *channels; + unsigned int gyro_max_val; + unsigned int gyro_max_scale; + unsigned int accel_max_val; + unsigned int accel_max_scale; + unsigned int temp_scale; + unsigned int int_clk; + unsigned int max_dec_rate; + const unsigned int *filter_freqs; + bool has_pps_clk_mode; + bool has_sleep_cnt; + const struct adis_data adis_data; +}; + +enum adis16480_int_pin { + ADIS16480_PIN_DIO1, + ADIS16480_PIN_DIO2, + ADIS16480_PIN_DIO3, + ADIS16480_PIN_DIO4 +}; + +enum adis16480_clock_mode { + ADIS16480_CLK_SYNC, + ADIS16480_CLK_PPS, + ADIS16480_CLK_INT +}; + +struct adis16480 { + const struct adis16480_chip_info *chip_info; + + struct adis adis; + struct clk *ext_clk; + enum adis16480_clock_mode clk_mode; + unsigned int clk_freq; + /* Alignment needed for the timestamp */ + __be16 data[ADIS16495_BURST_MAX_DATA] __aligned(8); +}; + +static const char * const adis16480_int_pin_names[4] = { + [ADIS16480_PIN_DIO1] = "DIO1", + [ADIS16480_PIN_DIO2] = "DIO2", + [ADIS16480_PIN_DIO3] = "DIO3", + [ADIS16480_PIN_DIO4] = "DIO4", +}; + +static bool low_rate_allow; +module_param(low_rate_allow, bool, 0444); +MODULE_PARM_DESC(low_rate_allow, + "Allow IMU rates below the minimum advisable when external clk is used in PPS mode (default: N)"); + +#ifdef CONFIG_DEBUG_FS + +static ssize_t adis16480_show_firmware_revision(struct file *file, + char __user *userbuf, size_t count, loff_t *ppos) +{ + struct adis16480 *adis16480 = file->private_data; + char buf[7]; + size_t len; + u16 rev; + int ret; + + ret = adis_read_reg_16(&adis16480->adis, ADIS16480_REG_FIRM_REV, &rev); + if (ret) + return ret; + + len = scnprintf(buf, sizeof(buf), "%x.%x\n", rev >> 8, rev & 0xff); + + return simple_read_from_buffer(userbuf, count, ppos, buf, len); +} + +static const struct file_operations adis16480_firmware_revision_fops = { + .open = simple_open, + .read = adis16480_show_firmware_revision, + .llseek = default_llseek, + .owner = THIS_MODULE, +}; + +static ssize_t adis16480_show_firmware_date(struct file *file, + char __user *userbuf, size_t count, loff_t *ppos) +{ + struct adis16480 *adis16480 = file->private_data; + u16 md, year; + char buf[12]; + size_t len; + int ret; + + ret = adis_read_reg_16(&adis16480->adis, ADIS16480_REG_FIRM_Y, &year); + if (ret) + return ret; + + ret = adis_read_reg_16(&adis16480->adis, ADIS16480_REG_FIRM_DM, &md); + if (ret) + return ret; + + len = snprintf(buf, sizeof(buf), "%.2x-%.2x-%.4x\n", + md >> 8, md & 0xff, year); + + return simple_read_from_buffer(userbuf, count, ppos, buf, len); +} + +static const struct file_operations adis16480_firmware_date_fops = { + .open = simple_open, + .read = adis16480_show_firmware_date, + .llseek = default_llseek, + .owner = THIS_MODULE, +}; + +static int adis16480_show_serial_number(void *arg, u64 *val) +{ + struct adis16480 *adis16480 = arg; + u16 serial; + int ret; + + ret = adis_read_reg_16(&adis16480->adis, ADIS16480_REG_SERIAL_NUM, + &serial); + if (ret) + return ret; + + *val = serial; + + return 0; +} +DEFINE_DEBUGFS_ATTRIBUTE(adis16480_serial_number_fops, + adis16480_show_serial_number, NULL, "0x%.4llx\n"); + +static int adis16480_show_product_id(void *arg, u64 *val) +{ + struct adis16480 *adis16480 = arg; + u16 prod_id; + int ret; + + ret = adis_read_reg_16(&adis16480->adis, ADIS16480_REG_PROD_ID, + &prod_id); + if (ret) + return ret; + + *val = prod_id; + + return 0; +} +DEFINE_DEBUGFS_ATTRIBUTE(adis16480_product_id_fops, + adis16480_show_product_id, NULL, "%llu\n"); + +static int adis16480_show_flash_count(void *arg, u64 *val) +{ + struct adis16480 *adis16480 = arg; + u32 flash_count; + int ret; + + ret = adis_read_reg_32(&adis16480->adis, ADIS16480_REG_FLASH_CNT, + &flash_count); + if (ret) + return ret; + + *val = flash_count; + + return 0; +} +DEFINE_DEBUGFS_ATTRIBUTE(adis16480_flash_count_fops, + adis16480_show_flash_count, NULL, "%lld\n"); + +static int adis16480_debugfs_init(struct iio_dev *indio_dev) +{ + struct adis16480 *adis16480 = iio_priv(indio_dev); + struct dentry *d = iio_get_debugfs_dentry(indio_dev); + + debugfs_create_file_unsafe("firmware_revision", 0400, + d, adis16480, &adis16480_firmware_revision_fops); + debugfs_create_file_unsafe("firmware_date", 0400, + d, adis16480, &adis16480_firmware_date_fops); + debugfs_create_file_unsafe("serial_number", 0400, + d, adis16480, &adis16480_serial_number_fops); + debugfs_create_file_unsafe("product_id", 0400, + d, adis16480, &adis16480_product_id_fops); + debugfs_create_file_unsafe("flash_count", 0400, + d, adis16480, &adis16480_flash_count_fops); + + return 0; +} + +#else + +static int adis16480_debugfs_init(struct iio_dev *indio_dev) +{ + return 0; +} + +#endif + +static int adis16480_set_freq(struct iio_dev *indio_dev, int val, int val2) +{ + struct adis16480 *st = iio_priv(indio_dev); + unsigned int t, sample_rate = st->clk_freq; + int ret; + + if (val < 0 || val2 < 0) + return -EINVAL; + + t = val * 1000 + val2 / 1000; + if (t == 0) + return -EINVAL; + + adis_dev_lock(&st->adis); + /* + * When using PPS mode, the input clock needs to be scaled so that we have an IMU + * sample rate between (optimally) 4000 and 4250. After this, we can use the + * decimation filter to lower the sampling rate in order to get what the user wants. + * Optimally, the user sample rate is a multiple of both the IMU sample rate and + * the input clock. Hence, calculating the sync_scale dynamically gives us better + * chances of achieving a perfect/integer value for DEC_RATE. The math here is: + * 1. lcm of the input clock and the desired output rate. + * 2. get the highest multiple of the previous result lower than the adis max rate. + * 3. The last result becomes the IMU sample rate. Use that to calculate SYNC_SCALE + * and DEC_RATE (to get the user output rate) + */ + if (st->clk_mode == ADIS16480_CLK_PPS) { + unsigned long scaled_rate = lcm(st->clk_freq, t); + int sync_scale; + + /* + * If lcm is bigger than the IMU maximum sampling rate there's no perfect + * solution. In this case, we get the highest multiple of the input clock + * lower than the IMU max sample rate. + */ + if (scaled_rate > st->chip_info->int_clk) + scaled_rate = st->chip_info->int_clk / st->clk_freq * st->clk_freq; + else + scaled_rate = st->chip_info->int_clk / scaled_rate * scaled_rate; + + /* + * This is not an hard requirement but it's not advised to run the IMU + * with a sample rate lower than 4000Hz due to possible undersampling + * issues. However, there are users that might really want to take the risk. + * Hence, we provide a module parameter for them. If set, we allow sample + * rates lower than 4KHz. By default, we won't allow this and we just roundup + * the rate to the next multiple of the input clock bigger than 4KHz. This + * is done like this as in some cases (when DEC_RATE is 0) might give + * us the closest value to the one desired by the user... + */ + if (scaled_rate < 4000000 && !low_rate_allow) + scaled_rate = roundup(4000000, st->clk_freq); + + sync_scale = scaled_rate / st->clk_freq; + ret = __adis_write_reg_16(&st->adis, ADIS16495_REG_SYNC_SCALE, sync_scale); + if (ret) + goto error; + + sample_rate = scaled_rate; + } + + t = DIV_ROUND_CLOSEST(sample_rate, t); + if (t) + t--; + + if (t > st->chip_info->max_dec_rate) + t = st->chip_info->max_dec_rate; + + ret = __adis_write_reg_16(&st->adis, ADIS16480_REG_DEC_RATE, t); +error: + adis_dev_unlock(&st->adis); + return ret; +} + +static int adis16480_get_freq(struct iio_dev *indio_dev, int *val, int *val2) +{ + struct adis16480 *st = iio_priv(indio_dev); + uint16_t t; + int ret; + unsigned int freq, sample_rate = st->clk_freq; + + adis_dev_lock(&st->adis); + + if (st->clk_mode == ADIS16480_CLK_PPS) { + u16 sync_scale; + + ret = __adis_read_reg_16(&st->adis, ADIS16495_REG_SYNC_SCALE, &sync_scale); + if (ret) + goto error; + + sample_rate = st->clk_freq * sync_scale; + } + + ret = __adis_read_reg_16(&st->adis, ADIS16480_REG_DEC_RATE, &t); + if (ret) + goto error; + + adis_dev_unlock(&st->adis); + + freq = DIV_ROUND_CLOSEST(sample_rate, (t + 1)); + + *val = freq / 1000; + *val2 = (freq % 1000) * 1000; + + return IIO_VAL_INT_PLUS_MICRO; +error: + adis_dev_unlock(&st->adis); + return ret; +} + +enum { + ADIS16480_SCAN_GYRO_X, + ADIS16480_SCAN_GYRO_Y, + ADIS16480_SCAN_GYRO_Z, + ADIS16480_SCAN_ACCEL_X, + ADIS16480_SCAN_ACCEL_Y, + ADIS16480_SCAN_ACCEL_Z, + ADIS16480_SCAN_MAGN_X, + ADIS16480_SCAN_MAGN_Y, + ADIS16480_SCAN_MAGN_Z, + ADIS16480_SCAN_BARO, + ADIS16480_SCAN_TEMP, +}; + +static const unsigned int adis16480_calibbias_regs[] = { + [ADIS16480_SCAN_GYRO_X] = ADIS16480_REG_X_GYRO_BIAS, + [ADIS16480_SCAN_GYRO_Y] = ADIS16480_REG_Y_GYRO_BIAS, + [ADIS16480_SCAN_GYRO_Z] = ADIS16480_REG_Z_GYRO_BIAS, + [ADIS16480_SCAN_ACCEL_X] = ADIS16480_REG_X_ACCEL_BIAS, + [ADIS16480_SCAN_ACCEL_Y] = ADIS16480_REG_Y_ACCEL_BIAS, + [ADIS16480_SCAN_ACCEL_Z] = ADIS16480_REG_Z_ACCEL_BIAS, + [ADIS16480_SCAN_MAGN_X] = ADIS16480_REG_X_HARD_IRON, + [ADIS16480_SCAN_MAGN_Y] = ADIS16480_REG_Y_HARD_IRON, + [ADIS16480_SCAN_MAGN_Z] = ADIS16480_REG_Z_HARD_IRON, + [ADIS16480_SCAN_BARO] = ADIS16480_REG_BAROM_BIAS, +}; + +static const unsigned int adis16480_calibscale_regs[] = { + [ADIS16480_SCAN_GYRO_X] = ADIS16480_REG_X_GYRO_SCALE, + [ADIS16480_SCAN_GYRO_Y] = ADIS16480_REG_Y_GYRO_SCALE, + [ADIS16480_SCAN_GYRO_Z] = ADIS16480_REG_Z_GYRO_SCALE, + [ADIS16480_SCAN_ACCEL_X] = ADIS16480_REG_X_ACCEL_SCALE, + [ADIS16480_SCAN_ACCEL_Y] = ADIS16480_REG_Y_ACCEL_SCALE, + [ADIS16480_SCAN_ACCEL_Z] = ADIS16480_REG_Z_ACCEL_SCALE, +}; + +static int adis16480_set_calibbias(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, int bias) +{ + unsigned int reg = adis16480_calibbias_regs[chan->scan_index]; + struct adis16480 *st = iio_priv(indio_dev); + + switch (chan->type) { + case IIO_MAGN: + case IIO_PRESSURE: + if (bias < -0x8000 || bias >= 0x8000) + return -EINVAL; + return adis_write_reg_16(&st->adis, reg, bias); + case IIO_ANGL_VEL: + case IIO_ACCEL: + return adis_write_reg_32(&st->adis, reg, bias); + default: + break; + } + + return -EINVAL; +} + +static int adis16480_get_calibbias(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, int *bias) +{ + unsigned int reg = adis16480_calibbias_regs[chan->scan_index]; + struct adis16480 *st = iio_priv(indio_dev); + uint16_t val16; + uint32_t val32; + int ret; + + switch (chan->type) { + case IIO_MAGN: + case IIO_PRESSURE: + ret = adis_read_reg_16(&st->adis, reg, &val16); + if (ret == 0) + *bias = sign_extend32(val16, 15); + break; + case IIO_ANGL_VEL: + case IIO_ACCEL: + ret = adis_read_reg_32(&st->adis, reg, &val32); + if (ret == 0) + *bias = sign_extend32(val32, 31); + break; + default: + ret = -EINVAL; + } + + if (ret) + return ret; + + return IIO_VAL_INT; +} + +static int adis16480_set_calibscale(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, int scale) +{ + unsigned int reg = adis16480_calibscale_regs[chan->scan_index]; + struct adis16480 *st = iio_priv(indio_dev); + + if (scale < -0x8000 || scale >= 0x8000) + return -EINVAL; + + return adis_write_reg_16(&st->adis, reg, scale); +} + +static int adis16480_get_calibscale(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, int *scale) +{ + unsigned int reg = adis16480_calibscale_regs[chan->scan_index]; + struct adis16480 *st = iio_priv(indio_dev); + uint16_t val16; + int ret; + + ret = adis_read_reg_16(&st->adis, reg, &val16); + if (ret) + return ret; + + *scale = sign_extend32(val16, 15); + return IIO_VAL_INT; +} + +static const unsigned int adis16480_def_filter_freqs[] = { + 310, + 55, + 275, + 63, +}; + +static const unsigned int adis16495_def_filter_freqs[] = { + 300, + 100, + 300, + 100, +}; + +static const unsigned int ad16480_filter_data[][2] = { + [ADIS16480_SCAN_GYRO_X] = { ADIS16480_REG_FILTER_BNK0, 0 }, + [ADIS16480_SCAN_GYRO_Y] = { ADIS16480_REG_FILTER_BNK0, 3 }, + [ADIS16480_SCAN_GYRO_Z] = { ADIS16480_REG_FILTER_BNK0, 6 }, + [ADIS16480_SCAN_ACCEL_X] = { ADIS16480_REG_FILTER_BNK0, 9 }, + [ADIS16480_SCAN_ACCEL_Y] = { ADIS16480_REG_FILTER_BNK0, 12 }, + [ADIS16480_SCAN_ACCEL_Z] = { ADIS16480_REG_FILTER_BNK1, 0 }, + [ADIS16480_SCAN_MAGN_X] = { ADIS16480_REG_FILTER_BNK1, 3 }, + [ADIS16480_SCAN_MAGN_Y] = { ADIS16480_REG_FILTER_BNK1, 6 }, + [ADIS16480_SCAN_MAGN_Z] = { ADIS16480_REG_FILTER_BNK1, 9 }, +}; + +static int adis16480_get_filter_freq(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, int *freq) +{ + struct adis16480 *st = iio_priv(indio_dev); + unsigned int enable_mask, offset, reg; + uint16_t val; + int ret; + + reg = ad16480_filter_data[chan->scan_index][0]; + offset = ad16480_filter_data[chan->scan_index][1]; + enable_mask = BIT(offset + 2); + + ret = adis_read_reg_16(&st->adis, reg, &val); + if (ret) + return ret; + + if (!(val & enable_mask)) + *freq = 0; + else + *freq = st->chip_info->filter_freqs[(val >> offset) & 0x3]; + + return IIO_VAL_INT; +} + +static int adis16480_set_filter_freq(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, unsigned int freq) +{ + struct adis16480 *st = iio_priv(indio_dev); + unsigned int enable_mask, offset, reg; + unsigned int diff, best_diff; + unsigned int i, best_freq; + uint16_t val; + int ret; + + reg = ad16480_filter_data[chan->scan_index][0]; + offset = ad16480_filter_data[chan->scan_index][1]; + enable_mask = BIT(offset + 2); + + adis_dev_lock(&st->adis); + + ret = __adis_read_reg_16(&st->adis, reg, &val); + if (ret) + goto out_unlock; + + if (freq == 0) { + val &= ~enable_mask; + } else { + best_freq = 0; + best_diff = st->chip_info->filter_freqs[0]; + for (i = 0; i < ARRAY_SIZE(adis16480_def_filter_freqs); i++) { + if (st->chip_info->filter_freqs[i] >= freq) { + diff = st->chip_info->filter_freqs[i] - freq; + if (diff < best_diff) { + best_diff = diff; + best_freq = i; + } + } + } + + val &= ~(0x3 << offset); + val |= best_freq << offset; + val |= enable_mask; + } + + ret = __adis_write_reg_16(&st->adis, reg, val); +out_unlock: + adis_dev_unlock(&st->adis); + + return ret; +} + +static int adis16480_read_raw(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, int *val, int *val2, long info) +{ + struct adis16480 *st = iio_priv(indio_dev); + unsigned int temp; + + switch (info) { + case IIO_CHAN_INFO_RAW: + return adis_single_conversion(indio_dev, chan, 0, val); + case IIO_CHAN_INFO_SCALE: + switch (chan->type) { + case IIO_ANGL_VEL: + *val = st->chip_info->gyro_max_scale; + *val2 = st->chip_info->gyro_max_val; + return IIO_VAL_FRACTIONAL; + case IIO_ACCEL: + *val = st->chip_info->accel_max_scale; + *val2 = st->chip_info->accel_max_val; + return IIO_VAL_FRACTIONAL; + case IIO_MAGN: + *val = 0; + *val2 = 100; /* 0.0001 gauss */ + return IIO_VAL_INT_PLUS_MICRO; + case IIO_TEMP: + /* + * +85 degrees Celsius = temp_max_scale + * +25 degrees Celsius = 0 + * LSB, 25 degrees Celsius = 60 / temp_max_scale + */ + *val = st->chip_info->temp_scale / 1000; + *val2 = (st->chip_info->temp_scale % 1000) * 1000; + return IIO_VAL_INT_PLUS_MICRO; + case IIO_PRESSURE: + /* + * max scale is 1310 mbar + * max raw value is 32767 shifted for 32bits + */ + *val = 131; /* 1310mbar = 131 kPa */ + *val2 = 32767 << 16; + return IIO_VAL_FRACTIONAL; + default: + return -EINVAL; + } + case IIO_CHAN_INFO_OFFSET: + /* Only the temperature channel has a offset */ + temp = 25 * 1000000LL; /* 25 degree Celsius = 0x0000 */ + *val = DIV_ROUND_CLOSEST_ULL(temp, st->chip_info->temp_scale); + return IIO_VAL_INT; + case IIO_CHAN_INFO_CALIBBIAS: + return adis16480_get_calibbias(indio_dev, chan, val); + case IIO_CHAN_INFO_CALIBSCALE: + return adis16480_get_calibscale(indio_dev, chan, val); + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: + return adis16480_get_filter_freq(indio_dev, chan, val); + case IIO_CHAN_INFO_SAMP_FREQ: + return adis16480_get_freq(indio_dev, val, val2); + default: + return -EINVAL; + } +} + +static int adis16480_write_raw(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, int val, int val2, long info) +{ + switch (info) { + case IIO_CHAN_INFO_CALIBBIAS: + return adis16480_set_calibbias(indio_dev, chan, val); + case IIO_CHAN_INFO_CALIBSCALE: + return adis16480_set_calibscale(indio_dev, chan, val); + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: + return adis16480_set_filter_freq(indio_dev, chan, val); + case IIO_CHAN_INFO_SAMP_FREQ: + return adis16480_set_freq(indio_dev, val, val2); + + default: + return -EINVAL; + } +} + +#define ADIS16480_MOD_CHANNEL(_type, _mod, _address, _si, _info_sep, _bits) \ + { \ + .type = (_type), \ + .modified = 1, \ + .channel2 = (_mod), \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_CALIBBIAS) | \ + _info_sep, \ + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .address = (_address), \ + .scan_index = (_si), \ + .scan_type = { \ + .sign = 's', \ + .realbits = (_bits), \ + .storagebits = (_bits), \ + .endianness = IIO_BE, \ + }, \ + } + +#define ADIS16480_GYRO_CHANNEL(_mod) \ + ADIS16480_MOD_CHANNEL(IIO_ANGL_VEL, IIO_MOD_ ## _mod, \ + ADIS16480_REG_ ## _mod ## _GYRO_OUT, ADIS16480_SCAN_GYRO_ ## _mod, \ + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY) | \ + BIT(IIO_CHAN_INFO_CALIBSCALE), \ + 32) + +#define ADIS16480_ACCEL_CHANNEL(_mod) \ + ADIS16480_MOD_CHANNEL(IIO_ACCEL, IIO_MOD_ ## _mod, \ + ADIS16480_REG_ ## _mod ## _ACCEL_OUT, ADIS16480_SCAN_ACCEL_ ## _mod, \ + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY) | \ + BIT(IIO_CHAN_INFO_CALIBSCALE), \ + 32) + +#define ADIS16480_MAGN_CHANNEL(_mod) \ + ADIS16480_MOD_CHANNEL(IIO_MAGN, IIO_MOD_ ## _mod, \ + ADIS16480_REG_ ## _mod ## _MAGN_OUT, ADIS16480_SCAN_MAGN_ ## _mod, \ + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), \ + 16) + +#define ADIS16480_PRESSURE_CHANNEL() \ + { \ + .type = IIO_PRESSURE, \ + .indexed = 1, \ + .channel = 0, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_CALIBBIAS) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .address = ADIS16480_REG_BAROM_OUT, \ + .scan_index = ADIS16480_SCAN_BARO, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 32, \ + .storagebits = 32, \ + .endianness = IIO_BE, \ + }, \ + } + +#define ADIS16480_TEMP_CHANNEL() { \ + .type = IIO_TEMP, \ + .indexed = 1, \ + .channel = 0, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE) | \ + BIT(IIO_CHAN_INFO_OFFSET), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .address = ADIS16480_REG_TEMP_OUT, \ + .scan_index = ADIS16480_SCAN_TEMP, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_BE, \ + }, \ + } + +static const struct iio_chan_spec adis16480_channels[] = { + ADIS16480_GYRO_CHANNEL(X), + ADIS16480_GYRO_CHANNEL(Y), + ADIS16480_GYRO_CHANNEL(Z), + ADIS16480_ACCEL_CHANNEL(X), + ADIS16480_ACCEL_CHANNEL(Y), + ADIS16480_ACCEL_CHANNEL(Z), + ADIS16480_MAGN_CHANNEL(X), + ADIS16480_MAGN_CHANNEL(Y), + ADIS16480_MAGN_CHANNEL(Z), + ADIS16480_PRESSURE_CHANNEL(), + ADIS16480_TEMP_CHANNEL(), + IIO_CHAN_SOFT_TIMESTAMP(11) +}; + +static const struct iio_chan_spec adis16485_channels[] = { + ADIS16480_GYRO_CHANNEL(X), + ADIS16480_GYRO_CHANNEL(Y), + ADIS16480_GYRO_CHANNEL(Z), + ADIS16480_ACCEL_CHANNEL(X), + ADIS16480_ACCEL_CHANNEL(Y), + ADIS16480_ACCEL_CHANNEL(Z), + ADIS16480_TEMP_CHANNEL(), + IIO_CHAN_SOFT_TIMESTAMP(7) +}; + +enum adis16480_variant { + ADIS16375, + ADIS16480, + ADIS16485, + ADIS16488, + ADIS16490, + ADIS16495_1, + ADIS16495_2, + ADIS16495_3, + ADIS16497_1, + ADIS16497_2, + ADIS16497_3, +}; + +#define ADIS16480_DIAG_STAT_XGYRO_FAIL 0 +#define ADIS16480_DIAG_STAT_YGYRO_FAIL 1 +#define ADIS16480_DIAG_STAT_ZGYRO_FAIL 2 +#define ADIS16480_DIAG_STAT_XACCL_FAIL 3 +#define ADIS16480_DIAG_STAT_YACCL_FAIL 4 +#define ADIS16480_DIAG_STAT_ZACCL_FAIL 5 +#define ADIS16480_DIAG_STAT_XMAGN_FAIL 8 +#define ADIS16480_DIAG_STAT_YMAGN_FAIL 9 +#define ADIS16480_DIAG_STAT_ZMAGN_FAIL 10 +#define ADIS16480_DIAG_STAT_BARO_FAIL 11 + +static const char * const adis16480_status_error_msgs[] = { + [ADIS16480_DIAG_STAT_XGYRO_FAIL] = "X-axis gyroscope self-test failure", + [ADIS16480_DIAG_STAT_YGYRO_FAIL] = "Y-axis gyroscope self-test failure", + [ADIS16480_DIAG_STAT_ZGYRO_FAIL] = "Z-axis gyroscope self-test failure", + [ADIS16480_DIAG_STAT_XACCL_FAIL] = "X-axis accelerometer self-test failure", + [ADIS16480_DIAG_STAT_YACCL_FAIL] = "Y-axis accelerometer self-test failure", + [ADIS16480_DIAG_STAT_ZACCL_FAIL] = "Z-axis accelerometer self-test failure", + [ADIS16480_DIAG_STAT_XMAGN_FAIL] = "X-axis magnetometer self-test failure", + [ADIS16480_DIAG_STAT_YMAGN_FAIL] = "Y-axis magnetometer self-test failure", + [ADIS16480_DIAG_STAT_ZMAGN_FAIL] = "Z-axis magnetometer self-test failure", + [ADIS16480_DIAG_STAT_BARO_FAIL] = "Barometer self-test failure", +}; + +static int adis16480_enable_irq(struct adis *adis, bool enable); + +#define ADIS16480_DATA(_prod_id, _timeouts, _burst_len) \ +{ \ + .diag_stat_reg = ADIS16480_REG_DIAG_STS, \ + .glob_cmd_reg = ADIS16480_REG_GLOB_CMD, \ + .prod_id_reg = ADIS16480_REG_PROD_ID, \ + .prod_id = (_prod_id), \ + .has_paging = true, \ + .read_delay = 5, \ + .write_delay = 5, \ + .self_test_mask = BIT(1), \ + .self_test_reg = ADIS16480_REG_GLOB_CMD, \ + .status_error_msgs = adis16480_status_error_msgs, \ + .status_error_mask = BIT(ADIS16480_DIAG_STAT_XGYRO_FAIL) | \ + BIT(ADIS16480_DIAG_STAT_YGYRO_FAIL) | \ + BIT(ADIS16480_DIAG_STAT_ZGYRO_FAIL) | \ + BIT(ADIS16480_DIAG_STAT_XACCL_FAIL) | \ + BIT(ADIS16480_DIAG_STAT_YACCL_FAIL) | \ + BIT(ADIS16480_DIAG_STAT_ZACCL_FAIL) | \ + BIT(ADIS16480_DIAG_STAT_XMAGN_FAIL) | \ + BIT(ADIS16480_DIAG_STAT_YMAGN_FAIL) | \ + BIT(ADIS16480_DIAG_STAT_ZMAGN_FAIL) | \ + BIT(ADIS16480_DIAG_STAT_BARO_FAIL), \ + .enable_irq = adis16480_enable_irq, \ + .timeouts = (_timeouts), \ + .burst_reg_cmd = ADIS16495_REG_BURST_CMD, \ + .burst_len = (_burst_len), \ + .burst_max_speed_hz = ADIS16495_BURST_MAX_SPEED \ +} + +static const struct adis_timeout adis16485_timeouts = { + .reset_ms = 560, + .sw_reset_ms = 120, + .self_test_ms = 12, +}; + +static const struct adis_timeout adis16480_timeouts = { + .reset_ms = 560, + .sw_reset_ms = 560, + .self_test_ms = 12, +}; + +static const struct adis_timeout adis16495_timeouts = { + .reset_ms = 170, + .sw_reset_ms = 130, + .self_test_ms = 40, +}; + +static const struct adis_timeout adis16495_1_timeouts = { + .reset_ms = 250, + .sw_reset_ms = 210, + .self_test_ms = 20, +}; + +static const struct adis16480_chip_info adis16480_chip_info[] = { + [ADIS16375] = { + .channels = adis16485_channels, + .num_channels = ARRAY_SIZE(adis16485_channels), + /* + * Typically we do IIO_RAD_TO_DEGREE in the denominator, which + * is exactly the same as IIO_DEGREE_TO_RAD in numerator, since + * it gives better approximation. However, in this case we + * cannot do it since it would not fit in a 32bit variable. + */ + .gyro_max_val = 22887 << 16, + .gyro_max_scale = IIO_DEGREE_TO_RAD(300), + .accel_max_val = IIO_M_S_2_TO_G(21973 << 16), + .accel_max_scale = 18, + .temp_scale = 5650, /* 5.65 milli degree Celsius */ + .int_clk = 2460000, + .max_dec_rate = 2048, + .has_sleep_cnt = true, + .filter_freqs = adis16480_def_filter_freqs, + .adis_data = ADIS16480_DATA(16375, &adis16485_timeouts, 0), + }, + [ADIS16480] = { + .channels = adis16480_channels, + .num_channels = ARRAY_SIZE(adis16480_channels), + .gyro_max_val = 22500 << 16, + .gyro_max_scale = IIO_DEGREE_TO_RAD(450), + .accel_max_val = IIO_M_S_2_TO_G(12500 << 16), + .accel_max_scale = 10, + .temp_scale = 5650, /* 5.65 milli degree Celsius */ + .int_clk = 2460000, + .max_dec_rate = 2048, + .has_sleep_cnt = true, + .filter_freqs = adis16480_def_filter_freqs, + .adis_data = ADIS16480_DATA(16480, &adis16480_timeouts, 0), + }, + [ADIS16485] = { + .channels = adis16485_channels, + .num_channels = ARRAY_SIZE(adis16485_channels), + .gyro_max_val = 22500 << 16, + .gyro_max_scale = IIO_DEGREE_TO_RAD(450), + .accel_max_val = IIO_M_S_2_TO_G(20000 << 16), + .accel_max_scale = 5, + .temp_scale = 5650, /* 5.65 milli degree Celsius */ + .int_clk = 2460000, + .max_dec_rate = 2048, + .has_sleep_cnt = true, + .filter_freqs = adis16480_def_filter_freqs, + .adis_data = ADIS16480_DATA(16485, &adis16485_timeouts, 0), + }, + [ADIS16488] = { + .channels = adis16480_channels, + .num_channels = ARRAY_SIZE(adis16480_channels), + .gyro_max_val = 22500 << 16, + .gyro_max_scale = IIO_DEGREE_TO_RAD(450), + .accel_max_val = IIO_M_S_2_TO_G(22500 << 16), + .accel_max_scale = 18, + .temp_scale = 5650, /* 5.65 milli degree Celsius */ + .int_clk = 2460000, + .max_dec_rate = 2048, + .has_sleep_cnt = true, + .filter_freqs = adis16480_def_filter_freqs, + .adis_data = ADIS16480_DATA(16488, &adis16485_timeouts, 0), + }, + [ADIS16490] = { + .channels = adis16485_channels, + .num_channels = ARRAY_SIZE(adis16485_channels), + .gyro_max_val = 20000 << 16, + .gyro_max_scale = IIO_DEGREE_TO_RAD(100), + .accel_max_val = IIO_M_S_2_TO_G(16000 << 16), + .accel_max_scale = 8, + .temp_scale = 14285, /* 14.285 milli degree Celsius */ + .int_clk = 4250000, + .max_dec_rate = 4250, + .filter_freqs = adis16495_def_filter_freqs, + .has_pps_clk_mode = true, + .adis_data = ADIS16480_DATA(16490, &adis16495_timeouts, 0), + }, + [ADIS16495_1] = { + .channels = adis16485_channels, + .num_channels = ARRAY_SIZE(adis16485_channels), + .gyro_max_val = 20000 << 16, + .gyro_max_scale = IIO_DEGREE_TO_RAD(125), + .accel_max_val = IIO_M_S_2_TO_G(32000 << 16), + .accel_max_scale = 8, + .temp_scale = 12500, /* 12.5 milli degree Celsius */ + .int_clk = 4250000, + .max_dec_rate = 4250, + .filter_freqs = adis16495_def_filter_freqs, + .has_pps_clk_mode = true, + /* 20 elements of 16bits */ + .adis_data = ADIS16480_DATA(16495, &adis16495_1_timeouts, + ADIS16495_BURST_MAX_DATA * 2), + }, + [ADIS16495_2] = { + .channels = adis16485_channels, + .num_channels = ARRAY_SIZE(adis16485_channels), + .gyro_max_val = 18000 << 16, + .gyro_max_scale = IIO_DEGREE_TO_RAD(450), + .accel_max_val = IIO_M_S_2_TO_G(32000 << 16), + .accel_max_scale = 8, + .temp_scale = 12500, /* 12.5 milli degree Celsius */ + .int_clk = 4250000, + .max_dec_rate = 4250, + .filter_freqs = adis16495_def_filter_freqs, + .has_pps_clk_mode = true, + /* 20 elements of 16bits */ + .adis_data = ADIS16480_DATA(16495, &adis16495_1_timeouts, + ADIS16495_BURST_MAX_DATA * 2), + }, + [ADIS16495_3] = { + .channels = adis16485_channels, + .num_channels = ARRAY_SIZE(adis16485_channels), + .gyro_max_val = 20000 << 16, + .gyro_max_scale = IIO_DEGREE_TO_RAD(2000), + .accel_max_val = IIO_M_S_2_TO_G(32000 << 16), + .accel_max_scale = 8, + .temp_scale = 12500, /* 12.5 milli degree Celsius */ + .int_clk = 4250000, + .max_dec_rate = 4250, + .filter_freqs = adis16495_def_filter_freqs, + .has_pps_clk_mode = true, + /* 20 elements of 16bits */ + .adis_data = ADIS16480_DATA(16495, &adis16495_1_timeouts, + ADIS16495_BURST_MAX_DATA * 2), + }, + [ADIS16497_1] = { + .channels = adis16485_channels, + .num_channels = ARRAY_SIZE(adis16485_channels), + .gyro_max_val = 20000 << 16, + .gyro_max_scale = IIO_DEGREE_TO_RAD(125), + .accel_max_val = IIO_M_S_2_TO_G(32000 << 16), + .accel_max_scale = 40, + .temp_scale = 12500, /* 12.5 milli degree Celsius */ + .int_clk = 4250000, + .max_dec_rate = 4250, + .filter_freqs = adis16495_def_filter_freqs, + .has_pps_clk_mode = true, + /* 20 elements of 16bits */ + .adis_data = ADIS16480_DATA(16497, &adis16495_1_timeouts, + ADIS16495_BURST_MAX_DATA * 2), + }, + [ADIS16497_2] = { + .channels = adis16485_channels, + .num_channels = ARRAY_SIZE(adis16485_channels), + .gyro_max_val = 18000 << 16, + .gyro_max_scale = IIO_DEGREE_TO_RAD(450), + .accel_max_val = IIO_M_S_2_TO_G(32000 << 16), + .accel_max_scale = 40, + .temp_scale = 12500, /* 12.5 milli degree Celsius */ + .int_clk = 4250000, + .max_dec_rate = 4250, + .filter_freqs = adis16495_def_filter_freqs, + .has_pps_clk_mode = true, + /* 20 elements of 16bits */ + .adis_data = ADIS16480_DATA(16497, &adis16495_1_timeouts, + ADIS16495_BURST_MAX_DATA * 2), + }, + [ADIS16497_3] = { + .channels = adis16485_channels, + .num_channels = ARRAY_SIZE(adis16485_channels), + .gyro_max_val = 20000 << 16, + .gyro_max_scale = IIO_DEGREE_TO_RAD(2000), + .accel_max_val = IIO_M_S_2_TO_G(32000 << 16), + .accel_max_scale = 40, + .temp_scale = 12500, /* 12.5 milli degree Celsius */ + .int_clk = 4250000, + .max_dec_rate = 4250, + .filter_freqs = adis16495_def_filter_freqs, + .has_pps_clk_mode = true, + /* 20 elements of 16bits */ + .adis_data = ADIS16480_DATA(16497, &adis16495_1_timeouts, + ADIS16495_BURST_MAX_DATA * 2), + }, +}; + +static bool adis16480_validate_crc(const u16 *buf, const u8 n_elem, const u32 crc) +{ + u32 crc_calc; + u16 crc_buf[15]; + int j; + + for (j = 0; j < n_elem; j++) + crc_buf[j] = swab16(buf[j]); + + crc_calc = crc32(~0, crc_buf, n_elem * 2); + crc_calc ^= ~0; + + return (crc == crc_calc); +} + +static irqreturn_t adis16480_trigger_handler(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + struct adis16480 *st = iio_priv(indio_dev); + struct adis *adis = &st->adis; + struct device *dev = &adis->spi->dev; + int ret, bit, offset, i = 0; + __be16 *buffer; + u32 crc; + bool valid; + + adis_dev_lock(adis); + if (adis->current_page != 0) { + adis->tx[0] = ADIS_WRITE_REG(ADIS_REG_PAGE_ID); + adis->tx[1] = 0; + ret = spi_write(adis->spi, adis->tx, 2); + if (ret) { + dev_err(dev, "Failed to change device page: %d\n", ret); + adis_dev_unlock(adis); + goto irq_done; + } + + adis->current_page = 0; + } + + ret = spi_sync(adis->spi, &adis->msg); + if (ret) { + dev_err(dev, "Failed to read data: %d\n", ret); + adis_dev_unlock(adis); + goto irq_done; + } + + adis_dev_unlock(adis); + + /* + * After making the burst request, the response can have one or two + * 16-bit responses containing the BURST_ID depending on the sclk. If + * clk > 3.6MHz, then we will have two BURST_ID in a row. If clk < 3MHZ, + * we have only one. To manage that variation, we use the transition from the + * BURST_ID to the SYS_E_FLAG register, which will not be equal to 0xA5A5. If + * we not find this variation in the first 4 segments, then the data should + * not be valid. + */ + buffer = adis->buffer; + for (offset = 0; offset < 4; offset++) { + u16 curr = be16_to_cpu(buffer[offset]); + u16 next = be16_to_cpu(buffer[offset + 1]); + + if (curr == ADIS16495_BURST_ID && next != ADIS16495_BURST_ID) { + offset++; + break; + } + } + + if (offset == 4) { + dev_err(dev, "Invalid burst data\n"); + goto irq_done; + } + + crc = be16_to_cpu(buffer[offset + 16]) << 16 | be16_to_cpu(buffer[offset + 15]); + valid = adis16480_validate_crc((u16 *)&buffer[offset], 15, crc); + if (!valid) { + dev_err(dev, "Invalid crc\n"); + goto irq_done; + } + + for_each_set_bit(bit, indio_dev->active_scan_mask, indio_dev->masklength) { + /* + * When burst mode is used, temperature is the first data + * channel in the sequence, but the temperature scan index + * is 10. + */ + switch (bit) { + case ADIS16480_SCAN_TEMP: + st->data[i++] = buffer[offset + 1]; + break; + case ADIS16480_SCAN_GYRO_X ... ADIS16480_SCAN_ACCEL_Z: + /* The lower register data is sequenced first */ + st->data[i++] = buffer[2 * bit + offset + 3]; + st->data[i++] = buffer[2 * bit + offset + 2]; + break; + } + } + + iio_push_to_buffers_with_timestamp(indio_dev, st->data, pf->timestamp); +irq_done: + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; +} + +static const struct iio_info adis16480_info = { + .read_raw = &adis16480_read_raw, + .write_raw = &adis16480_write_raw, + .update_scan_mode = adis_update_scan_mode, + .debugfs_reg_access = adis_debugfs_reg_access, +}; + +static int adis16480_stop_device(struct iio_dev *indio_dev) +{ + struct adis16480 *st = iio_priv(indio_dev); + struct device *dev = &st->adis.spi->dev; + int ret; + + ret = adis_write_reg_16(&st->adis, ADIS16480_REG_SLP_CNT, BIT(9)); + if (ret) + dev_err(dev, "Could not power down device: %d\n", ret); + + return ret; +} + +static int adis16480_enable_irq(struct adis *adis, bool enable) +{ + uint16_t val; + int ret; + + ret = __adis_read_reg_16(adis, ADIS16480_REG_FNCTIO_CTRL, &val); + if (ret) + return ret; + + val &= ~ADIS16480_DRDY_EN_MSK; + val |= ADIS16480_DRDY_EN(enable); + + return __adis_write_reg_16(adis, ADIS16480_REG_FNCTIO_CTRL, val); +} + +static int adis16480_config_irq_pin(struct adis16480 *st) +{ + struct device *dev = &st->adis.spi->dev; + struct fwnode_handle *fwnode = dev_fwnode(dev); + struct irq_data *desc; + enum adis16480_int_pin pin; + unsigned int irq_type; + uint16_t val; + int i, irq = 0; + + desc = irq_get_irq_data(st->adis.spi->irq); + if (!desc) { + dev_err(dev, "Could not find IRQ %d\n", irq); + return -EINVAL; + } + + /* Disable data ready since the default after reset is on */ + val = ADIS16480_DRDY_EN(0); + + /* + * Get the interrupt from the devicetre by reading the interrupt-names + * property. If it is not specified, use DIO1 pin as default. + * According to the datasheet, the factory default assigns DIO2 as data + * ready signal. However, in the previous versions of the driver, DIO1 + * pin was used. So, we should leave it as is since some devices might + * be expecting the interrupt on the wrong physical pin. + */ + pin = ADIS16480_PIN_DIO1; + for (i = 0; i < ARRAY_SIZE(adis16480_int_pin_names); i++) { + irq = fwnode_irq_get_byname(fwnode, adis16480_int_pin_names[i]); + if (irq > 0) { + pin = i; + break; + } + } + + val |= ADIS16480_DRDY_SEL(pin); + + /* + * Get the interrupt line behaviour. The data ready polarity can be + * configured as positive or negative, corresponding to + * IRQ_TYPE_EDGE_RISING or IRQ_TYPE_EDGE_FALLING respectively. + */ + irq_type = irqd_get_trigger_type(desc); + if (irq_type == IRQ_TYPE_EDGE_RISING) { /* Default */ + val |= ADIS16480_DRDY_POL(1); + } else if (irq_type == IRQ_TYPE_EDGE_FALLING) { + val |= ADIS16480_DRDY_POL(0); + } else { + dev_err(dev, "Invalid interrupt type 0x%x specified\n", irq_type); + return -EINVAL; + } + /* Write the data ready configuration to the FNCTIO_CTRL register */ + return adis_write_reg_16(&st->adis, ADIS16480_REG_FNCTIO_CTRL, val); +} + +static int adis16480_fw_get_ext_clk_pin(struct adis16480 *st) +{ + struct device *dev = &st->adis.spi->dev; + const char *ext_clk_pin; + enum adis16480_int_pin pin; + int i; + + pin = ADIS16480_PIN_DIO2; + if (device_property_read_string(dev, "adi,ext-clk-pin", &ext_clk_pin)) + goto clk_input_not_found; + + for (i = 0; i < ARRAY_SIZE(adis16480_int_pin_names); i++) { + if (strcasecmp(ext_clk_pin, adis16480_int_pin_names[i]) == 0) + return i; + } + +clk_input_not_found: + dev_info(dev, "clk input line not specified, using DIO2\n"); + return pin; +} + +static int adis16480_ext_clk_config(struct adis16480 *st, bool enable) +{ + struct device *dev = &st->adis.spi->dev; + unsigned int mode, mask; + enum adis16480_int_pin pin; + uint16_t val; + int ret; + + ret = adis_read_reg_16(&st->adis, ADIS16480_REG_FNCTIO_CTRL, &val); + if (ret) + return ret; + + pin = adis16480_fw_get_ext_clk_pin(st); + /* + * Each DIOx pin supports only one function at a time. When a single pin + * has two assignments, the enable bit for a lower priority function + * automatically resets to zero (disabling the lower priority function). + */ + if (pin == ADIS16480_DRDY_SEL(val)) + dev_warn(dev, "DIO%x pin supports only one function at a time\n", pin + 1); + + mode = ADIS16480_SYNC_EN(enable) | ADIS16480_SYNC_SEL(pin); + mask = ADIS16480_SYNC_EN_MSK | ADIS16480_SYNC_SEL_MSK; + /* Only ADIS1649x devices support pps ext clock mode */ + if (st->chip_info->has_pps_clk_mode) { + mode |= ADIS16480_SYNC_MODE(st->clk_mode); + mask |= ADIS16480_SYNC_MODE_MSK; + } + + val &= ~mask; + val |= mode; + + ret = adis_write_reg_16(&st->adis, ADIS16480_REG_FNCTIO_CTRL, val); + if (ret) + return ret; + + return clk_prepare_enable(st->ext_clk); +} + +static int adis16480_get_ext_clocks(struct adis16480 *st) +{ + struct device *dev = &st->adis.spi->dev; + + st->ext_clk = devm_clk_get_optional(dev, "sync"); + if (IS_ERR(st->ext_clk)) + return dev_err_probe(dev, PTR_ERR(st->ext_clk), "failed to get ext clk\n"); + if (st->ext_clk) { + st->clk_mode = ADIS16480_CLK_SYNC; + return 0; + } + + if (st->chip_info->has_pps_clk_mode) { + st->ext_clk = devm_clk_get_optional(dev, "pps"); + if (IS_ERR(st->ext_clk)) + return dev_err_probe(dev, PTR_ERR(st->ext_clk), "failed to get ext clk\n"); + if (st->ext_clk) { + st->clk_mode = ADIS16480_CLK_PPS; + return 0; + } + } + + st->clk_mode = ADIS16480_CLK_INT; + return 0; +} + +static void adis16480_stop(void *data) +{ + adis16480_stop_device(data); +} + +static void adis16480_clk_disable(void *data) +{ + clk_disable_unprepare(data); +} + +static int adis16480_probe(struct spi_device *spi) +{ + const struct spi_device_id *id = spi_get_device_id(spi); + const struct adis_data *adis16480_data; + irq_handler_t trigger_handler = NULL; + struct device *dev = &spi->dev; + struct iio_dev *indio_dev; + struct adis16480 *st; + int ret; + + indio_dev = devm_iio_device_alloc(dev, sizeof(*st)); + if (indio_dev == NULL) + return -ENOMEM; + + st = iio_priv(indio_dev); + + st->chip_info = &adis16480_chip_info[id->driver_data]; + indio_dev->name = spi_get_device_id(spi)->name; + indio_dev->channels = st->chip_info->channels; + indio_dev->num_channels = st->chip_info->num_channels; + indio_dev->info = &adis16480_info; + indio_dev->modes = INDIO_DIRECT_MODE; + + adis16480_data = &st->chip_info->adis_data; + + ret = adis_init(&st->adis, indio_dev, spi, adis16480_data); + if (ret) + return ret; + + ret = __adis_initial_startup(&st->adis); + if (ret) + return ret; + + if (st->chip_info->has_sleep_cnt) { + ret = devm_add_action_or_reset(dev, adis16480_stop, indio_dev); + if (ret) + return ret; + } + + ret = adis16480_config_irq_pin(st); + if (ret) + return ret; + + ret = adis16480_get_ext_clocks(st); + if (ret) + return ret; + + if (st->ext_clk) { + ret = adis16480_ext_clk_config(st, true); + if (ret) + return ret; + + ret = devm_add_action_or_reset(dev, adis16480_clk_disable, st->ext_clk); + if (ret) + return ret; + + st->clk_freq = clk_get_rate(st->ext_clk); + st->clk_freq *= 1000; /* micro */ + if (st->clk_mode == ADIS16480_CLK_PPS) { + u16 sync_scale; + + /* + * In PPS mode, the IMU sample rate is the clk_freq * sync_scale. Hence, + * default the IMU sample rate to the highest multiple of the input clock + * lower than the IMU max sample rate. The internal sample rate is the + * max... + */ + sync_scale = st->chip_info->int_clk / st->clk_freq; + ret = __adis_write_reg_16(&st->adis, ADIS16495_REG_SYNC_SCALE, sync_scale); + if (ret) + return ret; + } + } else { + st->clk_freq = st->chip_info->int_clk; + } + + /* Only use our trigger handler if burst mode is supported */ + if (adis16480_data->burst_len) + trigger_handler = adis16480_trigger_handler; + + ret = devm_adis_setup_buffer_and_trigger(&st->adis, indio_dev, + trigger_handler); + if (ret) + return ret; + + ret = devm_iio_device_register(dev, indio_dev); + if (ret) + return ret; + + adis16480_debugfs_init(indio_dev); + + return 0; +} + +static const struct spi_device_id adis16480_ids[] = { + { "adis16375", ADIS16375 }, + { "adis16480", ADIS16480 }, + { "adis16485", ADIS16485 }, + { "adis16488", ADIS16488 }, + { "adis16490", ADIS16490 }, + { "adis16495-1", ADIS16495_1 }, + { "adis16495-2", ADIS16495_2 }, + { "adis16495-3", ADIS16495_3 }, + { "adis16497-1", ADIS16497_1 }, + { "adis16497-2", ADIS16497_2 }, + { "adis16497-3", ADIS16497_3 }, + { } +}; +MODULE_DEVICE_TABLE(spi, adis16480_ids); + +static const struct of_device_id adis16480_of_match[] = { + { .compatible = "adi,adis16375" }, + { .compatible = "adi,adis16480" }, + { .compatible = "adi,adis16485" }, + { .compatible = "adi,adis16488" }, + { .compatible = "adi,adis16490" }, + { .compatible = "adi,adis16495-1" }, + { .compatible = "adi,adis16495-2" }, + { .compatible = "adi,adis16495-3" }, + { .compatible = "adi,adis16497-1" }, + { .compatible = "adi,adis16497-2" }, + { .compatible = "adi,adis16497-3" }, + { }, +}; +MODULE_DEVICE_TABLE(of, adis16480_of_match); + +static struct spi_driver adis16480_driver = { + .driver = { + .name = "adis16480", + .of_match_table = adis16480_of_match, + }, + .id_table = adis16480_ids, + .probe = adis16480_probe, +}; +module_spi_driver(adis16480_driver); + +MODULE_AUTHOR("Lars-Peter Clausen "); +MODULE_DESCRIPTION("Analog Devices ADIS16480 IMU driver"); +MODULE_LICENSE("GPL v2"); +MODULE_IMPORT_NS(IIO_ADISLIB); diff --git a/drivers/iio/imu/adis_buffer.c b/drivers/iio/imu/adis_buffer.c new file mode 100644 index 000000000..928933027 --- /dev/null +++ b/drivers/iio/imu/adis_buffer.c @@ -0,0 +1,216 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * Common library for ADIS16XXX devices + * + * Copyright 2012 Analog Devices Inc. + * Author: Lars-Peter Clausen + */ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +static int adis_update_scan_mode_burst(struct iio_dev *indio_dev, + const unsigned long *scan_mask) +{ + struct adis *adis = iio_device_get_drvdata(indio_dev); + unsigned int burst_length, burst_max_length; + u8 *tx; + + burst_length = adis->data->burst_len + adis->burst_extra_len; + + if (adis->data->burst_max_len) + burst_max_length = adis->data->burst_max_len; + else + burst_max_length = burst_length; + + adis->xfer = kcalloc(2, sizeof(*adis->xfer), GFP_KERNEL); + if (!adis->xfer) + return -ENOMEM; + + adis->buffer = kzalloc(burst_max_length + sizeof(u16), GFP_KERNEL); + if (!adis->buffer) { + kfree(adis->xfer); + adis->xfer = NULL; + return -ENOMEM; + } + + tx = adis->buffer + burst_max_length; + tx[0] = ADIS_READ_REG(adis->data->burst_reg_cmd); + tx[1] = 0; + + adis->xfer[0].tx_buf = tx; + adis->xfer[0].bits_per_word = 8; + adis->xfer[0].len = 2; + if (adis->data->burst_max_speed_hz) + adis->xfer[0].speed_hz = adis->data->burst_max_speed_hz; + adis->xfer[1].rx_buf = adis->buffer; + adis->xfer[1].bits_per_word = 8; + adis->xfer[1].len = burst_length; + if (adis->data->burst_max_speed_hz) + adis->xfer[1].speed_hz = adis->data->burst_max_speed_hz; + + spi_message_init(&adis->msg); + spi_message_add_tail(&adis->xfer[0], &adis->msg); + spi_message_add_tail(&adis->xfer[1], &adis->msg); + + return 0; +} + +int adis_update_scan_mode(struct iio_dev *indio_dev, + const unsigned long *scan_mask) +{ + struct adis *adis = iio_device_get_drvdata(indio_dev); + const struct iio_chan_spec *chan; + unsigned int scan_count; + unsigned int i, j; + __be16 *tx, *rx; + + kfree(adis->xfer); + kfree(adis->buffer); + + if (adis->data->burst_len) + return adis_update_scan_mode_burst(indio_dev, scan_mask); + + scan_count = indio_dev->scan_bytes / 2; + + adis->xfer = kcalloc(scan_count + 1, sizeof(*adis->xfer), GFP_KERNEL); + if (!adis->xfer) + return -ENOMEM; + + adis->buffer = kcalloc(indio_dev->scan_bytes, 2, GFP_KERNEL); + if (!adis->buffer) { + kfree(adis->xfer); + adis->xfer = NULL; + return -ENOMEM; + } + + rx = adis->buffer; + tx = rx + scan_count; + + spi_message_init(&adis->msg); + + for (j = 0; j <= scan_count; j++) { + adis->xfer[j].bits_per_word = 8; + if (j != scan_count) + adis->xfer[j].cs_change = 1; + adis->xfer[j].len = 2; + adis->xfer[j].delay.value = adis->data->read_delay; + adis->xfer[j].delay.unit = SPI_DELAY_UNIT_USECS; + if (j < scan_count) + adis->xfer[j].tx_buf = &tx[j]; + if (j >= 1) + adis->xfer[j].rx_buf = &rx[j - 1]; + spi_message_add_tail(&adis->xfer[j], &adis->msg); + } + + chan = indio_dev->channels; + for (i = 0; i < indio_dev->num_channels; i++, chan++) { + if (!test_bit(chan->scan_index, scan_mask)) + continue; + if (chan->scan_type.storagebits == 32) + *tx++ = cpu_to_be16((chan->address + 2) << 8); + *tx++ = cpu_to_be16(chan->address << 8); + } + + return 0; +} +EXPORT_SYMBOL_NS_GPL(adis_update_scan_mode, IIO_ADISLIB); + +static irqreturn_t adis_trigger_handler(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + struct adis *adis = iio_device_get_drvdata(indio_dev); + int ret; + + if (adis->data->has_paging) { + mutex_lock(&adis->state_lock); + if (adis->current_page != 0) { + adis->tx[0] = ADIS_WRITE_REG(ADIS_REG_PAGE_ID); + adis->tx[1] = 0; + ret = spi_write(adis->spi, adis->tx, 2); + if (ret) { + dev_err(&adis->spi->dev, "Failed to change device page: %d\n", ret); + mutex_unlock(&adis->state_lock); + goto irq_done; + } + + adis->current_page = 0; + } + } + + ret = spi_sync(adis->spi, &adis->msg); + if (adis->data->has_paging) + mutex_unlock(&adis->state_lock); + if (ret) { + dev_err(&adis->spi->dev, "Failed to read data: %d", ret); + goto irq_done; + } + + iio_push_to_buffers_with_timestamp(indio_dev, adis->buffer, + pf->timestamp); + +irq_done: + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; +} + +static void adis_buffer_cleanup(void *arg) +{ + struct adis *adis = arg; + + kfree(adis->buffer); + kfree(adis->xfer); +} + +/** + * devm_adis_setup_buffer_and_trigger() - Sets up buffer and trigger for + * the managed adis device + * @adis: The adis device + * @indio_dev: The IIO device + * @trigger_handler: Optional trigger handler, may be NULL. + * + * Returns 0 on success, a negative error code otherwise. + * + * This function sets up the buffer and trigger for a adis devices. If + * 'trigger_handler' is NULL the default trigger handler will be used. The + * default trigger handler will simply read the registers assigned to the + * currently active channels. + */ +int +devm_adis_setup_buffer_and_trigger(struct adis *adis, struct iio_dev *indio_dev, + irq_handler_t trigger_handler) +{ + int ret; + + if (!trigger_handler) + trigger_handler = adis_trigger_handler; + + ret = devm_iio_triggered_buffer_setup(&adis->spi->dev, indio_dev, + &iio_pollfunc_store_time, + trigger_handler, NULL); + if (ret) + return ret; + + if (adis->spi->irq) { + ret = devm_adis_probe_trigger(adis, indio_dev); + if (ret) + return ret; + } + + return devm_add_action_or_reset(&adis->spi->dev, adis_buffer_cleanup, + adis); +} +EXPORT_SYMBOL_NS_GPL(devm_adis_setup_buffer_and_trigger, IIO_ADISLIB); + diff --git a/drivers/iio/imu/adis_trigger.c b/drivers/iio/imu/adis_trigger.c new file mode 100644 index 000000000..f890bf842 --- /dev/null +++ b/drivers/iio/imu/adis_trigger.c @@ -0,0 +1,91 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * Common library for ADIS16XXX devices + * + * Copyright 2012 Analog Devices Inc. + * Author: Lars-Peter Clausen + */ + +#include +#include +#include +#include + +#include +#include +#include + +static int adis_data_rdy_trigger_set_state(struct iio_trigger *trig, bool state) +{ + struct adis *adis = iio_trigger_get_drvdata(trig); + + return adis_enable_irq(adis, state); +} + +static const struct iio_trigger_ops adis_trigger_ops = { + .set_trigger_state = &adis_data_rdy_trigger_set_state, +}; + +static int adis_validate_irq_flag(struct adis *adis) +{ + unsigned long direction = adis->irq_flag & IRQF_TRIGGER_MASK; + + /* We cannot mask the interrupt so ensure it's not enabled at request */ + if (adis->data->unmasked_drdy) + adis->irq_flag |= IRQF_NO_AUTOEN; + /* + * Typically this devices have data ready either on the rising edge or + * on the falling edge of the data ready pin. This checks enforces that + * one of those is set in the drivers... It defaults to + * IRQF_TRIGGER_RISING for backward compatibility with devices that + * don't support changing the pin polarity. + */ + if (direction == IRQF_TRIGGER_NONE) { + adis->irq_flag |= IRQF_TRIGGER_RISING; + return 0; + } else if (direction != IRQF_TRIGGER_RISING && + direction != IRQF_TRIGGER_FALLING) { + dev_err(&adis->spi->dev, "Invalid IRQ mask: %08lx\n", + adis->irq_flag); + return -EINVAL; + } + + return 0; +} + +/** + * devm_adis_probe_trigger() - Sets up trigger for a managed adis device + * @adis: The adis device + * @indio_dev: The IIO device + * + * Returns 0 on success or a negative error code + */ +int devm_adis_probe_trigger(struct adis *adis, struct iio_dev *indio_dev) +{ + int ret; + + adis->trig = devm_iio_trigger_alloc(&adis->spi->dev, "%s-dev%d", + indio_dev->name, + iio_device_id(indio_dev)); + if (!adis->trig) + return -ENOMEM; + + adis->trig->ops = &adis_trigger_ops; + iio_trigger_set_drvdata(adis->trig, adis); + + ret = adis_validate_irq_flag(adis); + if (ret) + return ret; + + ret = devm_request_irq(&adis->spi->dev, adis->spi->irq, + &iio_trigger_generic_data_rdy_poll, + adis->irq_flag, + indio_dev->name, + adis->trig); + if (ret) + return ret; + + return devm_iio_trigger_register(&adis->spi->dev, adis->trig); +} +EXPORT_SYMBOL_NS_GPL(devm_adis_probe_trigger, IIO_ADISLIB); + diff --git a/drivers/iio/imu/bmi160/Kconfig b/drivers/iio/imu/bmi160/Kconfig new file mode 100644 index 000000000..9d14d85cc --- /dev/null +++ b/drivers/iio/imu/bmi160/Kconfig @@ -0,0 +1,33 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# BMI160 IMU driver +# + +config BMI160 + tristate + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + +config BMI160_I2C + tristate "Bosch BMI160 I2C driver" + depends on I2C + select BMI160 + select REGMAP_I2C + help + If you say yes here you get support for BMI160 IMU on I2C with + accelerometer, gyroscope and external BMG160 magnetometer. + + This driver can also be built as a module. If so, the module will be + called bmi160_i2c. + +config BMI160_SPI + tristate "Bosch BMI160 SPI driver" + depends on SPI + select BMI160 + select REGMAP_SPI + help + If you say yes here you get support for BMI160 IMU on SPI with + accelerometer, gyroscope and external BMG160 magnetometer. + + This driver can also be built as a module. If so, the module will be + called bmi160_spi. diff --git a/drivers/iio/imu/bmi160/Makefile b/drivers/iio/imu/bmi160/Makefile new file mode 100644 index 000000000..fdcfeddf4 --- /dev/null +++ b/drivers/iio/imu/bmi160/Makefile @@ -0,0 +1,7 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# Makefile for Bosch BMI160 IMU +# +obj-$(CONFIG_BMI160) += bmi160_core.o +obj-$(CONFIG_BMI160_I2C) += bmi160_i2c.o +obj-$(CONFIG_BMI160_SPI) += bmi160_spi.o diff --git a/drivers/iio/imu/bmi160/bmi160.h b/drivers/iio/imu/bmi160/bmi160.h new file mode 100644 index 000000000..32c2ea2d7 --- /dev/null +++ b/drivers/iio/imu/bmi160/bmi160.h @@ -0,0 +1,31 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +#ifndef BMI160_H_ +#define BMI160_H_ + +#include +#include + +struct bmi160_data { + struct regmap *regmap; + struct iio_trigger *trig; + struct regulator_bulk_data supplies[2]; + struct iio_mount_matrix orientation; + /* + * Ensure natural alignment for timestamp if present. + * Max length needed: 2 * 3 channels + 4 bytes padding + 8 byte ts. + * If fewer channels are enabled, less space may be needed, as + * long as the timestamp is still aligned to 8 bytes. + */ + __le16 buf[12] __aligned(8); +}; + +extern const struct regmap_config bmi160_regmap_config; + +int bmi160_core_probe(struct device *dev, struct regmap *regmap, + const char *name, bool use_spi); + +int bmi160_enable_irq(struct regmap *regmap, bool enable); + +int bmi160_probe_trigger(struct iio_dev *indio_dev, int irq, u32 irq_type); + +#endif /* BMI160_H_ */ diff --git a/drivers/iio/imu/bmi160/bmi160_core.c b/drivers/iio/imu/bmi160/bmi160_core.c new file mode 100644 index 000000000..a77f1a834 --- /dev/null +++ b/drivers/iio/imu/bmi160/bmi160_core.c @@ -0,0 +1,891 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * BMI160 - Bosch IMU (accel, gyro plus external magnetometer) + * + * Copyright (c) 2016, Intel Corporation. + * Copyright (c) 2019, Martin Kelly. + * + * IIO core driver for BMI160, with support for I2C/SPI busses + * + * TODO: magnetometer, hardware FIFO + */ +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "bmi160.h" + +#define BMI160_REG_CHIP_ID 0x00 +#define BMI160_CHIP_ID_VAL 0xD1 + +#define BMI160_REG_PMU_STATUS 0x03 + +/* X axis data low byte address, the rest can be obtained using axis offset */ +#define BMI160_REG_DATA_MAGN_XOUT_L 0x04 +#define BMI160_REG_DATA_GYRO_XOUT_L 0x0C +#define BMI160_REG_DATA_ACCEL_XOUT_L 0x12 + +#define BMI160_REG_ACCEL_CONFIG 0x40 +#define BMI160_ACCEL_CONFIG_ODR_MASK GENMASK(3, 0) +#define BMI160_ACCEL_CONFIG_BWP_MASK GENMASK(6, 4) + +#define BMI160_REG_ACCEL_RANGE 0x41 +#define BMI160_ACCEL_RANGE_2G 0x03 +#define BMI160_ACCEL_RANGE_4G 0x05 +#define BMI160_ACCEL_RANGE_8G 0x08 +#define BMI160_ACCEL_RANGE_16G 0x0C + +#define BMI160_REG_GYRO_CONFIG 0x42 +#define BMI160_GYRO_CONFIG_ODR_MASK GENMASK(3, 0) +#define BMI160_GYRO_CONFIG_BWP_MASK GENMASK(5, 4) + +#define BMI160_REG_GYRO_RANGE 0x43 +#define BMI160_GYRO_RANGE_2000DPS 0x00 +#define BMI160_GYRO_RANGE_1000DPS 0x01 +#define BMI160_GYRO_RANGE_500DPS 0x02 +#define BMI160_GYRO_RANGE_250DPS 0x03 +#define BMI160_GYRO_RANGE_125DPS 0x04 + +#define BMI160_REG_CMD 0x7E +#define BMI160_CMD_ACCEL_PM_SUSPEND 0x10 +#define BMI160_CMD_ACCEL_PM_NORMAL 0x11 +#define BMI160_CMD_ACCEL_PM_LOW_POWER 0x12 +#define BMI160_CMD_GYRO_PM_SUSPEND 0x14 +#define BMI160_CMD_GYRO_PM_NORMAL 0x15 +#define BMI160_CMD_GYRO_PM_FAST_STARTUP 0x17 +#define BMI160_CMD_SOFTRESET 0xB6 + +#define BMI160_REG_INT_EN 0x51 +#define BMI160_DRDY_INT_EN BIT(4) + +#define BMI160_REG_INT_OUT_CTRL 0x53 +#define BMI160_INT_OUT_CTRL_MASK 0x0f +#define BMI160_INT1_OUT_CTRL_SHIFT 0 +#define BMI160_INT2_OUT_CTRL_SHIFT 4 +#define BMI160_EDGE_TRIGGERED BIT(0) +#define BMI160_ACTIVE_HIGH BIT(1) +#define BMI160_OPEN_DRAIN BIT(2) +#define BMI160_OUTPUT_EN BIT(3) + +#define BMI160_REG_INT_LATCH 0x54 +#define BMI160_INT1_LATCH_MASK BIT(4) +#define BMI160_INT2_LATCH_MASK BIT(5) + +/* INT1 and INT2 are in the opposite order as in INT_OUT_CTRL! */ +#define BMI160_REG_INT_MAP 0x56 +#define BMI160_INT1_MAP_DRDY_EN 0x80 +#define BMI160_INT2_MAP_DRDY_EN 0x08 + +#define BMI160_REG_DUMMY 0x7F + +#define BMI160_NORMAL_WRITE_USLEEP 2 +#define BMI160_SUSPENDED_WRITE_USLEEP 450 + +#define BMI160_ACCEL_PMU_MIN_USLEEP 3800 +#define BMI160_GYRO_PMU_MIN_USLEEP 80000 +#define BMI160_SOFTRESET_USLEEP 1000 + +#define BMI160_CHANNEL(_type, _axis, _index) { \ + .type = _type, \ + .modified = 1, \ + .channel2 = IIO_MOD_##_axis, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \ + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) | \ + BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .scan_index = _index, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_LE, \ + }, \ + .ext_info = bmi160_ext_info, \ +} + +/* scan indexes follow DATA register order */ +enum bmi160_scan_axis { + BMI160_SCAN_EXT_MAGN_X = 0, + BMI160_SCAN_EXT_MAGN_Y, + BMI160_SCAN_EXT_MAGN_Z, + BMI160_SCAN_RHALL, + BMI160_SCAN_GYRO_X, + BMI160_SCAN_GYRO_Y, + BMI160_SCAN_GYRO_Z, + BMI160_SCAN_ACCEL_X, + BMI160_SCAN_ACCEL_Y, + BMI160_SCAN_ACCEL_Z, + BMI160_SCAN_TIMESTAMP, +}; + +enum bmi160_sensor_type { + BMI160_ACCEL = 0, + BMI160_GYRO, + BMI160_EXT_MAGN, + BMI160_NUM_SENSORS /* must be last */ +}; + +enum bmi160_int_pin { + BMI160_PIN_INT1, + BMI160_PIN_INT2 +}; + +const struct regmap_config bmi160_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; +EXPORT_SYMBOL_NS(bmi160_regmap_config, IIO_BMI160); + +struct bmi160_regs { + u8 data; /* LSB byte register for X-axis */ + u8 config; + u8 config_odr_mask; + u8 config_bwp_mask; + u8 range; + u8 pmu_cmd_normal; + u8 pmu_cmd_suspend; +}; + +static struct bmi160_regs bmi160_regs[] = { + [BMI160_ACCEL] = { + .data = BMI160_REG_DATA_ACCEL_XOUT_L, + .config = BMI160_REG_ACCEL_CONFIG, + .config_odr_mask = BMI160_ACCEL_CONFIG_ODR_MASK, + .config_bwp_mask = BMI160_ACCEL_CONFIG_BWP_MASK, + .range = BMI160_REG_ACCEL_RANGE, + .pmu_cmd_normal = BMI160_CMD_ACCEL_PM_NORMAL, + .pmu_cmd_suspend = BMI160_CMD_ACCEL_PM_SUSPEND, + }, + [BMI160_GYRO] = { + .data = BMI160_REG_DATA_GYRO_XOUT_L, + .config = BMI160_REG_GYRO_CONFIG, + .config_odr_mask = BMI160_GYRO_CONFIG_ODR_MASK, + .config_bwp_mask = BMI160_GYRO_CONFIG_BWP_MASK, + .range = BMI160_REG_GYRO_RANGE, + .pmu_cmd_normal = BMI160_CMD_GYRO_PM_NORMAL, + .pmu_cmd_suspend = BMI160_CMD_GYRO_PM_SUSPEND, + }, +}; + +static unsigned long bmi160_pmu_time[] = { + [BMI160_ACCEL] = BMI160_ACCEL_PMU_MIN_USLEEP, + [BMI160_GYRO] = BMI160_GYRO_PMU_MIN_USLEEP, +}; + +struct bmi160_scale { + u8 bits; + int uscale; +}; + +struct bmi160_odr { + u8 bits; + int odr; + int uodr; +}; + +static const struct bmi160_scale bmi160_accel_scale[] = { + { BMI160_ACCEL_RANGE_2G, 598}, + { BMI160_ACCEL_RANGE_4G, 1197}, + { BMI160_ACCEL_RANGE_8G, 2394}, + { BMI160_ACCEL_RANGE_16G, 4788}, +}; + +static const struct bmi160_scale bmi160_gyro_scale[] = { + { BMI160_GYRO_RANGE_2000DPS, 1065}, + { BMI160_GYRO_RANGE_1000DPS, 532}, + { BMI160_GYRO_RANGE_500DPS, 266}, + { BMI160_GYRO_RANGE_250DPS, 133}, + { BMI160_GYRO_RANGE_125DPS, 66}, +}; + +struct bmi160_scale_item { + const struct bmi160_scale *tbl; + int num; +}; + +static const struct bmi160_scale_item bmi160_scale_table[] = { + [BMI160_ACCEL] = { + .tbl = bmi160_accel_scale, + .num = ARRAY_SIZE(bmi160_accel_scale), + }, + [BMI160_GYRO] = { + .tbl = bmi160_gyro_scale, + .num = ARRAY_SIZE(bmi160_gyro_scale), + }, +}; + +static const struct bmi160_odr bmi160_accel_odr[] = { + {0x01, 0, 781250}, + {0x02, 1, 562500}, + {0x03, 3, 125000}, + {0x04, 6, 250000}, + {0x05, 12, 500000}, + {0x06, 25, 0}, + {0x07, 50, 0}, + {0x08, 100, 0}, + {0x09, 200, 0}, + {0x0A, 400, 0}, + {0x0B, 800, 0}, + {0x0C, 1600, 0}, +}; + +static const struct bmi160_odr bmi160_gyro_odr[] = { + {0x06, 25, 0}, + {0x07, 50, 0}, + {0x08, 100, 0}, + {0x09, 200, 0}, + {0x0A, 400, 0}, + {0x0B, 800, 0}, + {0x0C, 1600, 0}, + {0x0D, 3200, 0}, +}; + +struct bmi160_odr_item { + const struct bmi160_odr *tbl; + int num; +}; + +static const struct bmi160_odr_item bmi160_odr_table[] = { + [BMI160_ACCEL] = { + .tbl = bmi160_accel_odr, + .num = ARRAY_SIZE(bmi160_accel_odr), + }, + [BMI160_GYRO] = { + .tbl = bmi160_gyro_odr, + .num = ARRAY_SIZE(bmi160_gyro_odr), + }, +}; + +static const struct iio_mount_matrix * +bmi160_get_mount_matrix(const struct iio_dev *indio_dev, + const struct iio_chan_spec *chan) +{ + struct bmi160_data *data = iio_priv(indio_dev); + + return &data->orientation; +} + +static const struct iio_chan_spec_ext_info bmi160_ext_info[] = { + IIO_MOUNT_MATRIX(IIO_SHARED_BY_DIR, bmi160_get_mount_matrix), + { } +}; + +static const struct iio_chan_spec bmi160_channels[] = { + BMI160_CHANNEL(IIO_ACCEL, X, BMI160_SCAN_ACCEL_X), + BMI160_CHANNEL(IIO_ACCEL, Y, BMI160_SCAN_ACCEL_Y), + BMI160_CHANNEL(IIO_ACCEL, Z, BMI160_SCAN_ACCEL_Z), + BMI160_CHANNEL(IIO_ANGL_VEL, X, BMI160_SCAN_GYRO_X), + BMI160_CHANNEL(IIO_ANGL_VEL, Y, BMI160_SCAN_GYRO_Y), + BMI160_CHANNEL(IIO_ANGL_VEL, Z, BMI160_SCAN_GYRO_Z), + IIO_CHAN_SOFT_TIMESTAMP(BMI160_SCAN_TIMESTAMP), +}; + +static enum bmi160_sensor_type bmi160_to_sensor(enum iio_chan_type iio_type) +{ + switch (iio_type) { + case IIO_ACCEL: + return BMI160_ACCEL; + case IIO_ANGL_VEL: + return BMI160_GYRO; + default: + return -EINVAL; + } +} + +static +int bmi160_set_mode(struct bmi160_data *data, enum bmi160_sensor_type t, + bool mode) +{ + int ret; + u8 cmd; + + if (mode) + cmd = bmi160_regs[t].pmu_cmd_normal; + else + cmd = bmi160_regs[t].pmu_cmd_suspend; + + ret = regmap_write(data->regmap, BMI160_REG_CMD, cmd); + if (ret) + return ret; + + usleep_range(bmi160_pmu_time[t], bmi160_pmu_time[t] + 1000); + + return 0; +} + +static +int bmi160_set_scale(struct bmi160_data *data, enum bmi160_sensor_type t, + int uscale) +{ + int i; + + for (i = 0; i < bmi160_scale_table[t].num; i++) + if (bmi160_scale_table[t].tbl[i].uscale == uscale) + break; + + if (i == bmi160_scale_table[t].num) + return -EINVAL; + + return regmap_write(data->regmap, bmi160_regs[t].range, + bmi160_scale_table[t].tbl[i].bits); +} + +static +int bmi160_get_scale(struct bmi160_data *data, enum bmi160_sensor_type t, + int *uscale) +{ + int i, ret, val; + + ret = regmap_read(data->regmap, bmi160_regs[t].range, &val); + if (ret) + return ret; + + for (i = 0; i < bmi160_scale_table[t].num; i++) + if (bmi160_scale_table[t].tbl[i].bits == val) { + *uscale = bmi160_scale_table[t].tbl[i].uscale; + return 0; + } + + return -EINVAL; +} + +static int bmi160_get_data(struct bmi160_data *data, int chan_type, + int axis, int *val) +{ + u8 reg; + int ret; + __le16 sample; + enum bmi160_sensor_type t = bmi160_to_sensor(chan_type); + + reg = bmi160_regs[t].data + (axis - IIO_MOD_X) * sizeof(sample); + + ret = regmap_bulk_read(data->regmap, reg, &sample, sizeof(sample)); + if (ret) + return ret; + + *val = sign_extend32(le16_to_cpu(sample), 15); + + return 0; +} + +static +int bmi160_set_odr(struct bmi160_data *data, enum bmi160_sensor_type t, + int odr, int uodr) +{ + int i; + + for (i = 0; i < bmi160_odr_table[t].num; i++) + if (bmi160_odr_table[t].tbl[i].odr == odr && + bmi160_odr_table[t].tbl[i].uodr == uodr) + break; + + if (i >= bmi160_odr_table[t].num) + return -EINVAL; + + return regmap_update_bits(data->regmap, + bmi160_regs[t].config, + bmi160_regs[t].config_odr_mask, + bmi160_odr_table[t].tbl[i].bits); +} + +static int bmi160_get_odr(struct bmi160_data *data, enum bmi160_sensor_type t, + int *odr, int *uodr) +{ + int i, val, ret; + + ret = regmap_read(data->regmap, bmi160_regs[t].config, &val); + if (ret) + return ret; + + val &= bmi160_regs[t].config_odr_mask; + + for (i = 0; i < bmi160_odr_table[t].num; i++) + if (val == bmi160_odr_table[t].tbl[i].bits) + break; + + if (i >= bmi160_odr_table[t].num) + return -EINVAL; + + *odr = bmi160_odr_table[t].tbl[i].odr; + *uodr = bmi160_odr_table[t].tbl[i].uodr; + + return 0; +} + +static irqreturn_t bmi160_trigger_handler(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + struct bmi160_data *data = iio_priv(indio_dev); + int i, ret, j = 0, base = BMI160_REG_DATA_MAGN_XOUT_L; + __le16 sample; + + for_each_set_bit(i, indio_dev->active_scan_mask, + indio_dev->masklength) { + ret = regmap_bulk_read(data->regmap, base + i * sizeof(sample), + &sample, sizeof(sample)); + if (ret) + goto done; + data->buf[j++] = sample; + } + + iio_push_to_buffers_with_timestamp(indio_dev, data->buf, pf->timestamp); +done: + iio_trigger_notify_done(indio_dev->trig); + return IRQ_HANDLED; +} + +static int bmi160_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask) +{ + int ret; + struct bmi160_data *data = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_RAW: + ret = bmi160_get_data(data, chan->type, chan->channel2, val); + if (ret) + return ret; + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + *val = 0; + ret = bmi160_get_scale(data, + bmi160_to_sensor(chan->type), val2); + return ret ? ret : IIO_VAL_INT_PLUS_MICRO; + case IIO_CHAN_INFO_SAMP_FREQ: + ret = bmi160_get_odr(data, bmi160_to_sensor(chan->type), + val, val2); + return ret ? ret : IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } + + return 0; +} + +static int bmi160_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct bmi160_data *data = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + return bmi160_set_scale(data, + bmi160_to_sensor(chan->type), val2); + case IIO_CHAN_INFO_SAMP_FREQ: + return bmi160_set_odr(data, bmi160_to_sensor(chan->type), + val, val2); + default: + return -EINVAL; + } + + return 0; +} + +static +IIO_CONST_ATTR(in_accel_sampling_frequency_available, + "0.78125 1.5625 3.125 6.25 12.5 25 50 100 200 400 800 1600"); +static +IIO_CONST_ATTR(in_anglvel_sampling_frequency_available, + "25 50 100 200 400 800 1600 3200"); +static +IIO_CONST_ATTR(in_accel_scale_available, + "0.000598 0.001197 0.002394 0.004788"); +static +IIO_CONST_ATTR(in_anglvel_scale_available, + "0.001065 0.000532 0.000266 0.000133 0.000066"); + +static struct attribute *bmi160_attrs[] = { + &iio_const_attr_in_accel_sampling_frequency_available.dev_attr.attr, + &iio_const_attr_in_anglvel_sampling_frequency_available.dev_attr.attr, + &iio_const_attr_in_accel_scale_available.dev_attr.attr, + &iio_const_attr_in_anglvel_scale_available.dev_attr.attr, + NULL, +}; + +static const struct attribute_group bmi160_attrs_group = { + .attrs = bmi160_attrs, +}; + +static const struct iio_info bmi160_info = { + .read_raw = bmi160_read_raw, + .write_raw = bmi160_write_raw, + .attrs = &bmi160_attrs_group, +}; + +static int bmi160_write_conf_reg(struct regmap *regmap, unsigned int reg, + unsigned int mask, unsigned int bits, + unsigned int write_usleep) +{ + int ret; + unsigned int val; + + ret = regmap_read(regmap, reg, &val); + if (ret) + return ret; + + val = (val & ~mask) | bits; + + ret = regmap_write(regmap, reg, val); + if (ret) + return ret; + + /* + * We need to wait after writing before we can write again. See the + * datasheet, page 93. + */ + usleep_range(write_usleep, write_usleep + 1000); + + return 0; +} + +static int bmi160_config_pin(struct regmap *regmap, enum bmi160_int_pin pin, + bool open_drain, u8 irq_mask, + unsigned long write_usleep) +{ + int ret; + struct device *dev = regmap_get_device(regmap); + u8 int_out_ctrl_shift; + u8 int_latch_mask; + u8 int_map_mask; + u8 int_out_ctrl_mask; + u8 int_out_ctrl_bits; + const char *pin_name; + + switch (pin) { + case BMI160_PIN_INT1: + int_out_ctrl_shift = BMI160_INT1_OUT_CTRL_SHIFT; + int_latch_mask = BMI160_INT1_LATCH_MASK; + int_map_mask = BMI160_INT1_MAP_DRDY_EN; + break; + case BMI160_PIN_INT2: + int_out_ctrl_shift = BMI160_INT2_OUT_CTRL_SHIFT; + int_latch_mask = BMI160_INT2_LATCH_MASK; + int_map_mask = BMI160_INT2_MAP_DRDY_EN; + break; + } + int_out_ctrl_mask = BMI160_INT_OUT_CTRL_MASK << int_out_ctrl_shift; + + /* + * Enable the requested pin with the right settings: + * - Push-pull/open-drain + * - Active low/high + * - Edge/level triggered + */ + int_out_ctrl_bits = BMI160_OUTPUT_EN; + if (open_drain) + /* Default is push-pull. */ + int_out_ctrl_bits |= BMI160_OPEN_DRAIN; + int_out_ctrl_bits |= irq_mask; + int_out_ctrl_bits <<= int_out_ctrl_shift; + + ret = bmi160_write_conf_reg(regmap, BMI160_REG_INT_OUT_CTRL, + int_out_ctrl_mask, int_out_ctrl_bits, + write_usleep); + if (ret) + return ret; + + /* Set the pin to input mode with no latching. */ + ret = bmi160_write_conf_reg(regmap, BMI160_REG_INT_LATCH, + int_latch_mask, int_latch_mask, + write_usleep); + if (ret) + return ret; + + /* Map interrupts to the requested pin. */ + ret = bmi160_write_conf_reg(regmap, BMI160_REG_INT_MAP, + int_map_mask, int_map_mask, + write_usleep); + if (ret) { + switch (pin) { + case BMI160_PIN_INT1: + pin_name = "INT1"; + break; + case BMI160_PIN_INT2: + pin_name = "INT2"; + break; + } + dev_err(dev, "Failed to configure %s IRQ pin", pin_name); + } + + return ret; +} + +int bmi160_enable_irq(struct regmap *regmap, bool enable) +{ + unsigned int enable_bit = 0; + + if (enable) + enable_bit = BMI160_DRDY_INT_EN; + + return bmi160_write_conf_reg(regmap, BMI160_REG_INT_EN, + BMI160_DRDY_INT_EN, enable_bit, + BMI160_NORMAL_WRITE_USLEEP); +} +EXPORT_SYMBOL_NS(bmi160_enable_irq, IIO_BMI160); + +static int bmi160_get_irq(struct fwnode_handle *fwnode, enum bmi160_int_pin *pin) +{ + int irq; + + /* Use INT1 if possible, otherwise fall back to INT2. */ + irq = fwnode_irq_get_byname(fwnode, "INT1"); + if (irq > 0) { + *pin = BMI160_PIN_INT1; + return irq; + } + + irq = fwnode_irq_get_byname(fwnode, "INT2"); + if (irq > 0) + *pin = BMI160_PIN_INT2; + + return irq; +} + +static int bmi160_config_device_irq(struct iio_dev *indio_dev, int irq_type, + enum bmi160_int_pin pin) +{ + bool open_drain; + u8 irq_mask; + struct bmi160_data *data = iio_priv(indio_dev); + struct device *dev = regmap_get_device(data->regmap); + + /* Level-triggered, active-low is the default if we set all zeroes. */ + if (irq_type == IRQF_TRIGGER_RISING) + irq_mask = BMI160_ACTIVE_HIGH | BMI160_EDGE_TRIGGERED; + else if (irq_type == IRQF_TRIGGER_FALLING) + irq_mask = BMI160_EDGE_TRIGGERED; + else if (irq_type == IRQF_TRIGGER_HIGH) + irq_mask = BMI160_ACTIVE_HIGH; + else if (irq_type == IRQF_TRIGGER_LOW) + irq_mask = 0; + else { + dev_err(&indio_dev->dev, + "Invalid interrupt type 0x%x specified\n", irq_type); + return -EINVAL; + } + + open_drain = device_property_read_bool(dev, "drive-open-drain"); + + return bmi160_config_pin(data->regmap, pin, open_drain, irq_mask, + BMI160_NORMAL_WRITE_USLEEP); +} + +static int bmi160_setup_irq(struct iio_dev *indio_dev, int irq, + enum bmi160_int_pin pin) +{ + struct irq_data *desc; + u32 irq_type; + int ret; + + desc = irq_get_irq_data(irq); + if (!desc) { + dev_err(&indio_dev->dev, "Could not find IRQ %d\n", irq); + return -EINVAL; + } + + irq_type = irqd_get_trigger_type(desc); + + ret = bmi160_config_device_irq(indio_dev, irq_type, pin); + if (ret) + return ret; + + return bmi160_probe_trigger(indio_dev, irq, irq_type); +} + +static int bmi160_chip_init(struct bmi160_data *data, bool use_spi) +{ + int ret; + unsigned int val; + struct device *dev = regmap_get_device(data->regmap); + + ret = regulator_bulk_enable(ARRAY_SIZE(data->supplies), data->supplies); + if (ret) { + dev_err(dev, "Failed to enable regulators: %d\n", ret); + return ret; + } + + ret = regmap_write(data->regmap, BMI160_REG_CMD, BMI160_CMD_SOFTRESET); + if (ret) + goto disable_regulator; + + usleep_range(BMI160_SOFTRESET_USLEEP, BMI160_SOFTRESET_USLEEP + 1); + + /* + * CS rising edge is needed before starting SPI, so do a dummy read + * See Section 3.2.1, page 86 of the datasheet + */ + if (use_spi) { + ret = regmap_read(data->regmap, BMI160_REG_DUMMY, &val); + if (ret) + goto disable_regulator; + } + + ret = regmap_read(data->regmap, BMI160_REG_CHIP_ID, &val); + if (ret) { + dev_err(dev, "Error reading chip id\n"); + goto disable_regulator; + } + if (val != BMI160_CHIP_ID_VAL) { + dev_err(dev, "Wrong chip id, got %x expected %x\n", + val, BMI160_CHIP_ID_VAL); + ret = -ENODEV; + goto disable_regulator; + } + + ret = bmi160_set_mode(data, BMI160_ACCEL, true); + if (ret) + goto disable_regulator; + + ret = bmi160_set_mode(data, BMI160_GYRO, true); + if (ret) + goto disable_accel; + + return 0; + +disable_accel: + bmi160_set_mode(data, BMI160_ACCEL, false); + +disable_regulator: + regulator_bulk_disable(ARRAY_SIZE(data->supplies), data->supplies); + return ret; +} + +static int bmi160_data_rdy_trigger_set_state(struct iio_trigger *trig, + bool enable) +{ + struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig); + struct bmi160_data *data = iio_priv(indio_dev); + + return bmi160_enable_irq(data->regmap, enable); +} + +static const struct iio_trigger_ops bmi160_trigger_ops = { + .set_trigger_state = &bmi160_data_rdy_trigger_set_state, +}; + +int bmi160_probe_trigger(struct iio_dev *indio_dev, int irq, u32 irq_type) +{ + struct bmi160_data *data = iio_priv(indio_dev); + int ret; + + data->trig = devm_iio_trigger_alloc(&indio_dev->dev, "%s-dev%d", + indio_dev->name, + iio_device_id(indio_dev)); + + if (data->trig == NULL) + return -ENOMEM; + + ret = devm_request_irq(&indio_dev->dev, irq, + &iio_trigger_generic_data_rdy_poll, + irq_type, "bmi160", data->trig); + if (ret) + return ret; + + data->trig->dev.parent = regmap_get_device(data->regmap); + data->trig->ops = &bmi160_trigger_ops; + iio_trigger_set_drvdata(data->trig, indio_dev); + + ret = devm_iio_trigger_register(&indio_dev->dev, data->trig); + if (ret) + return ret; + + indio_dev->trig = iio_trigger_get(data->trig); + + return 0; +} + +static void bmi160_chip_uninit(void *data) +{ + struct bmi160_data *bmi_data = data; + struct device *dev = regmap_get_device(bmi_data->regmap); + int ret; + + bmi160_set_mode(bmi_data, BMI160_GYRO, false); + bmi160_set_mode(bmi_data, BMI160_ACCEL, false); + + ret = regulator_bulk_disable(ARRAY_SIZE(bmi_data->supplies), + bmi_data->supplies); + if (ret) + dev_err(dev, "Failed to disable regulators: %d\n", ret); +} + +int bmi160_core_probe(struct device *dev, struct regmap *regmap, + const char *name, bool use_spi) +{ + struct iio_dev *indio_dev; + struct bmi160_data *data; + int irq; + enum bmi160_int_pin int_pin; + int ret; + + indio_dev = devm_iio_device_alloc(dev, sizeof(*data)); + if (!indio_dev) + return -ENOMEM; + + data = iio_priv(indio_dev); + dev_set_drvdata(dev, indio_dev); + data->regmap = regmap; + + data->supplies[0].supply = "vdd"; + data->supplies[1].supply = "vddio"; + ret = devm_regulator_bulk_get(dev, + ARRAY_SIZE(data->supplies), + data->supplies); + if (ret) { + dev_err(dev, "Failed to get regulators: %d\n", ret); + return ret; + } + + ret = iio_read_mount_matrix(dev, &data->orientation); + if (ret) + return ret; + + ret = bmi160_chip_init(data, use_spi); + if (ret) + return ret; + + ret = devm_add_action_or_reset(dev, bmi160_chip_uninit, data); + if (ret) + return ret; + + indio_dev->channels = bmi160_channels; + indio_dev->num_channels = ARRAY_SIZE(bmi160_channels); + indio_dev->name = name; + indio_dev->modes = INDIO_DIRECT_MODE; + indio_dev->info = &bmi160_info; + + ret = devm_iio_triggered_buffer_setup(dev, indio_dev, + iio_pollfunc_store_time, + bmi160_trigger_handler, NULL); + if (ret) + return ret; + + irq = bmi160_get_irq(dev_fwnode(dev), &int_pin); + if (irq > 0) { + ret = bmi160_setup_irq(indio_dev, irq, int_pin); + if (ret) + dev_err(&indio_dev->dev, "Failed to setup IRQ %d\n", + irq); + } else { + dev_info(&indio_dev->dev, "Not setting up IRQ trigger\n"); + } + + return devm_iio_device_register(dev, indio_dev); +} +EXPORT_SYMBOL_NS_GPL(bmi160_core_probe, IIO_BMI160); + +MODULE_AUTHOR("Daniel Baluta "); +MODULE_DESCRIPTION("Bosch BMI160 driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/imu/bmi160/bmi160_i2c.c b/drivers/iio/imu/bmi160/bmi160_i2c.c new file mode 100644 index 000000000..d93f4fa2a --- /dev/null +++ b/drivers/iio/imu/bmi160/bmi160_i2c.c @@ -0,0 +1,71 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * BMI160 - Bosch IMU, I2C bits + * + * Copyright (c) 2016, Intel Corporation. + * + * 7-bit I2C slave address is: + * - 0x68 if SDO is pulled to GND + * - 0x69 if SDO is pulled to VDDIO + */ +#include +#include +#include +#include + +#include "bmi160.h" + +static int bmi160_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct regmap *regmap; + const char *name; + + regmap = devm_regmap_init_i2c(client, &bmi160_regmap_config); + if (IS_ERR(regmap)) { + dev_err(&client->dev, "Failed to register i2c regmap: %pe\n", + regmap); + return PTR_ERR(regmap); + } + + if (id) + name = id->name; + else + name = dev_name(&client->dev); + + return bmi160_core_probe(&client->dev, regmap, name, false); +} + +static const struct i2c_device_id bmi160_i2c_id[] = { + {"bmi160", 0}, + {} +}; +MODULE_DEVICE_TABLE(i2c, bmi160_i2c_id); + +static const struct acpi_device_id bmi160_acpi_match[] = { + {"BMI0160", 0}, + { }, +}; +MODULE_DEVICE_TABLE(acpi, bmi160_acpi_match); + +static const struct of_device_id bmi160_of_match[] = { + { .compatible = "bosch,bmi160" }, + { }, +}; +MODULE_DEVICE_TABLE(of, bmi160_of_match); + +static struct i2c_driver bmi160_i2c_driver = { + .driver = { + .name = "bmi160_i2c", + .acpi_match_table = bmi160_acpi_match, + .of_match_table = bmi160_of_match, + }, + .probe = bmi160_i2c_probe, + .id_table = bmi160_i2c_id, +}; +module_i2c_driver(bmi160_i2c_driver); + +MODULE_AUTHOR("Daniel Baluta "); +MODULE_DESCRIPTION("BMI160 I2C driver"); +MODULE_LICENSE("GPL v2"); +MODULE_IMPORT_NS(IIO_BMI160); diff --git a/drivers/iio/imu/bmi160/bmi160_spi.c b/drivers/iio/imu/bmi160/bmi160_spi.c new file mode 100644 index 000000000..8b573ea99 --- /dev/null +++ b/drivers/iio/imu/bmi160/bmi160_spi.c @@ -0,0 +1,68 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * BMI160 - Bosch IMU, SPI bits + * + * Copyright (c) 2016, Intel Corporation. + * + */ +#include +#include +#include +#include + +#include "bmi160.h" + +static int bmi160_spi_probe(struct spi_device *spi) +{ + struct regmap *regmap; + const struct spi_device_id *id = spi_get_device_id(spi); + const char *name; + + regmap = devm_regmap_init_spi(spi, &bmi160_regmap_config); + if (IS_ERR(regmap)) { + dev_err(&spi->dev, "Failed to register spi regmap: %pe\n", + regmap); + return PTR_ERR(regmap); + } + + if (id) + name = id->name; + else + name = dev_name(&spi->dev); + + return bmi160_core_probe(&spi->dev, regmap, name, true); +} + +static const struct spi_device_id bmi160_spi_id[] = { + {"bmi160", 0}, + {} +}; +MODULE_DEVICE_TABLE(spi, bmi160_spi_id); + +static const struct acpi_device_id bmi160_acpi_match[] = { + {"BMI0160", 0}, + { }, +}; +MODULE_DEVICE_TABLE(acpi, bmi160_acpi_match); + +static const struct of_device_id bmi160_of_match[] = { + { .compatible = "bosch,bmi160" }, + { }, +}; +MODULE_DEVICE_TABLE(of, bmi160_of_match); + +static struct spi_driver bmi160_spi_driver = { + .probe = bmi160_spi_probe, + .id_table = bmi160_spi_id, + .driver = { + .acpi_match_table = bmi160_acpi_match, + .of_match_table = bmi160_of_match, + .name = "bmi160_spi", + }, +}; +module_spi_driver(bmi160_spi_driver); + +MODULE_AUTHOR("Daniel Baluta + * + * Portions of this driver are taken from the BNO055 driver patch + * from Vlad Dogaru which is Copyright (c) 2016, Intel Corporation. + * + * This driver is also based on BMI160 driver, which is: + * Copyright (c) 2016, Intel Corporation. + * Copyright (c) 2019, Martin Kelly. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "bno055.h" + +#define BNO055_FW_UID_FMT "bno055-caldata-%*phN.dat" +#define BNO055_FW_GENERIC_NAME "bno055-caldata.dat" + +/* common registers */ +#define BNO055_PAGESEL_REG 0x7 + +/* page 0 registers */ +#define BNO055_CHIP_ID_REG 0x0 +#define BNO055_CHIP_ID_MAGIC 0xA0 +#define BNO055_SW_REV_LSB_REG 0x4 +#define BNO055_SW_REV_MSB_REG 0x5 +#define BNO055_ACC_DATA_X_LSB_REG 0x8 +#define BNO055_ACC_DATA_Y_LSB_REG 0xA +#define BNO055_ACC_DATA_Z_LSB_REG 0xC +#define BNO055_MAG_DATA_X_LSB_REG 0xE +#define BNO055_MAG_DATA_Y_LSB_REG 0x10 +#define BNO055_MAG_DATA_Z_LSB_REG 0x12 +#define BNO055_GYR_DATA_X_LSB_REG 0x14 +#define BNO055_GYR_DATA_Y_LSB_REG 0x16 +#define BNO055_GYR_DATA_Z_LSB_REG 0x18 +#define BNO055_EUL_DATA_X_LSB_REG 0x1A +#define BNO055_EUL_DATA_Y_LSB_REG 0x1C +#define BNO055_EUL_DATA_Z_LSB_REG 0x1E +#define BNO055_QUAT_DATA_W_LSB_REG 0x20 +#define BNO055_LIA_DATA_X_LSB_REG 0x28 +#define BNO055_LIA_DATA_Y_LSB_REG 0x2A +#define BNO055_LIA_DATA_Z_LSB_REG 0x2C +#define BNO055_GRAVITY_DATA_X_LSB_REG 0x2E +#define BNO055_GRAVITY_DATA_Y_LSB_REG 0x30 +#define BNO055_GRAVITY_DATA_Z_LSB_REG 0x32 +#define BNO055_SCAN_CH_COUNT ((BNO055_GRAVITY_DATA_Z_LSB_REG - BNO055_ACC_DATA_X_LSB_REG) / 2) +#define BNO055_TEMP_REG 0x34 +#define BNO055_CALIB_STAT_REG 0x35 +#define BNO055_CALIB_STAT_MAGN_SHIFT 0 +#define BNO055_CALIB_STAT_ACCEL_SHIFT 2 +#define BNO055_CALIB_STAT_GYRO_SHIFT 4 +#define BNO055_CALIB_STAT_SYS_SHIFT 6 +#define BNO055_SYS_ERR_REG 0x3A +#define BNO055_POWER_MODE_REG 0x3E +#define BNO055_POWER_MODE_NORMAL 0 +#define BNO055_SYS_TRIGGER_REG 0x3F +#define BNO055_SYS_TRIGGER_RST_SYS BIT(5) +#define BNO055_SYS_TRIGGER_CLK_SEL BIT(7) +#define BNO055_OPR_MODE_REG 0x3D +#define BNO055_OPR_MODE_CONFIG 0x0 +#define BNO055_OPR_MODE_AMG 0x7 +#define BNO055_OPR_MODE_FUSION_FMC_OFF 0xB +#define BNO055_OPR_MODE_FUSION 0xC +#define BNO055_UNIT_SEL_REG 0x3B +/* Android orientation mode means: pitch value decreases turning clockwise */ +#define BNO055_UNIT_SEL_ANDROID BIT(7) +#define BNO055_UNIT_SEL_GYR_RPS BIT(1) +#define BNO055_CALDATA_START 0x55 +#define BNO055_CALDATA_END 0x6A +#define BNO055_CALDATA_LEN 22 + +/* + * The difference in address between the register that contains the + * value and the register that contains the offset. This applies for + * accel, gyro and magn channels. + */ +#define BNO055_REG_OFFSET_ADDR 0x4D + +/* page 1 registers */ +#define BNO055_PG1(x) ((x) | 0x80) +#define BNO055_ACC_CONFIG_REG BNO055_PG1(0x8) +#define BNO055_ACC_CONFIG_LPF_MASK GENMASK(4, 2) +#define BNO055_ACC_CONFIG_RANGE_MASK GENMASK(1, 0) +#define BNO055_MAG_CONFIG_REG BNO055_PG1(0x9) +#define BNO055_MAG_CONFIG_HIGHACCURACY 0x18 +#define BNO055_MAG_CONFIG_ODR_MASK GENMASK(2, 0) +#define BNO055_GYR_CONFIG_REG BNO055_PG1(0xA) +#define BNO055_GYR_CONFIG_RANGE_MASK GENMASK(2, 0) +#define BNO055_GYR_CONFIG_LPF_MASK GENMASK(5, 3) +#define BNO055_GYR_AM_SET_REG BNO055_PG1(0x1F) +#define BNO055_UID_LOWER_REG BNO055_PG1(0x50) +#define BNO055_UID_HIGHER_REG BNO055_PG1(0x5F) +#define BNO055_UID_LEN 16 + +struct bno055_sysfs_attr { + int *vals; + int len; + int *fusion_vals; + int *hw_xlate; + int type; +}; + +static int bno055_acc_lpf_vals[] = { + 7, 810000, 15, 630000, 31, 250000, 62, 500000, + 125, 0, 250, 0, 500, 0, 1000, 0, +}; + +static struct bno055_sysfs_attr bno055_acc_lpf = { + .vals = bno055_acc_lpf_vals, + .len = ARRAY_SIZE(bno055_acc_lpf_vals), + .fusion_vals = (int[]){62, 500000}, + .type = IIO_VAL_INT_PLUS_MICRO, +}; + +static int bno055_acc_range_vals[] = { + /* G: 2, 4, 8, 16 */ + 1962, 3924, 7848, 15696 +}; + +static struct bno055_sysfs_attr bno055_acc_range = { + .vals = bno055_acc_range_vals, + .len = ARRAY_SIZE(bno055_acc_range_vals), + .fusion_vals = (int[]){3924}, /* 4G */ + .type = IIO_VAL_INT, +}; + +/* + * Theoretically the IMU should return data in a given (i.e. fixed) unit + * regardless of the range setting. This happens for the accelerometer, but not + * for the gyroscope; the gyroscope range setting affects the scale. + * This is probably due to this[0] bug. + * For this reason we map the internal range setting onto the standard IIO scale + * attribute for gyro. + * Since the bug[0] may be fixed in future, we check for the IMU FW version and + * eventually warn the user. + * Currently we just don't care about "range" attributes for gyro. + * + * [0] https://community.bosch-sensortec.com/t5/MEMS-sensors-forum/BNO055-Wrong-sensitivity-resolution-in-datasheet/td-p/10266 + */ + +/* + * dps = hwval * (dps_range/2^15) + * rps = hwval * (rps_range/2^15) + * = hwval * (dps_range/(2^15 * k)) + * where k is rad-to-deg factor + */ +static int bno055_gyr_scale_vals[] = { + 125, 1877467, 250, 1877467, 500, 1877467, + 1000, 1877467, 2000, 1877467, +}; + +static struct bno055_sysfs_attr bno055_gyr_scale = { + .vals = bno055_gyr_scale_vals, + .len = ARRAY_SIZE(bno055_gyr_scale_vals), + .fusion_vals = (int[]){1, 900}, + .hw_xlate = (int[]){4, 3, 2, 1, 0}, + .type = IIO_VAL_FRACTIONAL, +}; + +static int bno055_gyr_lpf_vals[] = {12, 23, 32, 47, 64, 116, 230, 523}; +static struct bno055_sysfs_attr bno055_gyr_lpf = { + .vals = bno055_gyr_lpf_vals, + .len = ARRAY_SIZE(bno055_gyr_lpf_vals), + .fusion_vals = (int[]){32}, + .hw_xlate = (int[]){5, 4, 7, 3, 6, 2, 1, 0}, + .type = IIO_VAL_INT, +}; + +static int bno055_mag_odr_vals[] = {2, 6, 8, 10, 15, 20, 25, 30}; +static struct bno055_sysfs_attr bno055_mag_odr = { + .vals = bno055_mag_odr_vals, + .len = ARRAY_SIZE(bno055_mag_odr_vals), + .fusion_vals = (int[]){20}, + .type = IIO_VAL_INT, +}; + +struct bno055_priv { + struct regmap *regmap; + struct device *dev; + struct clk *clk; + int operation_mode; + int xfer_burst_break_thr; + struct mutex lock; + u8 uid[BNO055_UID_LEN]; + struct gpio_desc *reset_gpio; + bool sw_reset; + struct { + __le16 chans[BNO055_SCAN_CH_COUNT]; + s64 timestamp __aligned(8); + } buf; + struct dentry *debugfs; +}; + +static bool bno055_regmap_volatile(struct device *dev, unsigned int reg) +{ + /* data and status registers */ + if (reg >= BNO055_ACC_DATA_X_LSB_REG && reg <= BNO055_SYS_ERR_REG) + return true; + + /* when in fusion mode, config is updated by chip */ + if (reg == BNO055_MAG_CONFIG_REG || + reg == BNO055_ACC_CONFIG_REG || + reg == BNO055_GYR_CONFIG_REG) + return true; + + /* calibration data may be updated by the IMU */ + if (reg >= BNO055_CALDATA_START && reg <= BNO055_CALDATA_END) + return true; + + return false; +} + +static bool bno055_regmap_readable(struct device *dev, unsigned int reg) +{ + /* unnamed PG0 reserved areas */ + if ((reg < BNO055_PG1(0) && reg > BNO055_CALDATA_END) || + reg == 0x3C) + return false; + + /* unnamed PG1 reserved areas */ + if (reg > BNO055_PG1(BNO055_UID_HIGHER_REG) || + (reg < BNO055_PG1(BNO055_UID_LOWER_REG) && reg > BNO055_PG1(BNO055_GYR_AM_SET_REG)) || + reg == BNO055_PG1(0xE) || + (reg < BNO055_PG1(BNO055_PAGESEL_REG) && reg >= BNO055_PG1(0x0))) + return false; + return true; +} + +static bool bno055_regmap_writeable(struct device *dev, unsigned int reg) +{ + /* + * Unreadable registers are indeed reserved; there are no WO regs + * (except for a single bit in SYS_TRIGGER register) + */ + if (!bno055_regmap_readable(dev, reg)) + return false; + + /* data and status registers */ + if (reg >= BNO055_ACC_DATA_X_LSB_REG && reg <= BNO055_SYS_ERR_REG) + return false; + + /* ID areas */ + if (reg < BNO055_PAGESEL_REG || + (reg <= BNO055_UID_HIGHER_REG && reg >= BNO055_UID_LOWER_REG)) + return false; + + return true; +} + +static const struct regmap_range_cfg bno055_regmap_ranges[] = { + { + .range_min = 0, + .range_max = 0x7f * 2, + .selector_reg = BNO055_PAGESEL_REG, + .selector_mask = GENMASK(7, 0), + .selector_shift = 0, + .window_start = 0, + .window_len = 0x80, + }, +}; + +const struct regmap_config bno055_regmap_config = { + .name = "bno055", + .reg_bits = 8, + .val_bits = 8, + .ranges = bno055_regmap_ranges, + .num_ranges = 1, + .volatile_reg = bno055_regmap_volatile, + .max_register = 0x80 * 2, + .writeable_reg = bno055_regmap_writeable, + .readable_reg = bno055_regmap_readable, + .cache_type = REGCACHE_RBTREE, +}; +EXPORT_SYMBOL_NS_GPL(bno055_regmap_config, IIO_BNO055); + +/* must be called in configuration mode */ +static int bno055_calibration_load(struct bno055_priv *priv, const u8 *data, int len) +{ + if (len != BNO055_CALDATA_LEN) { + dev_dbg(priv->dev, "Invalid calibration file size %d (expected %d)", + len, BNO055_CALDATA_LEN); + return -EINVAL; + } + + dev_dbg(priv->dev, "loading cal data: %*ph", BNO055_CALDATA_LEN, data); + return regmap_bulk_write(priv->regmap, BNO055_CALDATA_START, + data, BNO055_CALDATA_LEN); +} + +static int bno055_operation_mode_do_set(struct bno055_priv *priv, + int operation_mode) +{ + int ret; + + ret = regmap_write(priv->regmap, BNO055_OPR_MODE_REG, + operation_mode); + if (ret) + return ret; + + /* Following datasheet specifications: sensor takes 7mS up to 19 mS to switch mode */ + msleep(20); + + return 0; +} + +static int bno055_system_reset(struct bno055_priv *priv) +{ + int ret; + + if (priv->reset_gpio) { + gpiod_set_value_cansleep(priv->reset_gpio, 0); + usleep_range(5000, 10000); + gpiod_set_value_cansleep(priv->reset_gpio, 1); + } else if (priv->sw_reset) { + ret = regmap_write(priv->regmap, BNO055_SYS_TRIGGER_REG, + BNO055_SYS_TRIGGER_RST_SYS); + if (ret) + return ret; + } else { + return 0; + } + + regcache_drop_region(priv->regmap, 0x0, 0xff); + usleep_range(650000, 700000); + + return 0; +} + +static int bno055_init(struct bno055_priv *priv, const u8 *caldata, int len) +{ + int ret; + + ret = bno055_operation_mode_do_set(priv, BNO055_OPR_MODE_CONFIG); + if (ret) + return ret; + + ret = regmap_write(priv->regmap, BNO055_POWER_MODE_REG, + BNO055_POWER_MODE_NORMAL); + if (ret) + return ret; + + ret = regmap_write(priv->regmap, BNO055_SYS_TRIGGER_REG, + priv->clk ? BNO055_SYS_TRIGGER_CLK_SEL : 0); + if (ret) + return ret; + + /* use standard SI units */ + ret = regmap_write(priv->regmap, BNO055_UNIT_SEL_REG, + BNO055_UNIT_SEL_ANDROID | BNO055_UNIT_SEL_GYR_RPS); + if (ret) + return ret; + + if (caldata) { + ret = bno055_calibration_load(priv, caldata, len); + if (ret) + dev_warn(priv->dev, "failed to load calibration data with error %d\n", + ret); + } + + return 0; +} + +static ssize_t bno055_operation_mode_set(struct bno055_priv *priv, + int operation_mode) +{ + u8 caldata[BNO055_CALDATA_LEN]; + int ret; + + mutex_lock(&priv->lock); + + ret = bno055_operation_mode_do_set(priv, BNO055_OPR_MODE_CONFIG); + if (ret) + goto exit_unlock; + + if (operation_mode == BNO055_OPR_MODE_FUSION || + operation_mode == BNO055_OPR_MODE_FUSION_FMC_OFF) { + /* for entering fusion mode, reset the chip to clear the algo state */ + ret = regmap_bulk_read(priv->regmap, BNO055_CALDATA_START, caldata, + BNO055_CALDATA_LEN); + if (ret) + goto exit_unlock; + + ret = bno055_system_reset(priv); + if (ret) + goto exit_unlock; + + ret = bno055_init(priv, caldata, BNO055_CALDATA_LEN); + if (ret) + goto exit_unlock; + } + + ret = bno055_operation_mode_do_set(priv, operation_mode); + if (ret) + goto exit_unlock; + + priv->operation_mode = operation_mode; + +exit_unlock: + mutex_unlock(&priv->lock); + return ret; +} + +static void bno055_uninit(void *arg) +{ + struct bno055_priv *priv = arg; + + /* stop the IMU */ + bno055_operation_mode_do_set(priv, BNO055_OPR_MODE_CONFIG); +} + +#define BNO055_CHANNEL(_type, _axis, _index, _address, _sep, _sh, _avail) { \ + .address = _address, \ + .type = _type, \ + .modified = 1, \ + .channel2 = IIO_MOD_##_axis, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | (_sep), \ + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) | (_sh), \ + .info_mask_shared_by_type_available = _avail, \ + .scan_index = _index, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_LE, \ + .repeat = IIO_MOD_##_axis == IIO_MOD_QUATERNION ? 4 : 0, \ + }, \ +} + +/* scan indexes follow DATA register order */ +enum bno055_scan_axis { + BNO055_SCAN_ACCEL_X, + BNO055_SCAN_ACCEL_Y, + BNO055_SCAN_ACCEL_Z, + BNO055_SCAN_MAGN_X, + BNO055_SCAN_MAGN_Y, + BNO055_SCAN_MAGN_Z, + BNO055_SCAN_GYRO_X, + BNO055_SCAN_GYRO_Y, + BNO055_SCAN_GYRO_Z, + BNO055_SCAN_YAW, + BNO055_SCAN_ROLL, + BNO055_SCAN_PITCH, + BNO055_SCAN_QUATERNION, + BNO055_SCAN_LIA_X, + BNO055_SCAN_LIA_Y, + BNO055_SCAN_LIA_Z, + BNO055_SCAN_GRAVITY_X, + BNO055_SCAN_GRAVITY_Y, + BNO055_SCAN_GRAVITY_Z, + BNO055_SCAN_TIMESTAMP, + _BNO055_SCAN_MAX +}; + +static const struct iio_chan_spec bno055_channels[] = { + /* accelerometer */ + BNO055_CHANNEL(IIO_ACCEL, X, BNO055_SCAN_ACCEL_X, + BNO055_ACC_DATA_X_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET), + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY)), + BNO055_CHANNEL(IIO_ACCEL, Y, BNO055_SCAN_ACCEL_Y, + BNO055_ACC_DATA_Y_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET), + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY)), + BNO055_CHANNEL(IIO_ACCEL, Z, BNO055_SCAN_ACCEL_Z, + BNO055_ACC_DATA_Z_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET), + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY)), + /* gyroscope */ + BNO055_CHANNEL(IIO_ANGL_VEL, X, BNO055_SCAN_GYRO_X, + BNO055_GYR_DATA_X_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET), + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY) | + BIT(IIO_CHAN_INFO_SCALE)), + BNO055_CHANNEL(IIO_ANGL_VEL, Y, BNO055_SCAN_GYRO_Y, + BNO055_GYR_DATA_Y_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET), + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY) | + BIT(IIO_CHAN_INFO_SCALE)), + BNO055_CHANNEL(IIO_ANGL_VEL, Z, BNO055_SCAN_GYRO_Z, + BNO055_GYR_DATA_Z_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET), + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY) | + BIT(IIO_CHAN_INFO_SCALE)), + /* magnetometer */ + BNO055_CHANNEL(IIO_MAGN, X, BNO055_SCAN_MAGN_X, + BNO055_MAG_DATA_X_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET), + BIT(IIO_CHAN_INFO_SAMP_FREQ), BIT(IIO_CHAN_INFO_SAMP_FREQ)), + BNO055_CHANNEL(IIO_MAGN, Y, BNO055_SCAN_MAGN_Y, + BNO055_MAG_DATA_Y_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET), + BIT(IIO_CHAN_INFO_SAMP_FREQ), BIT(IIO_CHAN_INFO_SAMP_FREQ)), + BNO055_CHANNEL(IIO_MAGN, Z, BNO055_SCAN_MAGN_Z, + BNO055_MAG_DATA_Z_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET), + BIT(IIO_CHAN_INFO_SAMP_FREQ), BIT(IIO_CHAN_INFO_SAMP_FREQ)), + /* euler angle */ + BNO055_CHANNEL(IIO_ROT, YAW, BNO055_SCAN_YAW, + BNO055_EUL_DATA_X_LSB_REG, 0, 0, 0), + BNO055_CHANNEL(IIO_ROT, ROLL, BNO055_SCAN_ROLL, + BNO055_EUL_DATA_Y_LSB_REG, 0, 0, 0), + BNO055_CHANNEL(IIO_ROT, PITCH, BNO055_SCAN_PITCH, + BNO055_EUL_DATA_Z_LSB_REG, 0, 0, 0), + /* quaternion */ + BNO055_CHANNEL(IIO_ROT, QUATERNION, BNO055_SCAN_QUATERNION, + BNO055_QUAT_DATA_W_LSB_REG, 0, 0, 0), + + /* linear acceleration */ + BNO055_CHANNEL(IIO_ACCEL, LINEAR_X, BNO055_SCAN_LIA_X, + BNO055_LIA_DATA_X_LSB_REG, 0, 0, 0), + BNO055_CHANNEL(IIO_ACCEL, LINEAR_Y, BNO055_SCAN_LIA_Y, + BNO055_LIA_DATA_Y_LSB_REG, 0, 0, 0), + BNO055_CHANNEL(IIO_ACCEL, LINEAR_Z, BNO055_SCAN_LIA_Z, + BNO055_LIA_DATA_Z_LSB_REG, 0, 0, 0), + + /* gravity vector */ + BNO055_CHANNEL(IIO_GRAVITY, X, BNO055_SCAN_GRAVITY_X, + BNO055_GRAVITY_DATA_X_LSB_REG, 0, 0, 0), + BNO055_CHANNEL(IIO_GRAVITY, Y, BNO055_SCAN_GRAVITY_Y, + BNO055_GRAVITY_DATA_Y_LSB_REG, 0, 0, 0), + BNO055_CHANNEL(IIO_GRAVITY, Z, BNO055_SCAN_GRAVITY_Z, + BNO055_GRAVITY_DATA_Z_LSB_REG, 0, 0, 0), + + { + .type = IIO_TEMP, + .info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED), + .scan_index = -1, + }, + IIO_CHAN_SOFT_TIMESTAMP(BNO055_SCAN_TIMESTAMP), +}; + +static int bno055_get_regmask(struct bno055_priv *priv, int *val, int *val2, + int reg, int mask, struct bno055_sysfs_attr *attr) +{ + const int shift = __ffs(mask); + int hwval, idx; + int ret; + int i; + + ret = regmap_read(priv->regmap, reg, &hwval); + if (ret) + return ret; + + idx = (hwval & mask) >> shift; + if (attr->hw_xlate) + for (i = 0; i < attr->len; i++) + if (attr->hw_xlate[i] == idx) { + idx = i; + break; + } + if (attr->type == IIO_VAL_INT) { + *val = attr->vals[idx]; + } else { /* IIO_VAL_INT_PLUS_MICRO or IIO_VAL_FRACTIONAL */ + *val = attr->vals[idx * 2]; + *val2 = attr->vals[idx * 2 + 1]; + } + + return attr->type; +} + +static int bno055_set_regmask(struct bno055_priv *priv, int val, int val2, + int reg, int mask, struct bno055_sysfs_attr *attr) +{ + const int shift = __ffs(mask); + int best_delta; + int req_val; + int tbl_val; + bool first; + int delta; + int hwval; + int ret; + int len; + int i; + + /* + * The closest value the HW supports is only one in fusion mode, + * and it is autoselected, so don't do anything, just return OK, + * as the closest possible value has been (virtually) selected + */ + if (priv->operation_mode != BNO055_OPR_MODE_AMG) + return 0; + + len = attr->len; + + /* + * We always get a request in INT_PLUS_MICRO, but we + * take care of the micro part only when we really have + * non-integer tables. This prevents 32-bit overflow with + * larger integers contained in integer tables. + */ + req_val = val; + if (attr->type != IIO_VAL_INT) { + len /= 2; + req_val = min(val, 2147) * 1000000 + val2; + } + + first = true; + for (i = 0; i < len; i++) { + switch (attr->type) { + case IIO_VAL_INT: + tbl_val = attr->vals[i]; + break; + case IIO_VAL_INT_PLUS_MICRO: + WARN_ON(attr->vals[i * 2] > 2147); + tbl_val = attr->vals[i * 2] * 1000000 + + attr->vals[i * 2 + 1]; + break; + case IIO_VAL_FRACTIONAL: + WARN_ON(attr->vals[i * 2] > 4294); + tbl_val = attr->vals[i * 2] * 1000000 / + attr->vals[i * 2 + 1]; + break; + default: + return -EINVAL; + } + delta = abs(tbl_val - req_val); + if (first || delta < best_delta) { + best_delta = delta; + hwval = i; + first = false; + } + } + + if (attr->hw_xlate) + hwval = attr->hw_xlate[hwval]; + + ret = bno055_operation_mode_do_set(priv, BNO055_OPR_MODE_CONFIG); + if (ret) + return ret; + + ret = regmap_update_bits(priv->regmap, reg, mask, hwval << shift); + if (ret) + return ret; + + return bno055_operation_mode_do_set(priv, BNO055_OPR_MODE_AMG); +} + +static int bno055_read_simple_chan(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask) +{ + struct bno055_priv *priv = iio_priv(indio_dev); + __le16 raw_val; + int ret; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + ret = regmap_bulk_read(priv->regmap, chan->address, + &raw_val, sizeof(raw_val)); + if (ret < 0) + return ret; + *val = sign_extend32(le16_to_cpu(raw_val), 15); + return IIO_VAL_INT; + case IIO_CHAN_INFO_OFFSET: + if (priv->operation_mode != BNO055_OPR_MODE_AMG) { + *val = 0; + } else { + ret = regmap_bulk_read(priv->regmap, + chan->address + + BNO055_REG_OFFSET_ADDR, + &raw_val, sizeof(raw_val)); + if (ret < 0) + return ret; + /* + * IMU reports sensor offsets; IIO wants correction + * offsets, thus we need the 'minus' here. + */ + *val = -sign_extend32(le16_to_cpu(raw_val), 15); + } + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + *val = 1; + switch (chan->type) { + case IIO_GRAVITY: + /* Table 3-35: 1 m/s^2 = 100 LSB */ + case IIO_ACCEL: + /* Table 3-17: 1 m/s^2 = 100 LSB */ + *val2 = 100; + break; + case IIO_MAGN: + /* + * Table 3-19: 1 uT = 16 LSB. But we need + * Gauss: 1G = 0.1 uT. + */ + *val2 = 160; + break; + case IIO_ANGL_VEL: + /* + * Table 3-22: 1 Rps = 900 LSB + * .. but this is not exactly true. See comment at the + * beginning of this file. + */ + if (priv->operation_mode != BNO055_OPR_MODE_AMG) { + *val = bno055_gyr_scale.fusion_vals[0]; + *val2 = bno055_gyr_scale.fusion_vals[1]; + return IIO_VAL_FRACTIONAL; + } + + return bno055_get_regmask(priv, val, val2, + BNO055_GYR_CONFIG_REG, + BNO055_GYR_CONFIG_RANGE_MASK, + &bno055_gyr_scale); + break; + case IIO_ROT: + /* Table 3-28: 1 degree = 16 LSB */ + *val2 = 16; + break; + default: + return -EINVAL; + } + return IIO_VAL_FRACTIONAL; + + case IIO_CHAN_INFO_SAMP_FREQ: + if (chan->type != IIO_MAGN) + return -EINVAL; + + return bno055_get_regmask(priv, val, val2, + BNO055_MAG_CONFIG_REG, + BNO055_MAG_CONFIG_ODR_MASK, + &bno055_mag_odr); + + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: + switch (chan->type) { + case IIO_ANGL_VEL: + return bno055_get_regmask(priv, val, val2, + BNO055_GYR_CONFIG_REG, + BNO055_GYR_CONFIG_LPF_MASK, + &bno055_gyr_lpf); + case IIO_ACCEL: + return bno055_get_regmask(priv, val, val2, + BNO055_ACC_CONFIG_REG, + BNO055_ACC_CONFIG_LPF_MASK, + &bno055_acc_lpf); + default: + return -EINVAL; + } + + default: + return -EINVAL; + } +} + +static int bno055_sysfs_attr_avail(struct bno055_priv *priv, struct bno055_sysfs_attr *attr, + const int **vals, int *length) +{ + if (priv->operation_mode != BNO055_OPR_MODE_AMG) { + /* locked when fusion enabled */ + *vals = attr->fusion_vals; + if (attr->type == IIO_VAL_INT) + *length = 1; + else + *length = 2; /* IIO_VAL_INT_PLUS_MICRO or IIO_VAL_FRACTIONAL*/ + } else { + *vals = attr->vals; + *length = attr->len; + } + + return attr->type; +} + +static int bno055_read_avail(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + const int **vals, int *type, int *length, + long mask) +{ + struct bno055_priv *priv = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + switch (chan->type) { + case IIO_ANGL_VEL: + *type = bno055_sysfs_attr_avail(priv, &bno055_gyr_scale, + vals, length); + return IIO_AVAIL_LIST; + default: + return -EINVAL; + } + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: + switch (chan->type) { + case IIO_ANGL_VEL: + *type = bno055_sysfs_attr_avail(priv, &bno055_gyr_lpf, + vals, length); + return IIO_AVAIL_LIST; + case IIO_ACCEL: + *type = bno055_sysfs_attr_avail(priv, &bno055_acc_lpf, + vals, length); + return IIO_AVAIL_LIST; + default: + return -EINVAL; + } + + break; + case IIO_CHAN_INFO_SAMP_FREQ: + switch (chan->type) { + case IIO_MAGN: + *type = bno055_sysfs_attr_avail(priv, &bno055_mag_odr, + vals, length); + return IIO_AVAIL_LIST; + default: + return -EINVAL; + } + default: + return -EINVAL; + } +} + +static int bno055_read_temp_chan(struct iio_dev *indio_dev, int *val) +{ + struct bno055_priv *priv = iio_priv(indio_dev); + unsigned int raw_val; + int ret; + + ret = regmap_read(priv->regmap, BNO055_TEMP_REG, &raw_val); + if (ret < 0) + return ret; + + /* + * Tables 3-36 and 3-37: one byte of priv, signed, 1 LSB = 1C. + * ABI wants milliC. + */ + *val = raw_val * 1000; + + return IIO_VAL_INT; +} + +static int bno055_read_quaternion(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int size, int *vals, int *val_len, + long mask) +{ + struct bno055_priv *priv = iio_priv(indio_dev); + __le16 raw_vals[4]; + int i, ret; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + if (size < 4) + return -EINVAL; + ret = regmap_bulk_read(priv->regmap, + BNO055_QUAT_DATA_W_LSB_REG, + raw_vals, sizeof(raw_vals)); + if (ret < 0) + return ret; + for (i = 0; i < 4; i++) + vals[i] = sign_extend32(le16_to_cpu(raw_vals[i]), 15); + *val_len = 4; + return IIO_VAL_INT_MULTIPLE; + case IIO_CHAN_INFO_SCALE: + /* Table 3-31: 1 quaternion = 2^14 LSB */ + if (size < 2) + return -EINVAL; + vals[0] = 1; + vals[1] = 14; + return IIO_VAL_FRACTIONAL_LOG2; + default: + return -EINVAL; + } +} + +static bool bno055_is_chan_readable(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan) +{ + struct bno055_priv *priv = iio_priv(indio_dev); + + if (priv->operation_mode != BNO055_OPR_MODE_AMG) + return true; + + switch (chan->type) { + case IIO_GRAVITY: + case IIO_ROT: + return false; + case IIO_ACCEL: + if (chan->channel2 == IIO_MOD_LINEAR_X || + chan->channel2 == IIO_MOD_LINEAR_Y || + chan->channel2 == IIO_MOD_LINEAR_Z) + return false; + return true; + default: + return true; + } +} + +static int _bno055_read_raw_multi(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int size, int *vals, int *val_len, + long mask) +{ + if (!bno055_is_chan_readable(indio_dev, chan)) + return -EBUSY; + + switch (chan->type) { + case IIO_MAGN: + case IIO_ACCEL: + case IIO_ANGL_VEL: + case IIO_GRAVITY: + if (size < 2) + return -EINVAL; + *val_len = 2; + return bno055_read_simple_chan(indio_dev, chan, + &vals[0], &vals[1], + mask); + case IIO_TEMP: + *val_len = 1; + return bno055_read_temp_chan(indio_dev, &vals[0]); + case IIO_ROT: + /* + * Rotation is exposed as either a quaternion or three + * Euler angles. + */ + if (chan->channel2 == IIO_MOD_QUATERNION) + return bno055_read_quaternion(indio_dev, chan, + size, vals, + val_len, mask); + if (size < 2) + return -EINVAL; + *val_len = 2; + return bno055_read_simple_chan(indio_dev, chan, + &vals[0], &vals[1], + mask); + default: + return -EINVAL; + } +} + +static int bno055_read_raw_multi(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int size, int *vals, int *val_len, + long mask) +{ + struct bno055_priv *priv = iio_priv(indio_dev); + int ret; + + mutex_lock(&priv->lock); + ret = _bno055_read_raw_multi(indio_dev, chan, size, + vals, val_len, mask); + mutex_unlock(&priv->lock); + return ret; +} + +static int _bno055_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct bno055_priv *priv = iio_priv(iio_dev); + + switch (chan->type) { + case IIO_MAGN: + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: + return bno055_set_regmask(priv, val, val2, + BNO055_MAG_CONFIG_REG, + BNO055_MAG_CONFIG_ODR_MASK, + &bno055_mag_odr); + default: + return -EINVAL; + } + case IIO_ACCEL: + switch (mask) { + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: + return bno055_set_regmask(priv, val, val2, + BNO055_ACC_CONFIG_REG, + BNO055_ACC_CONFIG_LPF_MASK, + &bno055_acc_lpf); + + default: + return -EINVAL; + } + case IIO_ANGL_VEL: + switch (mask) { + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: + return bno055_set_regmask(priv, val, val2, + BNO055_GYR_CONFIG_REG, + BNO055_GYR_CONFIG_LPF_MASK, + &bno055_gyr_lpf); + case IIO_CHAN_INFO_SCALE: + return bno055_set_regmask(priv, val, val2, + BNO055_GYR_CONFIG_REG, + BNO055_GYR_CONFIG_RANGE_MASK, + &bno055_gyr_scale); + default: + return -EINVAL; + } + default: + return -EINVAL; + } +} + +static int bno055_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct bno055_priv *priv = iio_priv(iio_dev); + int ret; + + mutex_lock(&priv->lock); + ret = _bno055_write_raw(iio_dev, chan, val, val2, mask); + mutex_unlock(&priv->lock); + + return ret; +} + +static ssize_t in_accel_range_raw_available_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); + int len = 0; + int i; + + if (priv->operation_mode != BNO055_OPR_MODE_AMG) + return sysfs_emit(buf, "%d\n", bno055_acc_range.fusion_vals[0]); + + for (i = 0; i < bno055_acc_range.len; i++) + len += sysfs_emit_at(buf, len, "%d ", bno055_acc_range.vals[i]); + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t fusion_enable_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); + + return sysfs_emit(buf, "%d\n", + priv->operation_mode != BNO055_OPR_MODE_AMG); +} + +static ssize_t fusion_enable_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t len) +{ + struct iio_dev *indio_dev = dev_to_iio_dev(dev); + struct bno055_priv *priv = iio_priv(indio_dev); + bool en; + int ret; + + if (indio_dev->active_scan_mask && + !bitmap_empty(indio_dev->active_scan_mask, _BNO055_SCAN_MAX)) + return -EBUSY; + + ret = kstrtobool(buf, &en); + if (ret) + return -EINVAL; + + if (!en) + return bno055_operation_mode_set(priv, BNO055_OPR_MODE_AMG) ?: len; + + /* + * Coming from AMG means the FMC was off, just switch to fusion but + * don't change anything that doesn't belong to us (i.e let FMC stay off). + * Coming from any other fusion mode means we don't need to do anything. + */ + if (priv->operation_mode == BNO055_OPR_MODE_AMG) + return bno055_operation_mode_set(priv, BNO055_OPR_MODE_FUSION_FMC_OFF) ?: len; + + return len; +} + +static ssize_t in_magn_calibration_fast_enable_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); + + return sysfs_emit(buf, "%d\n", + priv->operation_mode == BNO055_OPR_MODE_FUSION); +} + +static ssize_t in_magn_calibration_fast_enable_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t len) +{ + struct iio_dev *indio_dev = dev_to_iio_dev(dev); + struct bno055_priv *priv = iio_priv(indio_dev); + int ret; + + if (indio_dev->active_scan_mask && + !bitmap_empty(indio_dev->active_scan_mask, _BNO055_SCAN_MAX)) + return -EBUSY; + + if (sysfs_streq(buf, "0")) { + if (priv->operation_mode == BNO055_OPR_MODE_FUSION) { + ret = bno055_operation_mode_set(priv, BNO055_OPR_MODE_FUSION_FMC_OFF); + if (ret) + return ret; + } + } else { + if (priv->operation_mode == BNO055_OPR_MODE_AMG) + return -EINVAL; + + if (priv->operation_mode != BNO055_OPR_MODE_FUSION) { + ret = bno055_operation_mode_set(priv, BNO055_OPR_MODE_FUSION); + if (ret) + return ret; + } + } + + return len; +} + +static ssize_t in_accel_range_raw_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); + int val; + int ret; + + ret = bno055_get_regmask(priv, &val, NULL, + BNO055_ACC_CONFIG_REG, + BNO055_ACC_CONFIG_RANGE_MASK, + &bno055_acc_range); + if (ret < 0) + return ret; + + return sysfs_emit(buf, "%d\n", val); +} + +static ssize_t in_accel_range_raw_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t len) +{ + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); + unsigned long val; + int ret; + + ret = kstrtoul(buf, 10, &val); + if (ret) + return ret; + + mutex_lock(&priv->lock); + ret = bno055_set_regmask(priv, val, 0, + BNO055_ACC_CONFIG_REG, + BNO055_ACC_CONFIG_RANGE_MASK, + &bno055_acc_range); + mutex_unlock(&priv->lock); + + return ret ?: len; +} + +static ssize_t bno055_get_calib_status(struct device *dev, char *buf, int which) +{ + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); + int calib; + int ret; + int val; + + if (priv->operation_mode == BNO055_OPR_MODE_AMG || + (priv->operation_mode == BNO055_OPR_MODE_FUSION_FMC_OFF && + which == BNO055_CALIB_STAT_MAGN_SHIFT)) { + calib = 0; + } else { + mutex_lock(&priv->lock); + ret = regmap_read(priv->regmap, BNO055_CALIB_STAT_REG, &val); + mutex_unlock(&priv->lock); + + if (ret) + return -EIO; + + calib = ((val >> which) & GENMASK(1, 0)) + 1; + } + + return sysfs_emit(buf, "%d\n", calib); +} + +static ssize_t serialnumber_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); + + return sysfs_emit(buf, "%*ph\n", BNO055_UID_LEN, priv->uid); +} + +static ssize_t calibration_data_read(struct file *filp, struct kobject *kobj, + struct bin_attribute *bin_attr, char *buf, + loff_t pos, size_t count) +{ + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(kobj_to_dev(kobj))); + u8 data[BNO055_CALDATA_LEN]; + int ret; + + /* + * Calibration data is volatile; reading it in chunks will possibly + * results in inconsistent data. We require the user to read the whole + * blob in a single chunk + */ + if (count < BNO055_CALDATA_LEN || pos) + return -EINVAL; + + mutex_lock(&priv->lock); + ret = bno055_operation_mode_do_set(priv, BNO055_OPR_MODE_CONFIG); + if (ret) + goto exit_unlock; + + ret = regmap_bulk_read(priv->regmap, BNO055_CALDATA_START, data, + BNO055_CALDATA_LEN); + if (ret) + goto exit_unlock; + + ret = bno055_operation_mode_do_set(priv, priv->operation_mode); + if (ret) + goto exit_unlock; + + memcpy(buf, data, BNO055_CALDATA_LEN); + + ret = BNO055_CALDATA_LEN; +exit_unlock: + mutex_unlock(&priv->lock); + return ret; +} + +static ssize_t sys_calibration_auto_status_show(struct device *dev, + struct device_attribute *a, + char *buf) +{ + return bno055_get_calib_status(dev, buf, BNO055_CALIB_STAT_SYS_SHIFT); +} + +static ssize_t in_accel_calibration_auto_status_show(struct device *dev, + struct device_attribute *a, + char *buf) +{ + return bno055_get_calib_status(dev, buf, BNO055_CALIB_STAT_ACCEL_SHIFT); +} + +static ssize_t in_gyro_calibration_auto_status_show(struct device *dev, + struct device_attribute *a, + char *buf) +{ + return bno055_get_calib_status(dev, buf, BNO055_CALIB_STAT_GYRO_SHIFT); +} + +static ssize_t in_magn_calibration_auto_status_show(struct device *dev, + struct device_attribute *a, + char *buf) +{ + return bno055_get_calib_status(dev, buf, BNO055_CALIB_STAT_MAGN_SHIFT); +} + +static int bno055_debugfs_reg_access(struct iio_dev *iio_dev, unsigned int reg, + unsigned int writeval, unsigned int *readval) +{ + struct bno055_priv *priv = iio_priv(iio_dev); + + if (readval) + return regmap_read(priv->regmap, reg, readval); + else + return regmap_write(priv->regmap, reg, writeval); +} + +static ssize_t bno055_show_fw_version(struct file *file, char __user *userbuf, + size_t count, loff_t *ppos) +{ + struct bno055_priv *priv = file->private_data; + int rev, ver; + char *buf; + int ret; + + ret = regmap_read(priv->regmap, BNO055_SW_REV_LSB_REG, &rev); + if (ret) + return ret; + + ret = regmap_read(priv->regmap, BNO055_SW_REV_MSB_REG, &ver); + if (ret) + return ret; + + buf = kasprintf(GFP_KERNEL, "ver: 0x%x, rev: 0x%x\n", ver, rev); + if (!buf) + return -ENOMEM; + + ret = simple_read_from_buffer(userbuf, count, ppos, buf, strlen(buf)); + kfree(buf); + + return ret; +} + +static const struct file_operations bno055_fw_version_ops = { + .open = simple_open, + .read = bno055_show_fw_version, + .llseek = default_llseek, + .owner = THIS_MODULE, +}; + +static void bno055_debugfs_remove(void *_priv) +{ + struct bno055_priv *priv = _priv; + + debugfs_remove(priv->debugfs); + priv->debugfs = NULL; +} + +static void bno055_debugfs_init(struct iio_dev *iio_dev) +{ + struct bno055_priv *priv = iio_priv(iio_dev); + + priv->debugfs = debugfs_create_file("firmware_version", 0400, + iio_get_debugfs_dentry(iio_dev), + priv, &bno055_fw_version_ops); + if (!IS_ERR(priv->debugfs)) + devm_add_action_or_reset(priv->dev, bno055_debugfs_remove, + priv); + if (IS_ERR_OR_NULL(priv->debugfs)) + dev_warn(priv->dev, "failed to setup debugfs"); +} + +static IIO_DEVICE_ATTR_RW(fusion_enable, 0); +static IIO_DEVICE_ATTR_RW(in_magn_calibration_fast_enable, 0); +static IIO_DEVICE_ATTR_RW(in_accel_range_raw, 0); + +static IIO_DEVICE_ATTR_RO(in_accel_range_raw_available, 0); +static IIO_DEVICE_ATTR_RO(sys_calibration_auto_status, 0); +static IIO_DEVICE_ATTR_RO(in_accel_calibration_auto_status, 0); +static IIO_DEVICE_ATTR_RO(in_gyro_calibration_auto_status, 0); +static IIO_DEVICE_ATTR_RO(in_magn_calibration_auto_status, 0); +static IIO_DEVICE_ATTR_RO(serialnumber, 0); + +static struct attribute *bno055_attrs[] = { + &iio_dev_attr_in_accel_range_raw_available.dev_attr.attr, + &iio_dev_attr_in_accel_range_raw.dev_attr.attr, + &iio_dev_attr_fusion_enable.dev_attr.attr, + &iio_dev_attr_in_magn_calibration_fast_enable.dev_attr.attr, + &iio_dev_attr_sys_calibration_auto_status.dev_attr.attr, + &iio_dev_attr_in_accel_calibration_auto_status.dev_attr.attr, + &iio_dev_attr_in_gyro_calibration_auto_status.dev_attr.attr, + &iio_dev_attr_in_magn_calibration_auto_status.dev_attr.attr, + &iio_dev_attr_serialnumber.dev_attr.attr, + NULL +}; + +static BIN_ATTR_RO(calibration_data, BNO055_CALDATA_LEN); + +static struct bin_attribute *bno055_bin_attrs[] = { + &bin_attr_calibration_data, + NULL +}; + +static const struct attribute_group bno055_attrs_group = { + .attrs = bno055_attrs, + .bin_attrs = bno055_bin_attrs, +}; + +static const struct iio_info bno055_info = { + .read_raw_multi = bno055_read_raw_multi, + .read_avail = bno055_read_avail, + .write_raw = bno055_write_raw, + .attrs = &bno055_attrs_group, + .debugfs_reg_access = bno055_debugfs_reg_access, +}; + +/* + * Reads len samples from the HW, stores them in buf starting from buf_idx, + * and applies mask to cull (skip) unneeded samples. + * Updates buf_idx incrementing with the number of stored samples. + * Samples from HW are transferred into buf, then in-place copy on buf is + * performed in order to cull samples that need to be skipped. + * This avoids copies of the first samples until we hit the 1st sample to skip, + * and also avoids having an extra bounce buffer. + * buf must be able to contain len elements in spite of how many samples we are + * going to cull. + */ +static int bno055_scan_xfer(struct bno055_priv *priv, + int start_ch, int len, unsigned long mask, + __le16 *buf, int *buf_idx) +{ + const int base = BNO055_ACC_DATA_X_LSB_REG; + bool quat_in_read = false; + int buf_base = *buf_idx; + __le16 *dst, *src; + int offs_fixup = 0; + int xfer_len = len; + int ret; + int i, n; + + if (!mask) + return 0; + + /* + * All channels are made up 1 16-bit sample, except for quaternion that + * is made up 4 16-bit values. + * For us the quaternion CH is just like 4 regular CHs. + * If our read starts past the quaternion make sure to adjust the + * starting offset; if the quaternion is contained in our scan then make + * sure to adjust the read len. + */ + if (start_ch > BNO055_SCAN_QUATERNION) { + start_ch += 3; + } else if ((start_ch <= BNO055_SCAN_QUATERNION) && + ((start_ch + len) > BNO055_SCAN_QUATERNION)) { + quat_in_read = true; + xfer_len += 3; + } + + ret = regmap_bulk_read(priv->regmap, + base + start_ch * sizeof(__le16), + buf + buf_base, + xfer_len * sizeof(__le16)); + if (ret) + return ret; + + for_each_set_bit(i, &mask, len) { + if (quat_in_read && ((start_ch + i) > BNO055_SCAN_QUATERNION)) + offs_fixup = 3; + + dst = buf + *buf_idx; + src = buf + buf_base + offs_fixup + i; + + n = (start_ch + i == BNO055_SCAN_QUATERNION) ? 4 : 1; + + if (dst != src) + memcpy(dst, src, n * sizeof(__le16)); + + *buf_idx += n; + } + return 0; +} + +static irqreturn_t bno055_trigger_handler(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *iio_dev = pf->indio_dev; + struct bno055_priv *priv = iio_priv(iio_dev); + int xfer_start, start, end, prev_end; + unsigned long mask; + int quat_extra_len; + bool first = true; + int buf_idx = 0; + bool thr_hit; + int ret; + + mutex_lock(&priv->lock); + + /* + * Walk the bitmap and eventually perform several transfers. + * Bitmap ones-fields that are separated by gaps <= xfer_burst_break_thr + * will be included in same transfer. + * Every time the bitmap contains a gap wider than xfer_burst_break_thr + * then we split the transfer, skipping the gap. + */ + for_each_set_bitrange(start, end, iio_dev->active_scan_mask, + iio_dev->masklength) { + /* + * First transfer will start from the beginning of the first + * ones-field in the bitmap + */ + if (first) { + xfer_start = start; + } else { + /* + * We found the next ones-field; check whether to + * include it in * the current transfer or not (i.e. + * let's perform the current * transfer and prepare for + * another one). + */ + + /* + * In case the zeros-gap contains the quaternion bit, + * then its length is actually 4 words instead of 1 + * (i.e. +3 wrt other channels). + */ + quat_extra_len = ((start > BNO055_SCAN_QUATERNION) && + (prev_end <= BNO055_SCAN_QUATERNION)) ? 3 : 0; + + /* If the gap is wider than xfer_burst_break_thr then.. */ + thr_hit = (start - prev_end + quat_extra_len) > + priv->xfer_burst_break_thr; + + /* + * .. transfer all the data up to the gap. Then set the + * next transfer start index at right after the gap + * (i.e. at the start of this ones-field). + */ + if (thr_hit) { + mask = *iio_dev->active_scan_mask >> xfer_start; + ret = bno055_scan_xfer(priv, xfer_start, + prev_end - xfer_start, + mask, priv->buf.chans, &buf_idx); + if (ret) + goto done; + xfer_start = start; + } + } + first = false; + prev_end = end; + } + + /* + * We finished walking the bitmap; no more gaps to check for. Just + * perform the current transfer. + */ + mask = *iio_dev->active_scan_mask >> xfer_start; + ret = bno055_scan_xfer(priv, xfer_start, + prev_end - xfer_start, + mask, priv->buf.chans, &buf_idx); + + if (!ret) + iio_push_to_buffers_with_timestamp(iio_dev, + &priv->buf, pf->timestamp); +done: + mutex_unlock(&priv->lock); + iio_trigger_notify_done(iio_dev->trig); + return IRQ_HANDLED; +} + +static int bno055_buffer_preenable(struct iio_dev *indio_dev) +{ + struct bno055_priv *priv = iio_priv(indio_dev); + const unsigned long fusion_mask = + BIT(BNO055_SCAN_YAW) | + BIT(BNO055_SCAN_ROLL) | + BIT(BNO055_SCAN_PITCH) | + BIT(BNO055_SCAN_QUATERNION) | + BIT(BNO055_SCAN_LIA_X) | + BIT(BNO055_SCAN_LIA_Y) | + BIT(BNO055_SCAN_LIA_Z) | + BIT(BNO055_SCAN_GRAVITY_X) | + BIT(BNO055_SCAN_GRAVITY_Y) | + BIT(BNO055_SCAN_GRAVITY_Z); + + if (priv->operation_mode == BNO055_OPR_MODE_AMG && + bitmap_intersects(indio_dev->active_scan_mask, &fusion_mask, + _BNO055_SCAN_MAX)) + return -EBUSY; + return 0; +} + +static const struct iio_buffer_setup_ops bno055_buffer_setup_ops = { + .preenable = bno055_buffer_preenable, +}; + +int bno055_probe(struct device *dev, struct regmap *regmap, + int xfer_burst_break_thr, bool sw_reset) +{ + const struct firmware *caldata = NULL; + struct bno055_priv *priv; + struct iio_dev *iio_dev; + char *fw_name_buf; + unsigned int val; + int rev, ver; + int ret; + + iio_dev = devm_iio_device_alloc(dev, sizeof(*priv)); + if (!iio_dev) + return -ENOMEM; + + iio_dev->name = "bno055"; + priv = iio_priv(iio_dev); + mutex_init(&priv->lock); + priv->regmap = regmap; + priv->dev = dev; + priv->xfer_burst_break_thr = xfer_burst_break_thr; + priv->sw_reset = sw_reset; + + priv->reset_gpio = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_LOW); + if (IS_ERR(priv->reset_gpio)) + return dev_err_probe(dev, PTR_ERR(priv->reset_gpio), "Failed to get reset GPIO\n"); + + priv->clk = devm_clk_get_optional_enabled(dev, "clk"); + if (IS_ERR(priv->clk)) + return dev_err_probe(dev, PTR_ERR(priv->clk), "Failed to get CLK\n"); + + if (priv->reset_gpio) { + usleep_range(5000, 10000); + gpiod_set_value_cansleep(priv->reset_gpio, 1); + usleep_range(650000, 750000); + } else if (!sw_reset) { + dev_warn(dev, "No usable reset method; IMU may be unreliable\n"); + } + + ret = regmap_read(priv->regmap, BNO055_CHIP_ID_REG, &val); + if (ret) + return ret; + + if (val != BNO055_CHIP_ID_MAGIC) + dev_warn(dev, "Unrecognized chip ID 0x%x\n", val); + + /* + * In case we haven't a HW reset pin, we can still reset the chip via + * register write. This is probably nonsense in case we can't even + * communicate with the chip or the chip isn't the one we expect (i.e. + * we don't write to unknown chips), so we perform SW reset only after + * chip magic ID check + */ + if (!priv->reset_gpio) { + ret = bno055_system_reset(priv); + if (ret) + return ret; + } + + ret = regmap_read(priv->regmap, BNO055_SW_REV_LSB_REG, &rev); + if (ret) + return ret; + + ret = regmap_read(priv->regmap, BNO055_SW_REV_MSB_REG, &ver); + if (ret) + return ret; + + /* + * The stock FW version contains a bug (see comment at the beginning of + * this file) that causes the anglvel scale to be changed depending on + * the chip range setting. We workaround this, but we don't know what + * other FW versions might do. + */ + if (ver != 0x3 || rev != 0x11) + dev_warn(dev, "Untested firmware version. Anglvel scale may not work as expected\n"); + + ret = regmap_bulk_read(priv->regmap, BNO055_UID_LOWER_REG, + priv->uid, BNO055_UID_LEN); + if (ret) + return ret; + + /* Sensor calibration data */ + fw_name_buf = kasprintf(GFP_KERNEL, BNO055_FW_UID_FMT, + BNO055_UID_LEN, priv->uid); + if (!fw_name_buf) + return -ENOMEM; + + ret = request_firmware(&caldata, fw_name_buf, dev); + kfree(fw_name_buf); + if (ret) + ret = request_firmware(&caldata, BNO055_FW_GENERIC_NAME, dev); + if (ret) { + dev_notice(dev, "Calibration file load failed. See instruction in kernel Documentation/iio/bno055.rst\n"); + ret = bno055_init(priv, NULL, 0); + } else { + ret = bno055_init(priv, caldata->data, caldata->size); + release_firmware(caldata); + } + if (ret) + return ret; + + priv->operation_mode = BNO055_OPR_MODE_FUSION; + ret = bno055_operation_mode_do_set(priv, priv->operation_mode); + if (ret) + return ret; + + ret = devm_add_action_or_reset(dev, bno055_uninit, priv); + if (ret) + return ret; + + iio_dev->channels = bno055_channels; + iio_dev->num_channels = ARRAY_SIZE(bno055_channels); + iio_dev->info = &bno055_info; + iio_dev->modes = INDIO_DIRECT_MODE; + + ret = devm_iio_triggered_buffer_setup(dev, iio_dev, + iio_pollfunc_store_time, + bno055_trigger_handler, + &bno055_buffer_setup_ops); + if (ret) + return ret; + + ret = devm_iio_device_register(dev, iio_dev); + if (ret) + return ret; + + bno055_debugfs_init(iio_dev); + + return 0; +} +EXPORT_SYMBOL_NS_GPL(bno055_probe, IIO_BNO055); + +MODULE_AUTHOR("Andrea Merello "); +MODULE_DESCRIPTION("Bosch BNO055 driver"); +MODULE_LICENSE("GPL"); diff --git a/drivers/iio/imu/bno055/bno055.h b/drivers/iio/imu/bno055/bno055.h new file mode 100644 index 000000000..64f9fc95c --- /dev/null +++ b/drivers/iio/imu/bno055/bno055.h @@ -0,0 +1,13 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ +#ifndef __BNO055_H__ +#define __BNO055_H__ + +#include +#include + +struct device; +int bno055_probe(struct device *dev, struct regmap *regmap, + int xfer_burst_break_thr, bool sw_reset); +extern const struct regmap_config bno055_regmap_config; + +#endif diff --git a/drivers/iio/imu/bno055/bno055_i2c.c b/drivers/iio/imu/bno055/bno055_i2c.c new file mode 100644 index 000000000..c1bbc0fe3 --- /dev/null +++ b/drivers/iio/imu/bno055/bno055_i2c.c @@ -0,0 +1,57 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Support for I2C-interfaced Bosch BNO055 IMU. + * + * Copyright (C) 2021-2022 Istituto Italiano di Tecnologia + * Electronic Design Laboratory + * Written by Andrea Merello + */ + +#include +#include +#include +#include + +#include "bno055.h" + +#define BNO055_I2C_XFER_BURST_BREAK_THRESHOLD 3 + +static int bno055_i2c_probe(struct i2c_client *client) +{ + struct regmap *regmap; + + regmap = devm_regmap_init_i2c(client, &bno055_regmap_config); + if (IS_ERR(regmap)) + return dev_err_probe(&client->dev, PTR_ERR(regmap), + "Unable to init register map"); + + return bno055_probe(&client->dev, regmap, + BNO055_I2C_XFER_BURST_BREAK_THRESHOLD, true); +} + +static const struct i2c_device_id bno055_i2c_id[] = { + {"bno055", 0}, + { } +}; +MODULE_DEVICE_TABLE(i2c, bno055_i2c_id); + +static const struct of_device_id __maybe_unused bno055_i2c_of_match[] = { + { .compatible = "bosch,bno055" }, + { } +}; +MODULE_DEVICE_TABLE(of, bno055_i2c_of_match); + +static struct i2c_driver bno055_driver = { + .driver = { + .name = "bno055-i2c", + .of_match_table = bno055_i2c_of_match, + }, + .probe_new = bno055_i2c_probe, + .id_table = bno055_i2c_id, +}; +module_i2c_driver(bno055_driver); + +MODULE_AUTHOR("Andrea Merello"); +MODULE_DESCRIPTION("Bosch BNO055 I2C interface"); +MODULE_IMPORT_NS(IIO_BNO055); +MODULE_LICENSE("GPL"); diff --git a/drivers/iio/imu/bno055/bno055_ser_core.c b/drivers/iio/imu/bno055/bno055_ser_core.c new file mode 100644 index 000000000..57728a568 --- /dev/null +++ b/drivers/iio/imu/bno055/bno055_ser_core.c @@ -0,0 +1,560 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Serial line interface for Bosh BNO055 IMU (via serdev). + * This file implements serial communication up to the register read/write + * level. + * + * Copyright (C) 2021-2022 Istituto Italiano di Tecnologia + * Electronic Design Laboratory + * Written by Andrea Merello + * + * This driver is based on + * Plantower PMS7003 particulate matter sensor driver + * Which is + * Copyright (c) Tomasz Duszynski + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "bno055_ser_trace.h" +#include "bno055.h" + +/* + * Register writes cmd have the following format + * +------+------+-----+-----+----- ... ----+ + * | 0xAA | 0xOO | REG | LEN | payload[LEN] | + * +------+------+-----+-----+----- ... ----+ + * + * Register write responses have the following format + * +------+----------+ + * | 0xEE | ERROCODE | + * +------+----------+ + * + * .. except when writing the SYS_RST bit (i.e. triggering a system reset); in + * case the IMU accepts the command, then it resets without responding. We don't + * handle this (yet) here (so we inform the common bno055 code not to perform + * sw resets - bno055 on serial bus basically requires the hw reset pin). + * + * Register read have the following format + * +------+------+-----+-----+ + * | 0xAA | 0xO1 | REG | LEN | + * +------+------+-----+-----+ + * + * Successful register read response have the following format + * +------+-----+----- ... ----+ + * | 0xBB | LEN | payload[LEN] | + * +------+-----+----- ... ----+ + * + * Failed register read response have the following format + * +------+--------+ + * | 0xEE | ERRCODE| (ERRCODE always > 1) + * +------+--------+ + * + * Error codes are + * 01: OK + * 02: read/write FAIL + * 04: invalid address + * 05: write on RO + * 06: wrong start byte + * 07: bus overrun + * 08: len too high + * 09: len too low + * 10: bus RX byte timeout (timeout is 30mS) + * + * + * **WORKAROUND ALERT** + * + * Serial communication seems very fragile: the BNO055 buffer seems to overflow + * very easy; BNO055 seems able to sink few bytes, then it needs a brief pause. + * On the other hand, it is also picky on timeout: if there is a pause > 30mS in + * between two bytes then the transaction fails (IMU internal RX FSM resets). + * + * BNO055 has been seen also failing to process commands in case we send them + * too close each other (or if it is somehow busy?) + * + * In particular I saw these scenarios: + * 1) If we send 2 bytes per time, then the IMU never(?) overflows. + * 2) If we send 4 bytes per time (i.e. the full header), then the IMU could + * overflow, but it seem to sink all 4 bytes, then it returns error. + * 3) If we send more than 4 bytes, the IMU could overflow, and I saw it sending + * error after 4 bytes are sent; we have troubles in synchronizing again, + * because we are still sending data, and the IMU interprets it as the 1st + * byte of a new command. + * + * While we must avoid case 3, we could send 4 bytes per time and eventually + * retry in case of failure; this seemed convenient for reads (which requires + * TXing exactly 4 bytes), however it has been seen that, depending by the IMU + * settings (e.g. LPF), failures became less or more frequent; in certain IMU + * configurations they are very rare, but in certain others we keeps failing + * even after like 30 retries. + * + * So, we just split TXes in [2-bytes + delay] steps, and still keep an eye on + * the IMU response; in case it overflows (which is now unlikely), we retry. + */ + +/* + * Read operation overhead: + * 4 bytes req + 2byte resp hdr. + * 6 bytes = 60 bit (considering 1start + 1stop bits). + * 60/115200 = ~520uS + about 2500mS delay -> ~3mS + * In 3mS we could read back about 34 bytes that means 17 samples, this means + * that in case of scattered reads in which the gap is 17 samples or less it is + * still convenient to go for a burst. + * We have to take into account also IMU response time - IMU seems to be often + * reasonably quick to respond, but sometimes it seems to be in some "critical + * section" in which it delays handling of serial protocol. Because of this we + * round-up to 22, which is the max number of samples, always bursting indeed. + */ +#define BNO055_SER_XFER_BURST_BREAK_THRESHOLD 22 + +struct bno055_ser_priv { + enum { + CMD_NONE, + CMD_READ, + CMD_WRITE, + } expect_response; + int expected_data_len; + u8 *response_buf; + + /** + * enum cmd_status - represent the status of a command sent to the HW. + * @STATUS_CRIT: The command failed: the serial communication failed. + * @STATUS_OK: The command executed successfully. + * @STATUS_FAIL: The command failed: HW responded with an error. + */ + enum { + STATUS_CRIT = -1, + STATUS_OK = 0, + STATUS_FAIL = 1, + } cmd_status; + + /* + * Protects all the above fields, which are accessed in behalf of both + * the serdev RX callback and the regmap side + */ + struct mutex lock; + + /* Only accessed in serdev RX callback context*/ + struct { + enum { + RX_IDLE, + RX_START, + RX_DATA, + } state; + int databuf_count; + int expected_len; + int type; + } rx; + + /* Never accessed in behalf of serdev RX callback context */ + bool cmd_stale; + + struct completion cmd_complete; + struct serdev_device *serdev; +}; + +static int bno055_ser_send_chunk(struct bno055_ser_priv *priv, const u8 *data, int len) +{ + int ret; + + trace_send_chunk(len, data); + ret = serdev_device_write(priv->serdev, data, len, msecs_to_jiffies(25)); + if (ret < 0) + return ret; + + if (ret < len) + return -EIO; + + return 0; +} + +/* + * Send a read or write command. + * 'data' can be NULL (used in read case). 'len' parameter is always valid; in + * case 'data' is non-NULL then it must match 'data' size. + */ +static int bno055_ser_do_send_cmd(struct bno055_ser_priv *priv, + bool read, int addr, int len, const u8 *data) +{ + u8 hdr[] = {0xAA, read, addr, len}; + int chunk_len; + int ret; + + ret = bno055_ser_send_chunk(priv, hdr, 2); + if (ret) + goto fail; + usleep_range(2000, 3000); + ret = bno055_ser_send_chunk(priv, hdr + 2, 2); + if (ret) + goto fail; + + if (read) + return 0; + + while (len) { + chunk_len = min(len, 2); + usleep_range(2000, 3000); + ret = bno055_ser_send_chunk(priv, data, chunk_len); + if (ret) + goto fail; + data += chunk_len; + len -= chunk_len; + } + + return 0; +fail: + /* waiting more than 30mS should clear the BNO055 internal state */ + usleep_range(40000, 50000); + return ret; +} + +static int bno055_ser_send_cmd(struct bno055_ser_priv *priv, + bool read, int addr, int len, const u8 *data) +{ + const int retry_max = 5; + int retry = retry_max; + int ret = 0; + + /* + * In case previous command was interrupted we still need to wait it to + * complete before we can issue new commands + */ + if (priv->cmd_stale) { + ret = wait_for_completion_interruptible_timeout(&priv->cmd_complete, + msecs_to_jiffies(100)); + if (ret == -ERESTARTSYS) + return -ERESTARTSYS; + + priv->cmd_stale = false; + /* if serial protocol broke, bail out */ + if (priv->cmd_status == STATUS_CRIT) + return -EIO; + } + + /* + * Try to convince the IMU to cooperate.. as explained in the comments + * at the top of this file, the IMU could also refuse the command (i.e. + * it is not ready yet); retry in this case. + */ + do { + mutex_lock(&priv->lock); + priv->expect_response = read ? CMD_READ : CMD_WRITE; + reinit_completion(&priv->cmd_complete); + mutex_unlock(&priv->lock); + + if (retry != retry_max) + trace_cmd_retry(read, addr, retry_max - retry); + ret = bno055_ser_do_send_cmd(priv, read, addr, len, data); + if (ret) + continue; + + ret = wait_for_completion_interruptible_timeout(&priv->cmd_complete, + msecs_to_jiffies(100)); + if (ret == -ERESTARTSYS) { + priv->cmd_stale = true; + return -ERESTARTSYS; + } + + if (!ret) + return -ETIMEDOUT; + + if (priv->cmd_status == STATUS_OK) + return 0; + if (priv->cmd_status == STATUS_CRIT) + return -EIO; + + /* loop in case priv->cmd_status == STATUS_FAIL */ + } while (--retry); + + if (ret < 0) + return ret; + if (priv->cmd_status == STATUS_FAIL) + return -EINVAL; + return 0; +} + +static int bno055_ser_write_reg(void *context, const void *_data, size_t count) +{ + const u8 *data = _data; + struct bno055_ser_priv *priv = context; + + if (count < 2) { + dev_err(&priv->serdev->dev, "Invalid write count %zu", count); + return -EINVAL; + } + + trace_write_reg(data[0], data[1]); + return bno055_ser_send_cmd(priv, 0, data[0], count - 1, data + 1); +} + +static int bno055_ser_read_reg(void *context, + const void *_reg, size_t reg_size, + void *val, size_t val_size) +{ + int ret; + int reg_addr; + const u8 *reg = _reg; + struct bno055_ser_priv *priv = context; + + if (val_size > 128) { + dev_err(&priv->serdev->dev, "Invalid read valsize %zu", val_size); + return -EINVAL; + } + + reg_addr = *reg; + trace_read_reg(reg_addr, val_size); + mutex_lock(&priv->lock); + priv->expected_data_len = val_size; + priv->response_buf = val; + mutex_unlock(&priv->lock); + + ret = bno055_ser_send_cmd(priv, 1, reg_addr, val_size, NULL); + + mutex_lock(&priv->lock); + priv->response_buf = NULL; + mutex_unlock(&priv->lock); + + return ret; +} + +/* + * Handler for received data; this is called from the receiver callback whenever + * it got some packet from the serial bus. The status tells us whether the + * packet is valid (i.e. header ok && received payload len consistent wrt the + * header). It's now our responsibility to check whether this is what we + * expected, of whether we got some unexpected, yet valid, packet. + */ +static void bno055_ser_handle_rx(struct bno055_ser_priv *priv, int status) +{ + mutex_lock(&priv->lock); + switch (priv->expect_response) { + case CMD_NONE: + dev_warn(&priv->serdev->dev, "received unexpected, yet valid, data from sensor"); + mutex_unlock(&priv->lock); + return; + + case CMD_READ: + priv->cmd_status = status; + if (status == STATUS_OK && + priv->rx.databuf_count != priv->expected_data_len) { + /* + * If we got here, then the lower layer serial protocol + * seems consistent with itself; if we got an unexpected + * amount of data then signal it as a non critical error + */ + priv->cmd_status = STATUS_FAIL; + dev_warn(&priv->serdev->dev, + "received an unexpected amount of, yet valid, data from sensor"); + } + break; + + case CMD_WRITE: + priv->cmd_status = status; + break; + } + + priv->expect_response = CMD_NONE; + mutex_unlock(&priv->lock); + complete(&priv->cmd_complete); +} + +/* + * Serdev receiver FSM. This tracks the serial communication and parse the + * header. It pushes packets to bno055_ser_handle_rx(), eventually communicating + * failures (i.e. malformed packets). + * Ideally it doesn't know anything about upper layer (i.e. if this is the + * packet we were really expecting), but since we copies the payload into the + * receiver buffer (that is not valid when i.e. we don't expect data), we + * snoop a bit in the upper layer.. + * Also, we assume to RX one pkt per time (i.e. the HW doesn't send anything + * unless we require to AND we don't queue more than one request per time). + */ +static int bno055_ser_receive_buf(struct serdev_device *serdev, + const unsigned char *buf, size_t size) +{ + int status; + struct bno055_ser_priv *priv = serdev_device_get_drvdata(serdev); + int remaining = size; + + if (size == 0) + return 0; + + trace_recv(size, buf); + switch (priv->rx.state) { + case RX_IDLE: + /* + * New packet. + * Check for its 1st byte that identifies the pkt type. + */ + if (buf[0] != 0xEE && buf[0] != 0xBB) { + dev_err(&priv->serdev->dev, + "Invalid packet start %x", buf[0]); + bno055_ser_handle_rx(priv, STATUS_CRIT); + break; + } + priv->rx.type = buf[0]; + priv->rx.state = RX_START; + remaining--; + buf++; + priv->rx.databuf_count = 0; + fallthrough; + + case RX_START: + /* + * Packet RX in progress, we expect either 1-byte len or 1-byte + * status depending by the packet type. + */ + if (remaining == 0) + break; + + if (priv->rx.type == 0xEE) { + if (remaining > 1) { + dev_err(&priv->serdev->dev, "EE pkt. Extra data received"); + status = STATUS_CRIT; + } else { + status = (buf[0] == 1) ? STATUS_OK : STATUS_FAIL; + } + bno055_ser_handle_rx(priv, status); + priv->rx.state = RX_IDLE; + break; + + } else { + /*priv->rx.type == 0xBB */ + priv->rx.state = RX_DATA; + priv->rx.expected_len = buf[0]; + remaining--; + buf++; + } + fallthrough; + + case RX_DATA: + /* Header parsed; now receiving packet data payload */ + if (remaining == 0) + break; + + if (priv->rx.databuf_count + remaining > priv->rx.expected_len) { + /* + * This is an inconsistency in serial protocol, we lost + * sync and we don't know how to handle further data + */ + dev_err(&priv->serdev->dev, "BB pkt. Extra data received"); + bno055_ser_handle_rx(priv, STATUS_CRIT); + priv->rx.state = RX_IDLE; + break; + } + + mutex_lock(&priv->lock); + /* + * NULL e.g. when read cmd is stale or when no read cmd is + * actually pending. + */ + if (priv->response_buf && + /* + * Snoop on the upper layer protocol stuff to make sure not + * to write to an invalid memory. Apart for this, let's the + * upper layer manage any inconsistency wrt expected data + * len (as long as the serial protocol is consistent wrt + * itself (i.e. response header is consistent with received + * response len. + */ + (priv->rx.databuf_count + remaining <= priv->expected_data_len)) + memcpy(priv->response_buf + priv->rx.databuf_count, + buf, remaining); + mutex_unlock(&priv->lock); + + priv->rx.databuf_count += remaining; + + /* + * Reached expected len advertised by the IMU for the current + * packet. Pass it to the upper layer (for us it is just valid). + */ + if (priv->rx.databuf_count == priv->rx.expected_len) { + bno055_ser_handle_rx(priv, STATUS_OK); + priv->rx.state = RX_IDLE; + } + break; + } + + return size; +} + +static const struct serdev_device_ops bno055_ser_serdev_ops = { + .receive_buf = bno055_ser_receive_buf, + .write_wakeup = serdev_device_write_wakeup, +}; + +static struct regmap_bus bno055_ser_regmap_bus = { + .write = bno055_ser_write_reg, + .read = bno055_ser_read_reg, +}; + +static int bno055_ser_probe(struct serdev_device *serdev) +{ + struct bno055_ser_priv *priv; + struct regmap *regmap; + int ret; + + priv = devm_kzalloc(&serdev->dev, sizeof(*priv), GFP_KERNEL); + if (!priv) + return -ENOMEM; + + serdev_device_set_drvdata(serdev, priv); + priv->serdev = serdev; + mutex_init(&priv->lock); + init_completion(&priv->cmd_complete); + + serdev_device_set_client_ops(serdev, &bno055_ser_serdev_ops); + ret = devm_serdev_device_open(&serdev->dev, serdev); + if (ret) + return ret; + + if (serdev_device_set_baudrate(serdev, 115200) != 115200) { + dev_err(&serdev->dev, "Cannot set required baud rate"); + return -EIO; + } + + ret = serdev_device_set_parity(serdev, SERDEV_PARITY_NONE); + if (ret) { + dev_err(&serdev->dev, "Cannot set required parity setting"); + return ret; + } + serdev_device_set_flow_control(serdev, false); + + regmap = devm_regmap_init(&serdev->dev, &bno055_ser_regmap_bus, + priv, &bno055_regmap_config); + if (IS_ERR(regmap)) + return dev_err_probe(&serdev->dev, PTR_ERR(regmap), + "Unable to init register map"); + + return bno055_probe(&serdev->dev, regmap, + BNO055_SER_XFER_BURST_BREAK_THRESHOLD, false); +} + +static const struct of_device_id bno055_ser_of_match[] = { + { .compatible = "bosch,bno055" }, + { } +}; +MODULE_DEVICE_TABLE(of, bno055_ser_of_match); + +static struct serdev_device_driver bno055_ser_driver = { + .driver = { + .name = "bno055-ser", + .of_match_table = bno055_ser_of_match, + }, + .probe = bno055_ser_probe, +}; +module_serdev_device_driver(bno055_ser_driver); + +MODULE_AUTHOR("Andrea Merello "); +MODULE_DESCRIPTION("Bosch BNO055 serdev interface"); +MODULE_IMPORT_NS(IIO_BNO055); +MODULE_LICENSE("GPL"); diff --git a/drivers/iio/imu/bno055/bno055_ser_trace.c b/drivers/iio/imu/bno055/bno055_ser_trace.c new file mode 100644 index 000000000..48397b66d --- /dev/null +++ b/drivers/iio/imu/bno055/bno055_ser_trace.c @@ -0,0 +1,14 @@ +//SPDX-License-Identifier: GPL-2.0 + +/* + * bno055_ser Trace Support + * Copyright (C) 2022 Istituto Italiano di Tecnologia + * Electronic Design Laboratory + * + * Based on: + * Device core Trace Support + * Copyright (C) 2021, Intel Corporation + */ + +#define CREATE_TRACE_POINTS +#include "bno055_ser_trace.h" diff --git a/drivers/iio/imu/bno055/bno055_ser_trace.h b/drivers/iio/imu/bno055/bno055_ser_trace.h new file mode 100644 index 000000000..7d9eae166 --- /dev/null +++ b/drivers/iio/imu/bno055/bno055_ser_trace.h @@ -0,0 +1,104 @@ +/* SPDX-License-Identifier: GPL-2.0 */ + +#if !defined(__BNO055_SERDEV_TRACE_H__) || defined(TRACE_HEADER_MULTI_READ) +#define __BNO055_SERDEV_TRACE_H__ + +#include + +#undef TRACE_SYSTEM +#define TRACE_SYSTEM bno055_ser + +TRACE_EVENT(send_chunk, + TP_PROTO(int len, const u8 *data), + TP_ARGS(len, data), + TP_STRUCT__entry( + __field(int, len) + __dynamic_array(u8, chunk, len) + ), + TP_fast_assign( + __entry->len = len; + memcpy(__get_dynamic_array(chunk), + data, __entry->len); + ), + TP_printk("len: %d, data: = %*ph", + __entry->len, __entry->len, __get_dynamic_array(chunk) + ) +); + +TRACE_EVENT(cmd_retry, + TP_PROTO(bool read, int addr, int retry), + TP_ARGS(read, addr, retry), + TP_STRUCT__entry( + __field(bool, read) + __field(int, addr) + __field(int, retry) + ), + TP_fast_assign( + __entry->read = read; + __entry->addr = addr; + __entry->retry = retry; + ), + TP_printk("%s addr 0x%x retry #%d", + __entry->read ? "read" : "write", + __entry->addr, __entry->retry + ) +); + +TRACE_EVENT(write_reg, + TP_PROTO(u8 addr, u8 value), + TP_ARGS(addr, value), + TP_STRUCT__entry( + __field(u8, addr) + __field(u8, value) + ), + TP_fast_assign( + __entry->addr = addr; + __entry->value = value; + ), + TP_printk("reg 0x%x = 0x%x", + __entry->addr, __entry->value + ) +); + +TRACE_EVENT(read_reg, + TP_PROTO(int addr, size_t len), + TP_ARGS(addr, len), + TP_STRUCT__entry( + __field(int, addr) + __field(size_t, len) + ), + TP_fast_assign( + __entry->addr = addr; + __entry->len = len; + ), + TP_printk("reg 0x%x (len %zu)", + __entry->addr, __entry->len + ) +); + +TRACE_EVENT(recv, + TP_PROTO(size_t len, const unsigned char *buf), + TP_ARGS(len, buf), + TP_STRUCT__entry( + __field(size_t, len) + __dynamic_array(unsigned char, buf, len) + ), + TP_fast_assign( + __entry->len = len; + memcpy(__get_dynamic_array(buf), + buf, __entry->len); + ), + TP_printk("len: %zu, data: = %*ph", + __entry->len, (int)__entry->len, __get_dynamic_array(buf) + ) +); + +#endif /* __BNO055_SERDEV_TRACE_H__ || TRACE_HEADER_MULTI_READ */ + +#undef TRACE_INCLUDE_PATH +#define TRACE_INCLUDE_PATH . +#undef TRACE_INCLUDE_FILE +#define TRACE_INCLUDE_FILE bno055_ser_trace + +/* This part must be outside protection */ +#include diff --git a/drivers/iio/imu/fxos8700.h b/drivers/iio/imu/fxos8700.h new file mode 100644 index 000000000..6dfb8d709 --- /dev/null +++ b/drivers/iio/imu/fxos8700.h @@ -0,0 +1,10 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +#ifndef FXOS8700_H_ +#define FXOS8700_H_ + +extern const struct regmap_config fxos8700_regmap_config; + +int fxos8700_core_probe(struct device *dev, struct regmap *regmap, + const char *name, bool use_spi); + +#endif /* FXOS8700_H_ */ diff --git a/drivers/iio/imu/fxos8700_core.c b/drivers/iio/imu/fxos8700_core.c new file mode 100644 index 000000000..6d189c4b9 --- /dev/null +++ b/drivers/iio/imu/fxos8700_core.c @@ -0,0 +1,715 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * FXOS8700 - NXP IMU (accelerometer plus magnetometer) + * + * IIO core driver for FXOS8700, with support for I2C/SPI busses + * + * TODO: Buffer, trigger, and IRQ support + */ +#include +#include +#include +#include +#include + +#include +#include + +#include "fxos8700.h" + +/* Register Definitions */ +#define FXOS8700_STATUS 0x00 +#define FXOS8700_OUT_X_MSB 0x01 +#define FXOS8700_OUT_X_LSB 0x02 +#define FXOS8700_OUT_Y_MSB 0x03 +#define FXOS8700_OUT_Y_LSB 0x04 +#define FXOS8700_OUT_Z_MSB 0x05 +#define FXOS8700_OUT_Z_LSB 0x06 +#define FXOS8700_F_SETUP 0x09 +#define FXOS8700_TRIG_CFG 0x0a +#define FXOS8700_SYSMOD 0x0b +#define FXOS8700_INT_SOURCE 0x0c +#define FXOS8700_WHO_AM_I 0x0d +#define FXOS8700_XYZ_DATA_CFG 0x0e +#define FXOS8700_HP_FILTER_CUTOFF 0x0f +#define FXOS8700_PL_STATUS 0x10 +#define FXOS8700_PL_CFG 0x11 +#define FXOS8700_PL_COUNT 0x12 +#define FXOS8700_PL_BF_ZCOMP 0x13 +#define FXOS8700_PL_THS_REG 0x14 +#define FXOS8700_A_FFMT_CFG 0x15 +#define FXOS8700_A_FFMT_SRC 0x16 +#define FXOS8700_A_FFMT_THS 0x17 +#define FXOS8700_A_FFMT_COUNT 0x18 +#define FXOS8700_TRANSIENT_CFG 0x1d +#define FXOS8700_TRANSIENT_SRC 0x1e +#define FXOS8700_TRANSIENT_THS 0x1f +#define FXOS8700_TRANSIENT_COUNT 0x20 +#define FXOS8700_PULSE_CFG 0x21 +#define FXOS8700_PULSE_SRC 0x22 +#define FXOS8700_PULSE_THSX 0x23 +#define FXOS8700_PULSE_THSY 0x24 +#define FXOS8700_PULSE_THSZ 0x25 +#define FXOS8700_PULSE_TMLT 0x26 +#define FXOS8700_PULSE_LTCY 0x27 +#define FXOS8700_PULSE_WIND 0x28 +#define FXOS8700_ASLP_COUNT 0x29 +#define FXOS8700_CTRL_REG1 0x2a +#define FXOS8700_CTRL_REG2 0x2b +#define FXOS8700_CTRL_REG3 0x2c +#define FXOS8700_CTRL_REG4 0x2d +#define FXOS8700_CTRL_REG5 0x2e +#define FXOS8700_OFF_X 0x2f +#define FXOS8700_OFF_Y 0x30 +#define FXOS8700_OFF_Z 0x31 +#define FXOS8700_M_DR_STATUS 0x32 +#define FXOS8700_M_OUT_X_MSB 0x33 +#define FXOS8700_M_OUT_X_LSB 0x34 +#define FXOS8700_M_OUT_Y_MSB 0x35 +#define FXOS8700_M_OUT_Y_LSB 0x36 +#define FXOS8700_M_OUT_Z_MSB 0x37 +#define FXOS8700_M_OUT_Z_LSB 0x38 +#define FXOS8700_CMP_X_MSB 0x39 +#define FXOS8700_CMP_X_LSB 0x3a +#define FXOS8700_CMP_Y_MSB 0x3b +#define FXOS8700_CMP_Y_LSB 0x3c +#define FXOS8700_CMP_Z_MSB 0x3d +#define FXOS8700_CMP_Z_LSB 0x3e +#define FXOS8700_M_OFF_X_MSB 0x3f +#define FXOS8700_M_OFF_X_LSB 0x40 +#define FXOS8700_M_OFF_Y_MSB 0x41 +#define FXOS8700_M_OFF_Y_LSB 0x42 +#define FXOS8700_M_OFF_Z_MSB 0x43 +#define FXOS8700_M_OFF_Z_LSB 0x44 +#define FXOS8700_MAX_X_MSB 0x45 +#define FXOS8700_MAX_X_LSB 0x46 +#define FXOS8700_MAX_Y_MSB 0x47 +#define FXOS8700_MAX_Y_LSB 0x48 +#define FXOS8700_MAX_Z_MSB 0x49 +#define FXOS8700_MAX_Z_LSB 0x4a +#define FXOS8700_MIN_X_MSB 0x4b +#define FXOS8700_MIN_X_LSB 0x4c +#define FXOS8700_MIN_Y_MSB 0x4d +#define FXOS8700_MIN_Y_LSB 0x4e +#define FXOS8700_MIN_Z_MSB 0x4f +#define FXOS8700_MIN_Z_LSB 0x50 +#define FXOS8700_TEMP 0x51 +#define FXOS8700_M_THS_CFG 0x52 +#define FXOS8700_M_THS_SRC 0x53 +#define FXOS8700_M_THS_X_MSB 0x54 +#define FXOS8700_M_THS_X_LSB 0x55 +#define FXOS8700_M_THS_Y_MSB 0x56 +#define FXOS8700_M_THS_Y_LSB 0x57 +#define FXOS8700_M_THS_Z_MSB 0x58 +#define FXOS8700_M_THS_Z_LSB 0x59 +#define FXOS8700_M_THS_COUNT 0x5a +#define FXOS8700_M_CTRL_REG1 0x5b +#define FXOS8700_M_CTRL_REG2 0x5c +#define FXOS8700_M_CTRL_REG3 0x5d +#define FXOS8700_M_INT_SRC 0x5e +#define FXOS8700_A_VECM_CFG 0x5f +#define FXOS8700_A_VECM_THS_MSB 0x60 +#define FXOS8700_A_VECM_THS_LSB 0x61 +#define FXOS8700_A_VECM_CNT 0x62 +#define FXOS8700_A_VECM_INITX_MSB 0x63 +#define FXOS8700_A_VECM_INITX_LSB 0x64 +#define FXOS8700_A_VECM_INITY_MSB 0x65 +#define FXOS8700_A_VECM_INITY_LSB 0x66 +#define FXOS8700_A_VECM_INITZ_MSB 0x67 +#define FXOS8700_A_VECM_INITZ_LSB 0x68 +#define FXOS8700_M_VECM_CFG 0x69 +#define FXOS8700_M_VECM_THS_MSB 0x6a +#define FXOS8700_M_VECM_THS_LSB 0x6b +#define FXOS8700_M_VECM_CNT 0x6c +#define FXOS8700_M_VECM_INITX_MSB 0x6d +#define FXOS8700_M_VECM_INITX_LSB 0x6e +#define FXOS8700_M_VECM_INITY_MSB 0x6f +#define FXOS8700_M_VECM_INITY_LSB 0x70 +#define FXOS8700_M_VECM_INITZ_MSB 0x71 +#define FXOS8700_M_VECM_INITZ_LSB 0x72 +#define FXOS8700_A_FFMT_THS_X_MSB 0x73 +#define FXOS8700_A_FFMT_THS_X_LSB 0x74 +#define FXOS8700_A_FFMT_THS_Y_MSB 0x75 +#define FXOS8700_A_FFMT_THS_Y_LSB 0x76 +#define FXOS8700_A_FFMT_THS_Z_MSB 0x77 +#define FXOS8700_A_FFMT_THS_Z_LSB 0x78 +#define FXOS8700_A_TRAN_INIT_MSB 0x79 +#define FXOS8700_A_TRAN_INIT_LSB_X 0x7a +#define FXOS8700_A_TRAN_INIT_LSB_Y 0x7b +#define FXOS8700_A_TRAN_INIT_LSB_Z 0x7d +#define FXOS8700_TM_NVM_LOCK 0x7e +#define FXOS8700_NVM_DATA0_35 0x80 +#define FXOS8700_NVM_DATA_BNK3 0xa4 +#define FXOS8700_NVM_DATA_BNK2 0xa5 +#define FXOS8700_NVM_DATA_BNK1 0xa6 +#define FXOS8700_NVM_DATA_BNK0 0xa7 + +/* Bit definitions for FXOS8700_CTRL_REG1 */ +#define FXOS8700_CTRL_ODR_MAX 0x00 +#define FXOS8700_CTRL_ODR_MSK GENMASK(5, 3) + +/* Bit definitions for FXOS8700_M_CTRL_REG1 */ +#define FXOS8700_HMS_MASK GENMASK(1, 0) +#define FXOS8700_OS_MASK GENMASK(4, 2) + +/* Bit definitions for FXOS8700_M_CTRL_REG2 */ +#define FXOS8700_MAXMIN_RST BIT(2) +#define FXOS8700_MAXMIN_DIS_THS BIT(3) +#define FXOS8700_MAXMIN_DIS BIT(4) + +#define FXOS8700_ACTIVE 0x01 +#define FXOS8700_ACTIVE_MIN_USLEEP 4000 /* from table 6 in datasheet */ + +#define FXOS8700_DEVICE_ID 0xC7 +#define FXOS8700_PRE_DEVICE_ID 0xC4 +#define FXOS8700_DATA_BUF_SIZE 3 + +struct fxos8700_data { + struct regmap *regmap; + struct iio_trigger *trig; + __be16 buf[FXOS8700_DATA_BUF_SIZE] __aligned(IIO_DMA_MINALIGN); +}; + +/* Regmap info */ +static const struct regmap_range read_range[] = { + { + .range_min = FXOS8700_STATUS, + .range_max = FXOS8700_A_FFMT_COUNT, + }, { + .range_min = FXOS8700_TRANSIENT_CFG, + .range_max = FXOS8700_A_FFMT_THS_Z_LSB, + }, +}; + +static const struct regmap_range write_range[] = { + { + .range_min = FXOS8700_F_SETUP, + .range_max = FXOS8700_TRIG_CFG, + }, { + .range_min = FXOS8700_XYZ_DATA_CFG, + .range_max = FXOS8700_HP_FILTER_CUTOFF, + }, { + .range_min = FXOS8700_PL_CFG, + .range_max = FXOS8700_A_FFMT_CFG, + }, { + .range_min = FXOS8700_A_FFMT_THS, + .range_max = FXOS8700_TRANSIENT_CFG, + }, { + .range_min = FXOS8700_TRANSIENT_THS, + .range_max = FXOS8700_PULSE_CFG, + }, { + .range_min = FXOS8700_PULSE_THSX, + .range_max = FXOS8700_OFF_Z, + }, { + .range_min = FXOS8700_M_OFF_X_MSB, + .range_max = FXOS8700_M_OFF_Z_LSB, + }, { + .range_min = FXOS8700_M_THS_CFG, + .range_max = FXOS8700_M_THS_CFG, + }, { + .range_min = FXOS8700_M_THS_X_MSB, + .range_max = FXOS8700_M_CTRL_REG3, + }, { + .range_min = FXOS8700_A_VECM_CFG, + .range_max = FXOS8700_A_FFMT_THS_Z_LSB, + }, +}; + +static const struct regmap_access_table driver_read_table = { + .yes_ranges = read_range, + .n_yes_ranges = ARRAY_SIZE(read_range), +}; + +static const struct regmap_access_table driver_write_table = { + .yes_ranges = write_range, + .n_yes_ranges = ARRAY_SIZE(write_range), +}; + +const struct regmap_config fxos8700_regmap_config = { + .reg_bits = 8, + .val_bits = 8, + .max_register = FXOS8700_NVM_DATA_BNK0, + .rd_table = &driver_read_table, + .wr_table = &driver_write_table, +}; +EXPORT_SYMBOL(fxos8700_regmap_config); + +#define FXOS8700_CHANNEL(_type, _axis) { \ + .type = _type, \ + .modified = 1, \ + .channel2 = IIO_MOD_##_axis, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \ + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) | \ + BIT(IIO_CHAN_INFO_SAMP_FREQ), \ +} + +enum fxos8700_accel_scale_bits { + MODE_2G = 0, + MODE_4G, + MODE_8G, +}; + +/* scan indexes follow DATA register order */ +enum fxos8700_scan_axis { + FXOS8700_SCAN_ACCEL_X = 0, + FXOS8700_SCAN_ACCEL_Y, + FXOS8700_SCAN_ACCEL_Z, + FXOS8700_SCAN_MAGN_X, + FXOS8700_SCAN_MAGN_Y, + FXOS8700_SCAN_MAGN_Z, + FXOS8700_SCAN_RHALL, + FXOS8700_SCAN_TIMESTAMP, +}; + +enum fxos8700_sensor { + FXOS8700_ACCEL = 0, + FXOS8700_MAGN, + FXOS8700_NUM_SENSORS /* must be last */ +}; + +enum fxos8700_int_pin { + FXOS8700_PIN_INT1, + FXOS8700_PIN_INT2 +}; + +struct fxos8700_scale { + u8 bits; + int uscale; +}; + +struct fxos8700_odr { + u8 bits; + int odr; + int uodr; +}; + +static const struct fxos8700_scale fxos8700_accel_scale[] = { + { MODE_2G, 244}, + { MODE_4G, 488}, + { MODE_8G, 976}, +}; + +/* + * Accellerometer and magnetometer have the same ODR options, set in the + * CTRL_REG1 register. ODR is halved when using both sensors at once in + * hybrid mode. + */ +static const struct fxos8700_odr fxos8700_odr[] = { + {0x00, 800, 0}, + {0x01, 400, 0}, + {0x02, 200, 0}, + {0x03, 100, 0}, + {0x04, 50, 0}, + {0x05, 12, 500000}, + {0x06, 6, 250000}, + {0x07, 1, 562500}, +}; + +static const struct iio_chan_spec fxos8700_channels[] = { + FXOS8700_CHANNEL(IIO_ACCEL, X), + FXOS8700_CHANNEL(IIO_ACCEL, Y), + FXOS8700_CHANNEL(IIO_ACCEL, Z), + FXOS8700_CHANNEL(IIO_MAGN, X), + FXOS8700_CHANNEL(IIO_MAGN, Y), + FXOS8700_CHANNEL(IIO_MAGN, Z), + IIO_CHAN_SOFT_TIMESTAMP(FXOS8700_SCAN_TIMESTAMP), +}; + +static enum fxos8700_sensor fxos8700_to_sensor(enum iio_chan_type iio_type) +{ + switch (iio_type) { + case IIO_ACCEL: + return FXOS8700_ACCEL; + case IIO_MAGN: + return FXOS8700_MAGN; + default: + return -EINVAL; + } +} + +static int fxos8700_set_active_mode(struct fxos8700_data *data, + enum fxos8700_sensor t, bool mode) +{ + int ret; + + ret = regmap_write(data->regmap, FXOS8700_CTRL_REG1, mode); + if (ret) + return ret; + + usleep_range(FXOS8700_ACTIVE_MIN_USLEEP, + FXOS8700_ACTIVE_MIN_USLEEP + 1000); + + return 0; +} + +static int fxos8700_set_scale(struct fxos8700_data *data, + enum fxos8700_sensor t, int uscale) +{ + int i, ret, val; + bool active_mode; + static const int scale_num = ARRAY_SIZE(fxos8700_accel_scale); + struct device *dev = regmap_get_device(data->regmap); + + if (t == FXOS8700_MAGN) { + dev_err(dev, "Magnetometer scale is locked at 0.001Gs\n"); + return -EINVAL; + } + + /* + * When device is in active mode, it failed to set an ACCEL + * full-scale range(2g/4g/8g) in FXOS8700_XYZ_DATA_CFG. + * This is not align with the datasheet, but it is a fxos8700 + * chip behavier. Set the device in standby mode before setting + * an ACCEL full-scale range. + */ + ret = regmap_read(data->regmap, FXOS8700_CTRL_REG1, &val); + if (ret) + return ret; + + active_mode = val & FXOS8700_ACTIVE; + if (active_mode) { + ret = regmap_write(data->regmap, FXOS8700_CTRL_REG1, + val & ~FXOS8700_ACTIVE); + if (ret) + return ret; + } + + for (i = 0; i < scale_num; i++) + if (fxos8700_accel_scale[i].uscale == uscale) + break; + + if (i == scale_num) + return -EINVAL; + + ret = regmap_write(data->regmap, FXOS8700_XYZ_DATA_CFG, + fxos8700_accel_scale[i].bits); + if (ret) + return ret; + return regmap_write(data->regmap, FXOS8700_CTRL_REG1, + active_mode); +} + +static int fxos8700_get_scale(struct fxos8700_data *data, + enum fxos8700_sensor t, int *uscale) +{ + int i, ret, val; + static const int scale_num = ARRAY_SIZE(fxos8700_accel_scale); + + if (t == FXOS8700_MAGN) { + *uscale = 1000; /* Magnetometer is locked at 0.001Gs */ + return 0; + } + + ret = regmap_read(data->regmap, FXOS8700_XYZ_DATA_CFG, &val); + if (ret) + return ret; + + for (i = 0; i < scale_num; i++) { + if (fxos8700_accel_scale[i].bits == (val & 0x3)) { + *uscale = fxos8700_accel_scale[i].uscale; + return 0; + } + } + + return -EINVAL; +} + +static int fxos8700_get_data(struct fxos8700_data *data, int chan_type, + int axis, int *val) +{ + u8 base, reg; + s16 tmp; + int ret; + + /* + * Different register base addresses varies with channel types. + * This bug hasn't been noticed before because using an enum is + * really hard to read. Use an a switch statement to take over that. + */ + switch (chan_type) { + case IIO_ACCEL: + base = FXOS8700_OUT_X_MSB; + break; + case IIO_MAGN: + base = FXOS8700_M_OUT_X_MSB; + break; + default: + return -EINVAL; + } + + /* Block read 6 bytes of device output registers to avoid data loss */ + ret = regmap_bulk_read(data->regmap, base, data->buf, + sizeof(data->buf)); + if (ret) + return ret; + + /* Convert axis to buffer index */ + reg = axis - IIO_MOD_X; + + /* + * Convert to native endianness. The accel data and magn data + * are signed, so a forced type conversion is needed. + */ + tmp = be16_to_cpu(data->buf[reg]); + + /* + * ACCEL output data registers contain the X-axis, Y-axis, and Z-axis + * 14-bit left-justified sample data and MAGN output data registers + * contain the X-axis, Y-axis, and Z-axis 16-bit sample data. Apply + * a signed 2 bits right shift to the readback raw data from ACCEL + * output data register and keep that from MAGN sensor as the origin. + * Value should be extended to 32 bit. + */ + switch (chan_type) { + case IIO_ACCEL: + tmp = tmp >> 2; + break; + case IIO_MAGN: + /* Nothing to do */ + break; + default: + return -EINVAL; + } + + /* Convert to native endianness */ + *val = sign_extend32(tmp, 15); + + return 0; +} + +static int fxos8700_set_odr(struct fxos8700_data *data, enum fxos8700_sensor t, + int odr, int uodr) +{ + int i, ret, val; + bool active_mode; + static const int odr_num = ARRAY_SIZE(fxos8700_odr); + + ret = regmap_read(data->regmap, FXOS8700_CTRL_REG1, &val); + if (ret) + return ret; + + active_mode = val & FXOS8700_ACTIVE; + + if (active_mode) { + /* + * The device must be in standby mode to change any of the + * other fields within CTRL_REG1 + */ + ret = regmap_write(data->regmap, FXOS8700_CTRL_REG1, + val & ~FXOS8700_ACTIVE); + if (ret) + return ret; + } + + for (i = 0; i < odr_num; i++) + if (fxos8700_odr[i].odr == odr && fxos8700_odr[i].uodr == uodr) + break; + + if (i >= odr_num) + return -EINVAL; + + val &= ~FXOS8700_CTRL_ODR_MSK; + val |= FIELD_PREP(FXOS8700_CTRL_ODR_MSK, fxos8700_odr[i].bits) | FXOS8700_ACTIVE; + return regmap_write(data->regmap, FXOS8700_CTRL_REG1, val); +} + +static int fxos8700_get_odr(struct fxos8700_data *data, enum fxos8700_sensor t, + int *odr, int *uodr) +{ + int i, val, ret; + static const int odr_num = ARRAY_SIZE(fxos8700_odr); + + ret = regmap_read(data->regmap, FXOS8700_CTRL_REG1, &val); + if (ret) + return ret; + + val = FIELD_GET(FXOS8700_CTRL_ODR_MSK, val); + + for (i = 0; i < odr_num; i++) + if (val == fxos8700_odr[i].bits) + break; + + if (i >= odr_num) + return -EINVAL; + + *odr = fxos8700_odr[i].odr; + *uodr = fxos8700_odr[i].uodr; + + return 0; +} + +static int fxos8700_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask) +{ + int ret; + struct fxos8700_data *data = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_RAW: + ret = fxos8700_get_data(data, chan->type, chan->channel2, val); + if (ret) + return ret; + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + *val = 0; + ret = fxos8700_get_scale(data, fxos8700_to_sensor(chan->type), + val2); + return ret ? ret : IIO_VAL_INT_PLUS_MICRO; + case IIO_CHAN_INFO_SAMP_FREQ: + ret = fxos8700_get_odr(data, fxos8700_to_sensor(chan->type), + val, val2); + return ret ? ret : IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } +} + +static int fxos8700_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct fxos8700_data *data = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + return fxos8700_set_scale(data, fxos8700_to_sensor(chan->type), + val2); + case IIO_CHAN_INFO_SAMP_FREQ: + return fxos8700_set_odr(data, fxos8700_to_sensor(chan->type), + val, val2); + default: + return -EINVAL; + } +} + +static IIO_CONST_ATTR(in_accel_sampling_frequency_available, + "1.5625 6.25 12.5 50 100 200 400 800"); +static IIO_CONST_ATTR(in_magn_sampling_frequency_available, + "1.5625 6.25 12.5 50 100 200 400 800"); +static IIO_CONST_ATTR(in_accel_scale_available, "0.000244 0.000488 0.000976"); +static IIO_CONST_ATTR(in_magn_scale_available, "0.001000"); + +static struct attribute *fxos8700_attrs[] = { + &iio_const_attr_in_accel_sampling_frequency_available.dev_attr.attr, + &iio_const_attr_in_magn_sampling_frequency_available.dev_attr.attr, + &iio_const_attr_in_accel_scale_available.dev_attr.attr, + &iio_const_attr_in_magn_scale_available.dev_attr.attr, + NULL, +}; + +static const struct attribute_group fxos8700_attrs_group = { + .attrs = fxos8700_attrs, +}; + +static const struct iio_info fxos8700_info = { + .read_raw = fxos8700_read_raw, + .write_raw = fxos8700_write_raw, + .attrs = &fxos8700_attrs_group, +}; + +static int fxos8700_chip_init(struct fxos8700_data *data, bool use_spi) +{ + int ret; + unsigned int val; + struct device *dev = regmap_get_device(data->regmap); + + ret = regmap_read(data->regmap, FXOS8700_WHO_AM_I, &val); + if (ret) { + dev_err(dev, "Error reading chip id\n"); + return ret; + } + if (val != FXOS8700_DEVICE_ID && val != FXOS8700_PRE_DEVICE_ID) { + dev_err(dev, "Wrong chip id, got %x expected %x or %x\n", + val, FXOS8700_DEVICE_ID, FXOS8700_PRE_DEVICE_ID); + return -ENODEV; + } + + ret = fxos8700_set_active_mode(data, FXOS8700_ACCEL, true); + if (ret) + return ret; + + ret = fxos8700_set_active_mode(data, FXOS8700_MAGN, true); + if (ret) + return ret; + + /* + * The device must be in standby mode to change any of the other fields + * within CTRL_REG1 + */ + ret = regmap_write(data->regmap, FXOS8700_CTRL_REG1, 0x00); + if (ret) + return ret; + + /* Set max oversample ratio (OSR) and both devices active */ + ret = regmap_write(data->regmap, FXOS8700_M_CTRL_REG1, + FXOS8700_HMS_MASK | FXOS8700_OS_MASK); + if (ret) + return ret; + + /* Disable and rst min/max measurements & threshold */ + ret = regmap_write(data->regmap, FXOS8700_M_CTRL_REG2, + FXOS8700_MAXMIN_RST | FXOS8700_MAXMIN_DIS_THS | + FXOS8700_MAXMIN_DIS); + if (ret) + return ret; + + /* + * Set max full-scale range (+/-8G) for ACCEL sensor in chip + * initialization then activate the device. + */ + ret = regmap_write(data->regmap, FXOS8700_XYZ_DATA_CFG, MODE_8G); + if (ret) + return ret; + + /* Max ODR (800Hz individual or 400Hz hybrid), active mode */ + return regmap_update_bits(data->regmap, FXOS8700_CTRL_REG1, + FXOS8700_CTRL_ODR_MSK | FXOS8700_ACTIVE, + FIELD_PREP(FXOS8700_CTRL_ODR_MSK, FXOS8700_CTRL_ODR_MAX) | + FXOS8700_ACTIVE); +} + +static void fxos8700_chip_uninit(void *data) +{ + struct fxos8700_data *fxos8700_data = data; + + fxos8700_set_active_mode(fxos8700_data, FXOS8700_ACCEL, false); + fxos8700_set_active_mode(fxos8700_data, FXOS8700_MAGN, false); +} + +int fxos8700_core_probe(struct device *dev, struct regmap *regmap, + const char *name, bool use_spi) +{ + struct iio_dev *indio_dev; + struct fxos8700_data *data; + int ret; + + indio_dev = devm_iio_device_alloc(dev, sizeof(*data)); + if (!indio_dev) + return -ENOMEM; + + data = iio_priv(indio_dev); + dev_set_drvdata(dev, indio_dev); + data->regmap = regmap; + + ret = fxos8700_chip_init(data, use_spi); + if (ret) + return ret; + + ret = devm_add_action_or_reset(dev, fxos8700_chip_uninit, data); + if (ret) + return ret; + + indio_dev->channels = fxos8700_channels; + indio_dev->num_channels = ARRAY_SIZE(fxos8700_channels); + indio_dev->name = name ? name : "fxos8700"; + indio_dev->modes = INDIO_DIRECT_MODE; + indio_dev->info = &fxos8700_info; + + return devm_iio_device_register(dev, indio_dev); +} +EXPORT_SYMBOL_GPL(fxos8700_core_probe); + +MODULE_AUTHOR("Robert Jones "); +MODULE_DESCRIPTION("FXOS8700 6-Axis Acc and Mag Combo Sensor driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/imu/fxos8700_i2c.c b/drivers/iio/imu/fxos8700_i2c.c new file mode 100644 index 000000000..40a570325 --- /dev/null +++ b/drivers/iio/imu/fxos8700_i2c.c @@ -0,0 +1,70 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * FXOS8700 - NXP IMU, I2C bits + * + * 7-bit I2C slave address determined by SA1 and SA0 logic level + * inputs represented in the following table: + * SA1 | SA0 | Slave Address + * 0 | 0 | 0x1E + * 0 | 1 | 0x1D + * 1 | 0 | 0x1C + * 1 | 1 | 0x1F + */ +#include +#include +#include +#include +#include + +#include "fxos8700.h" + +static int fxos8700_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct regmap *regmap; + const char *name = NULL; + + regmap = devm_regmap_init_i2c(client, &fxos8700_regmap_config); + if (IS_ERR(regmap)) { + dev_err(&client->dev, "Failed to register i2c regmap %ld\n", PTR_ERR(regmap)); + return PTR_ERR(regmap); + } + + if (id) + name = id->name; + + return fxos8700_core_probe(&client->dev, regmap, name, false); +} + +static const struct i2c_device_id fxos8700_i2c_id[] = { + {"fxos8700", 0}, + { } +}; +MODULE_DEVICE_TABLE(i2c, fxos8700_i2c_id); + +static const struct acpi_device_id fxos8700_acpi_match[] = { + {"FXOS8700", 0}, + { } +}; +MODULE_DEVICE_TABLE(acpi, fxos8700_acpi_match); + +static const struct of_device_id fxos8700_of_match[] = { + { .compatible = "nxp,fxos8700" }, + { } +}; +MODULE_DEVICE_TABLE(of, fxos8700_of_match); + +static struct i2c_driver fxos8700_i2c_driver = { + .driver = { + .name = "fxos8700_i2c", + .acpi_match_table = ACPI_PTR(fxos8700_acpi_match), + .of_match_table = fxos8700_of_match, + }, + .probe = fxos8700_i2c_probe, + .id_table = fxos8700_i2c_id, +}; +module_i2c_driver(fxos8700_i2c_driver); + +MODULE_AUTHOR("Robert Jones "); +MODULE_DESCRIPTION("FXOS8700 I2C driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/imu/fxos8700_spi.c b/drivers/iio/imu/fxos8700_spi.c new file mode 100644 index 000000000..27e694cce --- /dev/null +++ b/drivers/iio/imu/fxos8700_spi.c @@ -0,0 +1,58 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * FXOS8700 - NXP IMU, SPI bits + */ +#include +#include +#include +#include +#include + +#include "fxos8700.h" + +static int fxos8700_spi_probe(struct spi_device *spi) +{ + struct regmap *regmap; + const struct spi_device_id *id = spi_get_device_id(spi); + + regmap = devm_regmap_init_spi(spi, &fxos8700_regmap_config); + if (IS_ERR(regmap)) { + dev_err(&spi->dev, "Failed to register spi regmap %ld\n", PTR_ERR(regmap)); + return PTR_ERR(regmap); + } + + return fxos8700_core_probe(&spi->dev, regmap, id->name, true); +} + +static const struct spi_device_id fxos8700_spi_id[] = { + {"fxos8700", 0}, + { } +}; +MODULE_DEVICE_TABLE(spi, fxos8700_spi_id); + +static const struct acpi_device_id fxos8700_acpi_match[] = { + {"FXOS8700", 0}, + { } +}; +MODULE_DEVICE_TABLE(acpi, fxos8700_acpi_match); + +static const struct of_device_id fxos8700_of_match[] = { + { .compatible = "nxp,fxos8700" }, + { } +}; +MODULE_DEVICE_TABLE(of, fxos8700_of_match); + +static struct spi_driver fxos8700_spi_driver = { + .probe = fxos8700_spi_probe, + .id_table = fxos8700_spi_id, + .driver = { + .acpi_match_table = ACPI_PTR(fxos8700_acpi_match), + .of_match_table = fxos8700_of_match, + .name = "fxos8700_spi", + }, +}; +module_spi_driver(fxos8700_spi_driver); + +MODULE_AUTHOR("Robert Jones "); +MODULE_DESCRIPTION("FXOS8700 SPI driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/imu/inv_icm42600/Kconfig b/drivers/iio/imu/inv_icm42600/Kconfig new file mode 100644 index 000000000..50cbcfcb6 --- /dev/null +++ b/drivers/iio/imu/inv_icm42600/Kconfig @@ -0,0 +1,29 @@ +# SPDX-License-Identifier: GPL-2.0-or-later + +config INV_ICM42600 + tristate + select IIO_BUFFER + +config INV_ICM42600_I2C + tristate "InvenSense ICM-426xx I2C driver" + depends on I2C + select INV_ICM42600 + select REGMAP_I2C + help + This driver supports the InvenSense ICM-426xx motion tracking + devices over I2C. + + This driver can be built as a module. The module will be called + inv-icm42600-i2c. + +config INV_ICM42600_SPI + tristate "InvenSense ICM-426xx SPI driver" + depends on SPI_MASTER + select INV_ICM42600 + select REGMAP_SPI + help + This driver supports the InvenSense ICM-426xx motion tracking + devices over SPI. + + This driver can be built as a module. The module will be called + inv-icm42600-spi. diff --git a/drivers/iio/imu/inv_icm42600/Makefile b/drivers/iio/imu/inv_icm42600/Makefile new file mode 100644 index 000000000..291714d9a --- /dev/null +++ b/drivers/iio/imu/inv_icm42600/Makefile @@ -0,0 +1,15 @@ +# SPDX-License-Identifier: GPL-2.0-or-later + +obj-$(CONFIG_INV_ICM42600) += inv-icm42600.o +inv-icm42600-y += inv_icm42600_core.o +inv-icm42600-y += inv_icm42600_gyro.o +inv-icm42600-y += inv_icm42600_accel.o +inv-icm42600-y += inv_icm42600_temp.o +inv-icm42600-y += inv_icm42600_buffer.o +inv-icm42600-y += inv_icm42600_timestamp.o + +obj-$(CONFIG_INV_ICM42600_I2C) += inv-icm42600-i2c.o +inv-icm42600-i2c-y += inv_icm42600_i2c.o + +obj-$(CONFIG_INV_ICM42600_SPI) += inv-icm42600-spi.o +inv-icm42600-spi-y += inv_icm42600_spi.o diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600.h b/drivers/iio/imu/inv_icm42600/inv_icm42600.h new file mode 100644 index 000000000..3d91469be --- /dev/null +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h @@ -0,0 +1,396 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ +/* + * Copyright (C) 2020 Invensense, Inc. + */ + +#ifndef INV_ICM42600_H_ +#define INV_ICM42600_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include "inv_icm42600_buffer.h" + +enum inv_icm42600_chip { + INV_CHIP_INVALID, + INV_CHIP_ICM42600, + INV_CHIP_ICM42602, + INV_CHIP_ICM42605, + INV_CHIP_ICM42622, + INV_CHIP_NB, +}; + +/* serial bus slew rates */ +enum inv_icm42600_slew_rate { + INV_ICM42600_SLEW_RATE_20_60NS, + INV_ICM42600_SLEW_RATE_12_36NS, + INV_ICM42600_SLEW_RATE_6_18NS, + INV_ICM42600_SLEW_RATE_4_12NS, + INV_ICM42600_SLEW_RATE_2_6NS, + INV_ICM42600_SLEW_RATE_INF_2NS, +}; + +enum inv_icm42600_sensor_mode { + INV_ICM42600_SENSOR_MODE_OFF, + INV_ICM42600_SENSOR_MODE_STANDBY, + INV_ICM42600_SENSOR_MODE_LOW_POWER, + INV_ICM42600_SENSOR_MODE_LOW_NOISE, + INV_ICM42600_SENSOR_MODE_NB, +}; + +/* gyroscope fullscale values */ +enum inv_icm42600_gyro_fs { + INV_ICM42600_GYRO_FS_2000DPS, + INV_ICM42600_GYRO_FS_1000DPS, + INV_ICM42600_GYRO_FS_500DPS, + INV_ICM42600_GYRO_FS_250DPS, + INV_ICM42600_GYRO_FS_125DPS, + INV_ICM42600_GYRO_FS_62_5DPS, + INV_ICM42600_GYRO_FS_31_25DPS, + INV_ICM42600_GYRO_FS_15_625DPS, + INV_ICM42600_GYRO_FS_NB, +}; + +/* accelerometer fullscale values */ +enum inv_icm42600_accel_fs { + INV_ICM42600_ACCEL_FS_16G, + INV_ICM42600_ACCEL_FS_8G, + INV_ICM42600_ACCEL_FS_4G, + INV_ICM42600_ACCEL_FS_2G, + INV_ICM42600_ACCEL_FS_NB, +}; + +/* ODR suffixed by LN or LP are Low-Noise or Low-Power mode only */ +enum inv_icm42600_odr { + INV_ICM42600_ODR_8KHZ_LN = 3, + INV_ICM42600_ODR_4KHZ_LN, + INV_ICM42600_ODR_2KHZ_LN, + INV_ICM42600_ODR_1KHZ_LN, + INV_ICM42600_ODR_200HZ, + INV_ICM42600_ODR_100HZ, + INV_ICM42600_ODR_50HZ, + INV_ICM42600_ODR_25HZ, + INV_ICM42600_ODR_12_5HZ, + INV_ICM42600_ODR_6_25HZ_LP, + INV_ICM42600_ODR_3_125HZ_LP, + INV_ICM42600_ODR_1_5625HZ_LP, + INV_ICM42600_ODR_500HZ, + INV_ICM42600_ODR_NB, +}; + +enum inv_icm42600_filter { + /* Low-Noise mode sensor data filter (3rd order filter by default) */ + INV_ICM42600_FILTER_BW_ODR_DIV_2, + + /* Low-Power mode sensor data filter (averaging) */ + INV_ICM42600_FILTER_AVG_1X = 1, + INV_ICM42600_FILTER_AVG_16X = 6, +}; + +struct inv_icm42600_sensor_conf { + int mode; + int fs; + int odr; + int filter; +}; +#define INV_ICM42600_SENSOR_CONF_INIT {-1, -1, -1, -1} + +struct inv_icm42600_conf { + struct inv_icm42600_sensor_conf gyro; + struct inv_icm42600_sensor_conf accel; + bool temp_en; +}; + +struct inv_icm42600_suspended { + enum inv_icm42600_sensor_mode gyro; + enum inv_icm42600_sensor_mode accel; + bool temp; +}; + +/** + * struct inv_icm42600_state - driver state variables + * @lock: lock for serializing multiple registers access. + * @chip: chip identifier. + * @name: chip name. + * @map: regmap pointer. + * @vdd_supply: VDD voltage regulator for the chip. + * @vddio_supply: I/O voltage regulator for the chip. + * @orientation: sensor chip orientation relative to main hardware. + * @conf: chip sensors configurations. + * @suspended: suspended sensors configuration. + * @indio_gyro: gyroscope IIO device. + * @indio_accel: accelerometer IIO device. + * @buffer: data transfer buffer aligned for DMA. + * @fifo: FIFO management structure. + * @timestamp: interrupt timestamps. + */ +struct inv_icm42600_state { + struct mutex lock; + enum inv_icm42600_chip chip; + const char *name; + struct regmap *map; + struct regulator *vdd_supply; + struct regulator *vddio_supply; + struct iio_mount_matrix orientation; + struct inv_icm42600_conf conf; + struct inv_icm42600_suspended suspended; + struct iio_dev *indio_gyro; + struct iio_dev *indio_accel; + uint8_t buffer[2] __aligned(IIO_DMA_MINALIGN); + struct inv_icm42600_fifo fifo; + struct { + int64_t gyro; + int64_t accel; + } timestamp; +}; + +/* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */ + +/* Bank selection register, available in all banks */ +#define INV_ICM42600_REG_BANK_SEL 0x76 +#define INV_ICM42600_BANK_SEL_MASK GENMASK(2, 0) + +/* User bank 0 (MSB 0x00) */ +#define INV_ICM42600_REG_DEVICE_CONFIG 0x0011 +#define INV_ICM42600_DEVICE_CONFIG_SOFT_RESET BIT(0) + +#define INV_ICM42600_REG_DRIVE_CONFIG 0x0013 +#define INV_ICM42600_DRIVE_CONFIG_I2C_MASK GENMASK(5, 3) +#define INV_ICM42600_DRIVE_CONFIG_I2C(_rate) \ + FIELD_PREP(INV_ICM42600_DRIVE_CONFIG_I2C_MASK, (_rate)) +#define INV_ICM42600_DRIVE_CONFIG_SPI_MASK GENMASK(2, 0) +#define INV_ICM42600_DRIVE_CONFIG_SPI(_rate) \ + FIELD_PREP(INV_ICM42600_DRIVE_CONFIG_SPI_MASK, (_rate)) + +#define INV_ICM42600_REG_INT_CONFIG 0x0014 +#define INV_ICM42600_INT_CONFIG_INT2_LATCHED BIT(5) +#define INV_ICM42600_INT_CONFIG_INT2_PUSH_PULL BIT(4) +#define INV_ICM42600_INT_CONFIG_INT2_ACTIVE_HIGH BIT(3) +#define INV_ICM42600_INT_CONFIG_INT2_ACTIVE_LOW 0x00 +#define INV_ICM42600_INT_CONFIG_INT1_LATCHED BIT(2) +#define INV_ICM42600_INT_CONFIG_INT1_PUSH_PULL BIT(1) +#define INV_ICM42600_INT_CONFIG_INT1_ACTIVE_HIGH BIT(0) +#define INV_ICM42600_INT_CONFIG_INT1_ACTIVE_LOW 0x00 + +#define INV_ICM42600_REG_FIFO_CONFIG 0x0016 +#define INV_ICM42600_FIFO_CONFIG_MASK GENMASK(7, 6) +#define INV_ICM42600_FIFO_CONFIG_BYPASS \ + FIELD_PREP(INV_ICM42600_FIFO_CONFIG_MASK, 0) +#define INV_ICM42600_FIFO_CONFIG_STREAM \ + FIELD_PREP(INV_ICM42600_FIFO_CONFIG_MASK, 1) +#define INV_ICM42600_FIFO_CONFIG_STOP_ON_FULL \ + FIELD_PREP(INV_ICM42600_FIFO_CONFIG_MASK, 2) + +/* all sensor data are 16 bits (2 registers wide) in big-endian */ +#define INV_ICM42600_REG_TEMP_DATA 0x001D +#define INV_ICM42600_REG_ACCEL_DATA_X 0x001F +#define INV_ICM42600_REG_ACCEL_DATA_Y 0x0021 +#define INV_ICM42600_REG_ACCEL_DATA_Z 0x0023 +#define INV_ICM42600_REG_GYRO_DATA_X 0x0025 +#define INV_ICM42600_REG_GYRO_DATA_Y 0x0027 +#define INV_ICM42600_REG_GYRO_DATA_Z 0x0029 +#define INV_ICM42600_DATA_INVALID -32768 + +#define INV_ICM42600_REG_INT_STATUS 0x002D +#define INV_ICM42600_INT_STATUS_UI_FSYNC BIT(6) +#define INV_ICM42600_INT_STATUS_PLL_RDY BIT(5) +#define INV_ICM42600_INT_STATUS_RESET_DONE BIT(4) +#define INV_ICM42600_INT_STATUS_DATA_RDY BIT(3) +#define INV_ICM42600_INT_STATUS_FIFO_THS BIT(2) +#define INV_ICM42600_INT_STATUS_FIFO_FULL BIT(1) +#define INV_ICM42600_INT_STATUS_AGC_RDY BIT(0) + +/* + * FIFO access registers + * FIFO count is 16 bits (2 registers) big-endian + * FIFO data is a continuous read register to read FIFO content + */ +#define INV_ICM42600_REG_FIFO_COUNT 0x002E +#define INV_ICM42600_REG_FIFO_DATA 0x0030 + +#define INV_ICM42600_REG_SIGNAL_PATH_RESET 0x004B +#define INV_ICM42600_SIGNAL_PATH_RESET_DMP_INIT_EN BIT(6) +#define INV_ICM42600_SIGNAL_PATH_RESET_DMP_MEM_RESET BIT(5) +#define INV_ICM42600_SIGNAL_PATH_RESET_RESET BIT(3) +#define INV_ICM42600_SIGNAL_PATH_RESET_TMST_STROBE BIT(2) +#define INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH BIT(1) + +/* default configuration: all data big-endian and fifo count in bytes */ +#define INV_ICM42600_REG_INTF_CONFIG0 0x004C +#define INV_ICM42600_INTF_CONFIG0_FIFO_HOLD_LAST_DATA BIT(7) +#define INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_REC BIT(6) +#define INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_ENDIAN BIT(5) +#define INV_ICM42600_INTF_CONFIG0_SENSOR_DATA_ENDIAN BIT(4) +#define INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_MASK GENMASK(1, 0) +#define INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_SPI_DIS \ + FIELD_PREP(INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_MASK, 2) +#define INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_I2C_DIS \ + FIELD_PREP(INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_MASK, 3) + +#define INV_ICM42600_REG_INTF_CONFIG1 0x004D +#define INV_ICM42600_INTF_CONFIG1_ACCEL_LP_CLK_RC BIT(3) + +#define INV_ICM42600_REG_PWR_MGMT0 0x004E +#define INV_ICM42600_PWR_MGMT0_TEMP_DIS BIT(5) +#define INV_ICM42600_PWR_MGMT0_IDLE BIT(4) +#define INV_ICM42600_PWR_MGMT0_GYRO(_mode) \ + FIELD_PREP(GENMASK(3, 2), (_mode)) +#define INV_ICM42600_PWR_MGMT0_ACCEL(_mode) \ + FIELD_PREP(GENMASK(1, 0), (_mode)) + +#define INV_ICM42600_REG_GYRO_CONFIG0 0x004F +#define INV_ICM42600_GYRO_CONFIG0_FS(_fs) \ + FIELD_PREP(GENMASK(7, 5), (_fs)) +#define INV_ICM42600_GYRO_CONFIG0_ODR(_odr) \ + FIELD_PREP(GENMASK(3, 0), (_odr)) + +#define INV_ICM42600_REG_ACCEL_CONFIG0 0x0050 +#define INV_ICM42600_ACCEL_CONFIG0_FS(_fs) \ + FIELD_PREP(GENMASK(7, 5), (_fs)) +#define INV_ICM42600_ACCEL_CONFIG0_ODR(_odr) \ + FIELD_PREP(GENMASK(3, 0), (_odr)) + +#define INV_ICM42600_REG_GYRO_ACCEL_CONFIG0 0x0052 +#define INV_ICM42600_GYRO_ACCEL_CONFIG0_ACCEL_FILT(_f) \ + FIELD_PREP(GENMASK(7, 4), (_f)) +#define INV_ICM42600_GYRO_ACCEL_CONFIG0_GYRO_FILT(_f) \ + FIELD_PREP(GENMASK(3, 0), (_f)) + +#define INV_ICM42600_REG_TMST_CONFIG 0x0054 +#define INV_ICM42600_TMST_CONFIG_MASK GENMASK(4, 0) +#define INV_ICM42600_TMST_CONFIG_TMST_TO_REGS_EN BIT(4) +#define INV_ICM42600_TMST_CONFIG_TMST_RES_16US BIT(3) +#define INV_ICM42600_TMST_CONFIG_TMST_DELTA_EN BIT(2) +#define INV_ICM42600_TMST_CONFIG_TMST_FSYNC_EN BIT(1) +#define INV_ICM42600_TMST_CONFIG_TMST_EN BIT(0) + +#define INV_ICM42600_REG_FIFO_CONFIG1 0x005F +#define INV_ICM42600_FIFO_CONFIG1_RESUME_PARTIAL_RD BIT(6) +#define INV_ICM42600_FIFO_CONFIG1_WM_GT_TH BIT(5) +#define INV_ICM42600_FIFO_CONFIG1_TMST_FSYNC_EN BIT(3) +#define INV_ICM42600_FIFO_CONFIG1_TEMP_EN BIT(2) +#define INV_ICM42600_FIFO_CONFIG1_GYRO_EN BIT(1) +#define INV_ICM42600_FIFO_CONFIG1_ACCEL_EN BIT(0) + +/* FIFO watermark is 16 bits (2 registers wide) in little-endian */ +#define INV_ICM42600_REG_FIFO_WATERMARK 0x0060 +#define INV_ICM42600_FIFO_WATERMARK_VAL(_wm) \ + cpu_to_le16((_wm) & GENMASK(11, 0)) +/* FIFO is 2048 bytes, let 12 samples for reading latency */ +#define INV_ICM42600_FIFO_WATERMARK_MAX (2048 - 12 * 16) + +#define INV_ICM42600_REG_INT_CONFIG1 0x0064 +#define INV_ICM42600_INT_CONFIG1_TPULSE_DURATION BIT(6) +#define INV_ICM42600_INT_CONFIG1_TDEASSERT_DISABLE BIT(5) +#define INV_ICM42600_INT_CONFIG1_ASYNC_RESET BIT(4) + +#define INV_ICM42600_REG_INT_SOURCE0 0x0065 +#define INV_ICM42600_INT_SOURCE0_UI_FSYNC_INT1_EN BIT(6) +#define INV_ICM42600_INT_SOURCE0_PLL_RDY_INT1_EN BIT(5) +#define INV_ICM42600_INT_SOURCE0_RESET_DONE_INT1_EN BIT(4) +#define INV_ICM42600_INT_SOURCE0_UI_DRDY_INT1_EN BIT(3) +#define INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN BIT(2) +#define INV_ICM42600_INT_SOURCE0_FIFO_FULL_INT1_EN BIT(1) +#define INV_ICM42600_INT_SOURCE0_UI_AGC_RDY_INT1_EN BIT(0) + +#define INV_ICM42600_REG_WHOAMI 0x0075 +#define INV_ICM42600_WHOAMI_ICM42600 0x40 +#define INV_ICM42600_WHOAMI_ICM42602 0x41 +#define INV_ICM42600_WHOAMI_ICM42605 0x42 +#define INV_ICM42600_WHOAMI_ICM42622 0x46 + +/* User bank 1 (MSB 0x10) */ +#define INV_ICM42600_REG_SENSOR_CONFIG0 0x1003 +#define INV_ICM42600_SENSOR_CONFIG0_ZG_DISABLE BIT(5) +#define INV_ICM42600_SENSOR_CONFIG0_YG_DISABLE BIT(4) +#define INV_ICM42600_SENSOR_CONFIG0_XG_DISABLE BIT(3) +#define INV_ICM42600_SENSOR_CONFIG0_ZA_DISABLE BIT(2) +#define INV_ICM42600_SENSOR_CONFIG0_YA_DISABLE BIT(1) +#define INV_ICM42600_SENSOR_CONFIG0_XA_DISABLE BIT(0) + +/* Timestamp value is 20 bits (3 registers) in little-endian */ +#define INV_ICM42600_REG_TMSTVAL 0x1062 +#define INV_ICM42600_TMSTVAL_MASK GENMASK(19, 0) + +#define INV_ICM42600_REG_INTF_CONFIG4 0x107A +#define INV_ICM42600_INTF_CONFIG4_I3C_BUS_ONLY BIT(6) +#define INV_ICM42600_INTF_CONFIG4_SPI_AP_4WIRE BIT(1) + +#define INV_ICM42600_REG_INTF_CONFIG6 0x107C +#define INV_ICM42600_INTF_CONFIG6_MASK GENMASK(4, 0) +#define INV_ICM42600_INTF_CONFIG6_I3C_EN BIT(4) +#define INV_ICM42600_INTF_CONFIG6_I3C_IBI_BYTE_EN BIT(3) +#define INV_ICM42600_INTF_CONFIG6_I3C_IBI_EN BIT(2) +#define INV_ICM42600_INTF_CONFIG6_I3C_DDR_EN BIT(1) +#define INV_ICM42600_INTF_CONFIG6_I3C_SDR_EN BIT(0) + +/* User bank 4 (MSB 0x40) */ +#define INV_ICM42600_REG_INT_SOURCE8 0x404F +#define INV_ICM42600_INT_SOURCE8_FSYNC_IBI_EN BIT(5) +#define INV_ICM42600_INT_SOURCE8_PLL_RDY_IBI_EN BIT(4) +#define INV_ICM42600_INT_SOURCE8_UI_DRDY_IBI_EN BIT(3) +#define INV_ICM42600_INT_SOURCE8_FIFO_THS_IBI_EN BIT(2) +#define INV_ICM42600_INT_SOURCE8_FIFO_FULL_IBI_EN BIT(1) +#define INV_ICM42600_INT_SOURCE8_AGC_RDY_IBI_EN BIT(0) + +#define INV_ICM42600_REG_OFFSET_USER0 0x4077 +#define INV_ICM42600_REG_OFFSET_USER1 0x4078 +#define INV_ICM42600_REG_OFFSET_USER2 0x4079 +#define INV_ICM42600_REG_OFFSET_USER3 0x407A +#define INV_ICM42600_REG_OFFSET_USER4 0x407B +#define INV_ICM42600_REG_OFFSET_USER5 0x407C +#define INV_ICM42600_REG_OFFSET_USER6 0x407D +#define INV_ICM42600_REG_OFFSET_USER7 0x407E +#define INV_ICM42600_REG_OFFSET_USER8 0x407F + +/* Sleep times required by the driver */ +#define INV_ICM42600_POWER_UP_TIME_MS 100 +#define INV_ICM42600_RESET_TIME_MS 1 +#define INV_ICM42600_ACCEL_STARTUP_TIME_MS 20 +#define INV_ICM42600_GYRO_STARTUP_TIME_MS 60 +#define INV_ICM42600_GYRO_STOP_TIME_MS 150 +#define INV_ICM42600_TEMP_STARTUP_TIME_MS 14 +#define INV_ICM42600_SUSPEND_DELAY_MS 2000 + +typedef int (*inv_icm42600_bus_setup)(struct inv_icm42600_state *); + +extern const struct regmap_config inv_icm42600_regmap_config; +extern const struct dev_pm_ops inv_icm42600_pm_ops; + +const struct iio_mount_matrix * +inv_icm42600_get_mount_matrix(const struct iio_dev *indio_dev, + const struct iio_chan_spec *chan); + +uint32_t inv_icm42600_odr_to_period(enum inv_icm42600_odr odr); + +int inv_icm42600_set_accel_conf(struct inv_icm42600_state *st, + struct inv_icm42600_sensor_conf *conf, + unsigned int *sleep_ms); + +int inv_icm42600_set_gyro_conf(struct inv_icm42600_state *st, + struct inv_icm42600_sensor_conf *conf, + unsigned int *sleep_ms); + +int inv_icm42600_set_temp_conf(struct inv_icm42600_state *st, bool enable, + unsigned int *sleep_ms); + +int inv_icm42600_debugfs_reg(struct iio_dev *indio_dev, unsigned int reg, + unsigned int writeval, unsigned int *readval); + +int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq, + inv_icm42600_bus_setup bus_setup); + +struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st); + +int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev); + +struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st); + +int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev); + +#endif diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c new file mode 100644 index 000000000..c3f433ad3 --- /dev/null +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c @@ -0,0 +1,784 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * Copyright (C) 2020 Invensense, Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "inv_icm42600.h" +#include "inv_icm42600_temp.h" +#include "inv_icm42600_buffer.h" +#include "inv_icm42600_timestamp.h" + +#define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \ + { \ + .type = IIO_ACCEL, \ + .modified = 1, \ + .channel2 = _modifier, \ + .info_mask_separate = \ + BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_CALIBBIAS), \ + .info_mask_shared_by_type = \ + BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_shared_by_type_available = \ + BIT(IIO_CHAN_INFO_SCALE) | \ + BIT(IIO_CHAN_INFO_CALIBBIAS), \ + .info_mask_shared_by_all = \ + BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .info_mask_shared_by_all_available = \ + BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .scan_index = _index, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_BE, \ + }, \ + .ext_info = _ext_info, \ + } + +enum inv_icm42600_accel_scan { + INV_ICM42600_ACCEL_SCAN_X, + INV_ICM42600_ACCEL_SCAN_Y, + INV_ICM42600_ACCEL_SCAN_Z, + INV_ICM42600_ACCEL_SCAN_TEMP, + INV_ICM42600_ACCEL_SCAN_TIMESTAMP, +}; + +static const struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = { + IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm42600_get_mount_matrix), + {}, +}; + +static const struct iio_chan_spec inv_icm42600_accel_channels[] = { + INV_ICM42600_ACCEL_CHAN(IIO_MOD_X, INV_ICM42600_ACCEL_SCAN_X, + inv_icm42600_accel_ext_infos), + INV_ICM42600_ACCEL_CHAN(IIO_MOD_Y, INV_ICM42600_ACCEL_SCAN_Y, + inv_icm42600_accel_ext_infos), + INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z, + inv_icm42600_accel_ext_infos), + INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP), + IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_ACCEL_SCAN_TIMESTAMP), +}; + +/* + * IIO buffer data: size must be a power of 2 and timestamp aligned + * 16 bytes: 6 bytes acceleration, 2 bytes temperature, 8 bytes timestamp + */ +struct inv_icm42600_accel_buffer { + struct inv_icm42600_fifo_sensor_data accel; + int16_t temp; + int64_t timestamp __aligned(8); +}; + +#define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \ + (BIT(INV_ICM42600_ACCEL_SCAN_X) | \ + BIT(INV_ICM42600_ACCEL_SCAN_Y) | \ + BIT(INV_ICM42600_ACCEL_SCAN_Z)) + +#define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_ACCEL_SCAN_TEMP) + +static const unsigned long inv_icm42600_accel_scan_masks[] = { + /* 3-axis accel + temperature */ + INV_ICM42600_SCAN_MASK_ACCEL_3AXIS | INV_ICM42600_SCAN_MASK_TEMP, + 0, +}; + +/* enable accelerometer sensor and FIFO write */ +static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev, + const unsigned long *scan_mask) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm42600_timestamp *ts = iio_priv(indio_dev); + struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; + unsigned int fifo_en = 0; + unsigned int sleep_temp = 0; + unsigned int sleep_accel = 0; + unsigned int sleep; + int ret; + + mutex_lock(&st->lock); + + if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) { + /* enable temp sensor */ + ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp); + if (ret) + goto out_unlock; + fifo_en |= INV_ICM42600_SENSOR_TEMP; + } + + if (*scan_mask & INV_ICM42600_SCAN_MASK_ACCEL_3AXIS) { + /* enable accel sensor */ + conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE; + ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_accel); + if (ret) + goto out_unlock; + fifo_en |= INV_ICM42600_SENSOR_ACCEL; + } + + /* update data FIFO write */ + inv_icm42600_timestamp_apply_odr(ts, 0, 0, 0); + ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en); + if (ret) + goto out_unlock; + + ret = inv_icm42600_buffer_update_watermark(st); + +out_unlock: + mutex_unlock(&st->lock); + /* sleep maximum required time */ + if (sleep_accel > sleep_temp) + sleep = sleep_accel; + else + sleep = sleep_temp; + if (sleep) + msleep(sleep); + return ret; +} + +static int inv_icm42600_accel_read_sensor(struct inv_icm42600_state *st, + struct iio_chan_spec const *chan, + int16_t *val) +{ + struct device *dev = regmap_get_device(st->map); + struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; + unsigned int reg; + __be16 *data; + int ret; + + if (chan->type != IIO_ACCEL) + return -EINVAL; + + switch (chan->channel2) { + case IIO_MOD_X: + reg = INV_ICM42600_REG_ACCEL_DATA_X; + break; + case IIO_MOD_Y: + reg = INV_ICM42600_REG_ACCEL_DATA_Y; + break; + case IIO_MOD_Z: + reg = INV_ICM42600_REG_ACCEL_DATA_Z; + break; + default: + return -EINVAL; + } + + pm_runtime_get_sync(dev); + mutex_lock(&st->lock); + + /* enable accel sensor */ + conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE; + ret = inv_icm42600_set_accel_conf(st, &conf, NULL); + if (ret) + goto exit; + + /* read accel register data */ + data = (__be16 *)&st->buffer[0]; + ret = regmap_bulk_read(st->map, reg, data, sizeof(*data)); + if (ret) + goto exit; + + *val = (int16_t)be16_to_cpup(data); + if (*val == INV_ICM42600_DATA_INVALID) + ret = -EINVAL; +exit: + mutex_unlock(&st->lock); + pm_runtime_mark_last_busy(dev); + pm_runtime_put_autosuspend(dev); + return ret; +} + +/* IIO format int + nano */ +static const int inv_icm42600_accel_scale[] = { + /* +/- 16G => 0.004788403 m/s-2 */ + [2 * INV_ICM42600_ACCEL_FS_16G] = 0, + [2 * INV_ICM42600_ACCEL_FS_16G + 1] = 4788403, + /* +/- 8G => 0.002394202 m/s-2 */ + [2 * INV_ICM42600_ACCEL_FS_8G] = 0, + [2 * INV_ICM42600_ACCEL_FS_8G + 1] = 2394202, + /* +/- 4G => 0.001197101 m/s-2 */ + [2 * INV_ICM42600_ACCEL_FS_4G] = 0, + [2 * INV_ICM42600_ACCEL_FS_4G + 1] = 1197101, + /* +/- 2G => 0.000598550 m/s-2 */ + [2 * INV_ICM42600_ACCEL_FS_2G] = 0, + [2 * INV_ICM42600_ACCEL_FS_2G + 1] = 598550, +}; + +static int inv_icm42600_accel_read_scale(struct inv_icm42600_state *st, + int *val, int *val2) +{ + unsigned int idx; + + idx = st->conf.accel.fs; + + *val = inv_icm42600_accel_scale[2 * idx]; + *val2 = inv_icm42600_accel_scale[2 * idx + 1]; + return IIO_VAL_INT_PLUS_NANO; +} + +static int inv_icm42600_accel_write_scale(struct inv_icm42600_state *st, + int val, int val2) +{ + struct device *dev = regmap_get_device(st->map); + unsigned int idx; + struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; + int ret; + + for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_scale); idx += 2) { + if (val == inv_icm42600_accel_scale[idx] && + val2 == inv_icm42600_accel_scale[idx + 1]) + break; + } + if (idx >= ARRAY_SIZE(inv_icm42600_accel_scale)) + return -EINVAL; + + conf.fs = idx / 2; + + pm_runtime_get_sync(dev); + mutex_lock(&st->lock); + + ret = inv_icm42600_set_accel_conf(st, &conf, NULL); + + mutex_unlock(&st->lock); + pm_runtime_mark_last_busy(dev); + pm_runtime_put_autosuspend(dev); + + return ret; +} + +/* IIO format int + micro */ +static const int inv_icm42600_accel_odr[] = { + /* 12.5Hz */ + 12, 500000, + /* 25Hz */ + 25, 0, + /* 50Hz */ + 50, 0, + /* 100Hz */ + 100, 0, + /* 200Hz */ + 200, 0, + /* 1kHz */ + 1000, 0, + /* 2kHz */ + 2000, 0, + /* 4kHz */ + 4000, 0, +}; + +static const int inv_icm42600_accel_odr_conv[] = { + INV_ICM42600_ODR_12_5HZ, + INV_ICM42600_ODR_25HZ, + INV_ICM42600_ODR_50HZ, + INV_ICM42600_ODR_100HZ, + INV_ICM42600_ODR_200HZ, + INV_ICM42600_ODR_1KHZ_LN, + INV_ICM42600_ODR_2KHZ_LN, + INV_ICM42600_ODR_4KHZ_LN, +}; + +static int inv_icm42600_accel_read_odr(struct inv_icm42600_state *st, + int *val, int *val2) +{ + unsigned int odr; + unsigned int i; + + odr = st->conf.accel.odr; + + for (i = 0; i < ARRAY_SIZE(inv_icm42600_accel_odr_conv); ++i) { + if (inv_icm42600_accel_odr_conv[i] == odr) + break; + } + if (i >= ARRAY_SIZE(inv_icm42600_accel_odr_conv)) + return -EINVAL; + + *val = inv_icm42600_accel_odr[2 * i]; + *val2 = inv_icm42600_accel_odr[2 * i + 1]; + + return IIO_VAL_INT_PLUS_MICRO; +} + +static int inv_icm42600_accel_write_odr(struct iio_dev *indio_dev, + int val, int val2) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm42600_timestamp *ts = iio_priv(indio_dev); + struct device *dev = regmap_get_device(st->map); + unsigned int idx; + struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; + int ret; + + for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_odr); idx += 2) { + if (val == inv_icm42600_accel_odr[idx] && + val2 == inv_icm42600_accel_odr[idx + 1]) + break; + } + if (idx >= ARRAY_SIZE(inv_icm42600_accel_odr)) + return -EINVAL; + + conf.odr = inv_icm42600_accel_odr_conv[idx / 2]; + + pm_runtime_get_sync(dev); + mutex_lock(&st->lock); + + ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr), + iio_buffer_enabled(indio_dev)); + if (ret) + goto out_unlock; + + ret = inv_icm42600_set_accel_conf(st, &conf, NULL); + if (ret) + goto out_unlock; + inv_icm42600_buffer_update_fifo_period(st); + inv_icm42600_buffer_update_watermark(st); + +out_unlock: + mutex_unlock(&st->lock); + pm_runtime_mark_last_busy(dev); + pm_runtime_put_autosuspend(dev); + + return ret; +} + +/* + * Calibration bias values, IIO range format int + micro. + * Value is limited to +/-1g coded on 12 bits signed. Step is 0.5mg. + */ +static int inv_icm42600_accel_calibbias[] = { + -10, 42010, /* min: -10.042010 m/s² */ + 0, 4903, /* step: 0.004903 m/s² */ + 10, 37106, /* max: 10.037106 m/s² */ +}; + +static int inv_icm42600_accel_read_offset(struct inv_icm42600_state *st, + struct iio_chan_spec const *chan, + int *val, int *val2) +{ + struct device *dev = regmap_get_device(st->map); + int64_t val64; + int32_t bias; + unsigned int reg; + int16_t offset; + uint8_t data[2]; + int ret; + + if (chan->type != IIO_ACCEL) + return -EINVAL; + + switch (chan->channel2) { + case IIO_MOD_X: + reg = INV_ICM42600_REG_OFFSET_USER4; + break; + case IIO_MOD_Y: + reg = INV_ICM42600_REG_OFFSET_USER6; + break; + case IIO_MOD_Z: + reg = INV_ICM42600_REG_OFFSET_USER7; + break; + default: + return -EINVAL; + } + + pm_runtime_get_sync(dev); + mutex_lock(&st->lock); + + ret = regmap_bulk_read(st->map, reg, st->buffer, sizeof(data)); + memcpy(data, st->buffer, sizeof(data)); + + mutex_unlock(&st->lock); + pm_runtime_mark_last_busy(dev); + pm_runtime_put_autosuspend(dev); + if (ret) + return ret; + + /* 12 bits signed value */ + switch (chan->channel2) { + case IIO_MOD_X: + offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11); + break; + case IIO_MOD_Y: + offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11); + break; + case IIO_MOD_Z: + offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11); + break; + default: + return -EINVAL; + } + + /* + * convert raw offset to g then to m/s² + * 12 bits signed raw step 0.5mg to g: 5 / 10000 + * g to m/s²: 9.806650 + * result in micro (1000000) + * (offset * 5 * 9.806650 * 1000000) / 10000 + */ + val64 = (int64_t)offset * 5LL * 9806650LL; + /* for rounding, add + or - divisor (10000) divided by 2 */ + if (val64 >= 0) + val64 += 10000LL / 2LL; + else + val64 -= 10000LL / 2LL; + bias = div_s64(val64, 10000L); + *val = bias / 1000000L; + *val2 = bias % 1000000L; + + return IIO_VAL_INT_PLUS_MICRO; +} + +static int inv_icm42600_accel_write_offset(struct inv_icm42600_state *st, + struct iio_chan_spec const *chan, + int val, int val2) +{ + struct device *dev = regmap_get_device(st->map); + int64_t val64; + int32_t min, max; + unsigned int reg, regval; + int16_t offset; + int ret; + + if (chan->type != IIO_ACCEL) + return -EINVAL; + + switch (chan->channel2) { + case IIO_MOD_X: + reg = INV_ICM42600_REG_OFFSET_USER4; + break; + case IIO_MOD_Y: + reg = INV_ICM42600_REG_OFFSET_USER6; + break; + case IIO_MOD_Z: + reg = INV_ICM42600_REG_OFFSET_USER7; + break; + default: + return -EINVAL; + } + + /* inv_icm42600_accel_calibbias: min - step - max in micro */ + min = inv_icm42600_accel_calibbias[0] * 1000000L + + inv_icm42600_accel_calibbias[1]; + max = inv_icm42600_accel_calibbias[4] * 1000000L + + inv_icm42600_accel_calibbias[5]; + val64 = (int64_t)val * 1000000LL + (int64_t)val2; + if (val64 < min || val64 > max) + return -EINVAL; + + /* + * convert m/s² to g then to raw value + * m/s² to g: 1 / 9.806650 + * g to raw 12 bits signed, step 0.5mg: 10000 / 5 + * val in micro (1000000) + * val * 10000 / (9.806650 * 1000000 * 5) + */ + val64 = val64 * 10000LL; + /* for rounding, add + or - divisor (9806650 * 5) divided by 2 */ + if (val64 >= 0) + val64 += 9806650 * 5 / 2; + else + val64 -= 9806650 * 5 / 2; + offset = div_s64(val64, 9806650 * 5); + + /* clamp value limited to 12 bits signed */ + if (offset < -2048) + offset = -2048; + else if (offset > 2047) + offset = 2047; + + pm_runtime_get_sync(dev); + mutex_lock(&st->lock); + + switch (chan->channel2) { + case IIO_MOD_X: + /* OFFSET_USER4 register is shared */ + ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4, + ®val); + if (ret) + goto out_unlock; + st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F); + st->buffer[1] = offset & 0xFF; + break; + case IIO_MOD_Y: + /* OFFSET_USER7 register is shared */ + ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7, + ®val); + if (ret) + goto out_unlock; + st->buffer[0] = offset & 0xFF; + st->buffer[1] = ((offset & 0xF00) >> 8) | (regval & 0xF0); + break; + case IIO_MOD_Z: + /* OFFSET_USER7 register is shared */ + ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7, + ®val); + if (ret) + goto out_unlock; + st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F); + st->buffer[1] = offset & 0xFF; + break; + default: + ret = -EINVAL; + goto out_unlock; + } + + ret = regmap_bulk_write(st->map, reg, st->buffer, 2); + +out_unlock: + mutex_unlock(&st->lock); + pm_runtime_mark_last_busy(dev); + pm_runtime_put_autosuspend(dev); + return ret; +} + +static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + int16_t data; + int ret; + + switch (chan->type) { + case IIO_ACCEL: + break; + case IIO_TEMP: + return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask); + default: + return -EINVAL; + } + + switch (mask) { + case IIO_CHAN_INFO_RAW: + ret = iio_device_claim_direct_mode(indio_dev); + if (ret) + return ret; + ret = inv_icm42600_accel_read_sensor(st, chan, &data); + iio_device_release_direct_mode(indio_dev); + if (ret) + return ret; + *val = data; + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + return inv_icm42600_accel_read_scale(st, val, val2); + case IIO_CHAN_INFO_SAMP_FREQ: + return inv_icm42600_accel_read_odr(st, val, val2); + case IIO_CHAN_INFO_CALIBBIAS: + return inv_icm42600_accel_read_offset(st, chan, val, val2); + default: + return -EINVAL; + } +} + +static int inv_icm42600_accel_read_avail(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + const int **vals, + int *type, int *length, long mask) +{ + if (chan->type != IIO_ACCEL) + return -EINVAL; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + *vals = inv_icm42600_accel_scale; + *type = IIO_VAL_INT_PLUS_NANO; + *length = ARRAY_SIZE(inv_icm42600_accel_scale); + return IIO_AVAIL_LIST; + case IIO_CHAN_INFO_SAMP_FREQ: + *vals = inv_icm42600_accel_odr; + *type = IIO_VAL_INT_PLUS_MICRO; + *length = ARRAY_SIZE(inv_icm42600_accel_odr); + return IIO_AVAIL_LIST; + case IIO_CHAN_INFO_CALIBBIAS: + *vals = inv_icm42600_accel_calibbias; + *type = IIO_VAL_INT_PLUS_MICRO; + return IIO_AVAIL_RANGE; + default: + return -EINVAL; + } +} + +static int inv_icm42600_accel_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + int ret; + + if (chan->type != IIO_ACCEL) + return -EINVAL; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + ret = iio_device_claim_direct_mode(indio_dev); + if (ret) + return ret; + ret = inv_icm42600_accel_write_scale(st, val, val2); + iio_device_release_direct_mode(indio_dev); + return ret; + case IIO_CHAN_INFO_SAMP_FREQ: + return inv_icm42600_accel_write_odr(indio_dev, val, val2); + case IIO_CHAN_INFO_CALIBBIAS: + ret = iio_device_claim_direct_mode(indio_dev); + if (ret) + return ret; + ret = inv_icm42600_accel_write_offset(st, chan, val, val2); + iio_device_release_direct_mode(indio_dev); + return ret; + default: + return -EINVAL; + } +} + +static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + long mask) +{ + if (chan->type != IIO_ACCEL) + return -EINVAL; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + return IIO_VAL_INT_PLUS_NANO; + case IIO_CHAN_INFO_SAMP_FREQ: + return IIO_VAL_INT_PLUS_MICRO; + case IIO_CHAN_INFO_CALIBBIAS: + return IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } +} + +static int inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev, + unsigned int val) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + int ret; + + mutex_lock(&st->lock); + + st->fifo.watermark.accel = val; + ret = inv_icm42600_buffer_update_watermark(st); + + mutex_unlock(&st->lock); + + return ret; +} + +static int inv_icm42600_accel_hwfifo_flush(struct iio_dev *indio_dev, + unsigned int count) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + int ret; + + if (count == 0) + return 0; + + mutex_lock(&st->lock); + + ret = inv_icm42600_buffer_hwfifo_flush(st, count); + if (!ret) + ret = st->fifo.nb.accel; + + mutex_unlock(&st->lock); + + return ret; +} + +static const struct iio_info inv_icm42600_accel_info = { + .read_raw = inv_icm42600_accel_read_raw, + .read_avail = inv_icm42600_accel_read_avail, + .write_raw = inv_icm42600_accel_write_raw, + .write_raw_get_fmt = inv_icm42600_accel_write_raw_get_fmt, + .debugfs_reg_access = inv_icm42600_debugfs_reg, + .update_scan_mode = inv_icm42600_accel_update_scan_mode, + .hwfifo_set_watermark = inv_icm42600_accel_hwfifo_set_watermark, + .hwfifo_flush_to_buffer = inv_icm42600_accel_hwfifo_flush, +}; + +struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st) +{ + struct device *dev = regmap_get_device(st->map); + const char *name; + struct inv_icm42600_timestamp *ts; + struct iio_dev *indio_dev; + int ret; + + name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name); + if (!name) + return ERR_PTR(-ENOMEM); + + indio_dev = devm_iio_device_alloc(dev, sizeof(*ts)); + if (!indio_dev) + return ERR_PTR(-ENOMEM); + + ts = iio_priv(indio_dev); + inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.accel.odr)); + + iio_device_set_drvdata(indio_dev, st); + indio_dev->name = name; + indio_dev->info = &inv_icm42600_accel_info; + indio_dev->modes = INDIO_DIRECT_MODE; + indio_dev->channels = inv_icm42600_accel_channels; + indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels); + indio_dev->available_scan_masks = inv_icm42600_accel_scan_masks; + + ret = devm_iio_kfifo_buffer_setup(dev, indio_dev, + &inv_icm42600_buffer_ops); + if (ret) + return ERR_PTR(ret); + + ret = devm_iio_device_register(dev, indio_dev); + if (ret) + return ERR_PTR(ret); + + return indio_dev; +} + +int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm42600_timestamp *ts = iio_priv(indio_dev); + ssize_t i, size; + unsigned int no; + const void *accel, *gyro, *timestamp; + const int8_t *temp; + unsigned int odr; + int64_t ts_val; + struct inv_icm42600_accel_buffer buffer; + + /* parse all fifo packets */ + for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) { + size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i], + &accel, &gyro, &temp, ×tamp, &odr); + /* quit if error or FIFO is empty */ + if (size <= 0) + return size; + + /* skip packet if no accel data or data is invalid */ + if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel)) + continue; + + /* update odr */ + if (odr & INV_ICM42600_SENSOR_ACCEL) + inv_icm42600_timestamp_apply_odr(ts, st->fifo.period, + st->fifo.nb.total, no); + + /* buffer is copied to userspace, zeroing it to avoid any data leak */ + memset(&buffer, 0, sizeof(buffer)); + memcpy(&buffer.accel, accel, sizeof(buffer.accel)); + /* convert 8 bits FIFO temperature in high resolution format */ + buffer.temp = temp ? (*temp * 64) : 0; + ts_val = inv_icm42600_timestamp_pop(ts); + iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val); + } + + return 0; +} diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c new file mode 100644 index 000000000..32d7f8364 --- /dev/null +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c @@ -0,0 +1,601 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * Copyright (C) 2020 Invensense, Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "inv_icm42600.h" +#include "inv_icm42600_timestamp.h" +#include "inv_icm42600_buffer.h" + +/* FIFO header: 1 byte */ +#define INV_ICM42600_FIFO_HEADER_MSG BIT(7) +#define INV_ICM42600_FIFO_HEADER_ACCEL BIT(6) +#define INV_ICM42600_FIFO_HEADER_GYRO BIT(5) +#define INV_ICM42600_FIFO_HEADER_TMST_FSYNC GENMASK(3, 2) +#define INV_ICM42600_FIFO_HEADER_ODR_ACCEL BIT(1) +#define INV_ICM42600_FIFO_HEADER_ODR_GYRO BIT(0) + +struct inv_icm42600_fifo_1sensor_packet { + uint8_t header; + struct inv_icm42600_fifo_sensor_data data; + int8_t temp; +} __packed; +#define INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE 8 + +struct inv_icm42600_fifo_2sensors_packet { + uint8_t header; + struct inv_icm42600_fifo_sensor_data accel; + struct inv_icm42600_fifo_sensor_data gyro; + int8_t temp; + __be16 timestamp; +} __packed; +#define INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE 16 + +ssize_t inv_icm42600_fifo_decode_packet(const void *packet, const void **accel, + const void **gyro, const int8_t **temp, + const void **timestamp, unsigned int *odr) +{ + const struct inv_icm42600_fifo_1sensor_packet *pack1 = packet; + const struct inv_icm42600_fifo_2sensors_packet *pack2 = packet; + uint8_t header = *((const uint8_t *)packet); + + /* FIFO empty */ + if (header & INV_ICM42600_FIFO_HEADER_MSG) { + *accel = NULL; + *gyro = NULL; + *temp = NULL; + *timestamp = NULL; + *odr = 0; + return 0; + } + + /* handle odr flags */ + *odr = 0; + if (header & INV_ICM42600_FIFO_HEADER_ODR_GYRO) + *odr |= INV_ICM42600_SENSOR_GYRO; + if (header & INV_ICM42600_FIFO_HEADER_ODR_ACCEL) + *odr |= INV_ICM42600_SENSOR_ACCEL; + + /* accel + gyro */ + if ((header & INV_ICM42600_FIFO_HEADER_ACCEL) && + (header & INV_ICM42600_FIFO_HEADER_GYRO)) { + *accel = &pack2->accel; + *gyro = &pack2->gyro; + *temp = &pack2->temp; + *timestamp = &pack2->timestamp; + return INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE; + } + + /* accel only */ + if (header & INV_ICM42600_FIFO_HEADER_ACCEL) { + *accel = &pack1->data; + *gyro = NULL; + *temp = &pack1->temp; + *timestamp = NULL; + return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE; + } + + /* gyro only */ + if (header & INV_ICM42600_FIFO_HEADER_GYRO) { + *accel = NULL; + *gyro = &pack1->data; + *temp = &pack1->temp; + *timestamp = NULL; + return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE; + } + + /* invalid packet if here */ + return -EINVAL; +} + +void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st) +{ + uint32_t period_gyro, period_accel, period; + + if (st->fifo.en & INV_ICM42600_SENSOR_GYRO) + period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr); + else + period_gyro = U32_MAX; + + if (st->fifo.en & INV_ICM42600_SENSOR_ACCEL) + period_accel = inv_icm42600_odr_to_period(st->conf.accel.odr); + else + period_accel = U32_MAX; + + if (period_gyro <= period_accel) + period = period_gyro; + else + period = period_accel; + + st->fifo.period = period; +} + +int inv_icm42600_buffer_set_fifo_en(struct inv_icm42600_state *st, + unsigned int fifo_en) +{ + unsigned int mask, val; + int ret; + + /* update only FIFO EN bits */ + mask = INV_ICM42600_FIFO_CONFIG1_TMST_FSYNC_EN | + INV_ICM42600_FIFO_CONFIG1_TEMP_EN | + INV_ICM42600_FIFO_CONFIG1_GYRO_EN | + INV_ICM42600_FIFO_CONFIG1_ACCEL_EN; + + val = 0; + if (fifo_en & INV_ICM42600_SENSOR_GYRO) + val |= INV_ICM42600_FIFO_CONFIG1_GYRO_EN; + if (fifo_en & INV_ICM42600_SENSOR_ACCEL) + val |= INV_ICM42600_FIFO_CONFIG1_ACCEL_EN; + if (fifo_en & INV_ICM42600_SENSOR_TEMP) + val |= INV_ICM42600_FIFO_CONFIG1_TEMP_EN; + + ret = regmap_update_bits(st->map, INV_ICM42600_REG_FIFO_CONFIG1, mask, val); + if (ret) + return ret; + + st->fifo.en = fifo_en; + inv_icm42600_buffer_update_fifo_period(st); + + return 0; +} + +static size_t inv_icm42600_get_packet_size(unsigned int fifo_en) +{ + size_t packet_size; + + if ((fifo_en & INV_ICM42600_SENSOR_GYRO) && + (fifo_en & INV_ICM42600_SENSOR_ACCEL)) + packet_size = INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE; + else + packet_size = INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE; + + return packet_size; +} + +static unsigned int inv_icm42600_wm_truncate(unsigned int watermark, + size_t packet_size) +{ + size_t wm_size; + unsigned int wm; + + wm_size = watermark * packet_size; + if (wm_size > INV_ICM42600_FIFO_WATERMARK_MAX) + wm_size = INV_ICM42600_FIFO_WATERMARK_MAX; + + wm = wm_size / packet_size; + + return wm; +} + +/** + * inv_icm42600_buffer_update_watermark - update watermark FIFO threshold + * @st: driver internal state + * + * Returns 0 on success, a negative error code otherwise. + * + * FIFO watermark threshold is computed based on the required watermark values + * set for gyro and accel sensors. Since watermark is all about acceptable data + * latency, use the smallest setting between the 2. It means choosing the + * smallest latency but this is not as simple as choosing the smallest watermark + * value. Latency depends on watermark and ODR. It requires several steps: + * 1) compute gyro and accel latencies and choose the smallest value. + * 2) adapt the choosen latency so that it is a multiple of both gyro and accel + * ones. Otherwise it is possible that you don't meet a requirement. (for + * example with gyro @100Hz wm 4 and accel @100Hz with wm 6, choosing the + * value of 4 will not meet accel latency requirement because 6 is not a + * multiple of 4. You need to use the value 2.) + * 3) Since all periods are multiple of each others, watermark is computed by + * dividing this computed latency by the smallest period, which corresponds + * to the FIFO frequency. Beware that this is only true because we are not + * using 500Hz frequency which is not a multiple of the others. + */ +int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st) +{ + size_t packet_size, wm_size; + unsigned int wm_gyro, wm_accel, watermark; + uint32_t period_gyro, period_accel, period; + uint32_t latency_gyro, latency_accel, latency; + bool restore; + __le16 raw_wm; + int ret; + + packet_size = inv_icm42600_get_packet_size(st->fifo.en); + + /* compute sensors latency, depending on sensor watermark and odr */ + wm_gyro = inv_icm42600_wm_truncate(st->fifo.watermark.gyro, packet_size); + wm_accel = inv_icm42600_wm_truncate(st->fifo.watermark.accel, packet_size); + /* use us for odr to avoid overflow using 32 bits values */ + period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr) / 1000UL; + period_accel = inv_icm42600_odr_to_period(st->conf.accel.odr) / 1000UL; + latency_gyro = period_gyro * wm_gyro; + latency_accel = period_accel * wm_accel; + + /* 0 value for watermark means that the sensor is turned off */ + if (latency_gyro == 0) { + watermark = wm_accel; + } else if (latency_accel == 0) { + watermark = wm_gyro; + } else { + /* compute the smallest latency that is a multiple of both */ + if (latency_gyro <= latency_accel) + latency = latency_gyro - (latency_accel % latency_gyro); + else + latency = latency_accel - (latency_gyro % latency_accel); + /* use the shortest period */ + if (period_gyro <= period_accel) + period = period_gyro; + else + period = period_accel; + /* all this works because periods are multiple of each others */ + watermark = latency / period; + if (watermark < 1) + watermark = 1; + } + + /* compute watermark value in bytes */ + wm_size = watermark * packet_size; + + /* changing FIFO watermark requires to turn off watermark interrupt */ + ret = regmap_update_bits_check(st->map, INV_ICM42600_REG_INT_SOURCE0, + INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN, + 0, &restore); + if (ret) + return ret; + + raw_wm = INV_ICM42600_FIFO_WATERMARK_VAL(wm_size); + memcpy(st->buffer, &raw_wm, sizeof(raw_wm)); + ret = regmap_bulk_write(st->map, INV_ICM42600_REG_FIFO_WATERMARK, + st->buffer, sizeof(raw_wm)); + if (ret) + return ret; + + /* restore watermark interrupt */ + if (restore) { + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0, + INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN, + INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN); + if (ret) + return ret; + } + + return 0; +} + +static int inv_icm42600_buffer_preenable(struct iio_dev *indio_dev) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + struct device *dev = regmap_get_device(st->map); + struct inv_icm42600_timestamp *ts = iio_priv(indio_dev); + + pm_runtime_get_sync(dev); + + mutex_lock(&st->lock); + inv_icm42600_timestamp_reset(ts); + mutex_unlock(&st->lock); + + return 0; +} + +/* + * update_scan_mode callback is turning sensors on and setting data FIFO enable + * bits. + */ +static int inv_icm42600_buffer_postenable(struct iio_dev *indio_dev) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + int ret; + + mutex_lock(&st->lock); + + /* exit if FIFO is already on */ + if (st->fifo.on) { + ret = 0; + goto out_on; + } + + /* set FIFO threshold interrupt */ + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0, + INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN, + INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN); + if (ret) + goto out_unlock; + + /* flush FIFO data */ + ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET, + INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH); + if (ret) + goto out_unlock; + + /* set FIFO in streaming mode */ + ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG, + INV_ICM42600_FIFO_CONFIG_STREAM); + if (ret) + goto out_unlock; + + /* workaround: first read of FIFO count after reset is always 0 */ + ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT, st->buffer, 2); + if (ret) + goto out_unlock; + +out_on: + /* increase FIFO on counter */ + st->fifo.on++; +out_unlock: + mutex_unlock(&st->lock); + return ret; +} + +static int inv_icm42600_buffer_predisable(struct iio_dev *indio_dev) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + int ret; + + mutex_lock(&st->lock); + + /* exit if there are several sensors using the FIFO */ + if (st->fifo.on > 1) { + ret = 0; + goto out_off; + } + + /* set FIFO in bypass mode */ + ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG, + INV_ICM42600_FIFO_CONFIG_BYPASS); + if (ret) + goto out_unlock; + + /* flush FIFO data */ + ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET, + INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH); + if (ret) + goto out_unlock; + + /* disable FIFO threshold interrupt */ + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0, + INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN, 0); + if (ret) + goto out_unlock; + +out_off: + /* decrease FIFO on counter */ + st->fifo.on--; +out_unlock: + mutex_unlock(&st->lock); + return ret; +} + +static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + struct device *dev = regmap_get_device(st->map); + unsigned int sensor; + unsigned int *watermark; + struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; + unsigned int sleep_temp = 0; + unsigned int sleep_sensor = 0; + unsigned int sleep; + int ret; + + if (indio_dev == st->indio_gyro) { + sensor = INV_ICM42600_SENSOR_GYRO; + watermark = &st->fifo.watermark.gyro; + } else if (indio_dev == st->indio_accel) { + sensor = INV_ICM42600_SENSOR_ACCEL; + watermark = &st->fifo.watermark.accel; + } else { + return -EINVAL; + } + + mutex_lock(&st->lock); + + ret = inv_icm42600_buffer_set_fifo_en(st, st->fifo.en & ~sensor); + if (ret) + goto out_unlock; + + *watermark = 0; + ret = inv_icm42600_buffer_update_watermark(st); + if (ret) + goto out_unlock; + + conf.mode = INV_ICM42600_SENSOR_MODE_OFF; + if (sensor == INV_ICM42600_SENSOR_GYRO) + ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep_sensor); + else + ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_sensor); + if (ret) + goto out_unlock; + + /* if FIFO is off, turn temperature off */ + if (!st->fifo.on) + ret = inv_icm42600_set_temp_conf(st, false, &sleep_temp); + +out_unlock: + mutex_unlock(&st->lock); + + /* sleep maximum required time */ + if (sleep_sensor > sleep_temp) + sleep = sleep_sensor; + else + sleep = sleep_temp; + if (sleep) + msleep(sleep); + + pm_runtime_mark_last_busy(dev); + pm_runtime_put_autosuspend(dev); + + return ret; +} + +const struct iio_buffer_setup_ops inv_icm42600_buffer_ops = { + .preenable = inv_icm42600_buffer_preenable, + .postenable = inv_icm42600_buffer_postenable, + .predisable = inv_icm42600_buffer_predisable, + .postdisable = inv_icm42600_buffer_postdisable, +}; + +int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st, + unsigned int max) +{ + size_t max_count; + __be16 *raw_fifo_count; + ssize_t i, size; + const void *accel, *gyro, *timestamp; + const int8_t *temp; + unsigned int odr; + int ret; + + /* reset all samples counters */ + st->fifo.count = 0; + st->fifo.nb.gyro = 0; + st->fifo.nb.accel = 0; + st->fifo.nb.total = 0; + + /* compute maximum FIFO read size */ + if (max == 0) + max_count = sizeof(st->fifo.data); + else + max_count = max * inv_icm42600_get_packet_size(st->fifo.en); + + /* read FIFO count value */ + raw_fifo_count = (__be16 *)st->buffer; + ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT, + raw_fifo_count, sizeof(*raw_fifo_count)); + if (ret) + return ret; + st->fifo.count = be16_to_cpup(raw_fifo_count); + + /* check and clamp FIFO count value */ + if (st->fifo.count == 0) + return 0; + if (st->fifo.count > max_count) + st->fifo.count = max_count; + + /* read all FIFO data in internal buffer */ + ret = regmap_noinc_read(st->map, INV_ICM42600_REG_FIFO_DATA, + st->fifo.data, st->fifo.count); + if (ret) + return ret; + + /* compute number of samples for each sensor */ + for (i = 0; i < st->fifo.count; i += size) { + size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i], + &accel, &gyro, &temp, ×tamp, &odr); + if (size <= 0) + break; + if (gyro != NULL && inv_icm42600_fifo_is_data_valid(gyro)) + st->fifo.nb.gyro++; + if (accel != NULL && inv_icm42600_fifo_is_data_valid(accel)) + st->fifo.nb.accel++; + st->fifo.nb.total++; + } + + return 0; +} + +int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st) +{ + struct inv_icm42600_timestamp *ts; + int ret; + + if (st->fifo.nb.total == 0) + return 0; + + /* handle gyroscope timestamp and FIFO data parsing */ + ts = iio_priv(st->indio_gyro); + inv_icm42600_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total, + st->fifo.nb.gyro, st->timestamp.gyro); + if (st->fifo.nb.gyro > 0) { + ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro); + if (ret) + return ret; + } + + /* handle accelerometer timestamp and FIFO data parsing */ + ts = iio_priv(st->indio_accel); + inv_icm42600_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total, + st->fifo.nb.accel, st->timestamp.accel); + if (st->fifo.nb.accel > 0) { + ret = inv_icm42600_accel_parse_fifo(st->indio_accel); + if (ret) + return ret; + } + + return 0; +} + +int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st, + unsigned int count) +{ + struct inv_icm42600_timestamp *ts; + int64_t gyro_ts, accel_ts; + int ret; + + gyro_ts = iio_get_time_ns(st->indio_gyro); + accel_ts = iio_get_time_ns(st->indio_accel); + + ret = inv_icm42600_buffer_fifo_read(st, count); + if (ret) + return ret; + + if (st->fifo.nb.total == 0) + return 0; + + if (st->fifo.nb.gyro > 0) { + ts = iio_priv(st->indio_gyro); + inv_icm42600_timestamp_interrupt(ts, st->fifo.period, + st->fifo.nb.total, st->fifo.nb.gyro, + gyro_ts); + ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro); + if (ret) + return ret; + } + + if (st->fifo.nb.accel > 0) { + ts = iio_priv(st->indio_accel); + inv_icm42600_timestamp_interrupt(ts, st->fifo.period, + st->fifo.nb.total, st->fifo.nb.accel, + accel_ts); + ret = inv_icm42600_accel_parse_fifo(st->indio_accel); + if (ret) + return ret; + } + + return 0; +} + +int inv_icm42600_buffer_init(struct inv_icm42600_state *st) +{ + unsigned int val; + int ret; + + /* + * Default FIFO configuration (bits 7 to 5) + * - use invalid value + * - FIFO count in bytes + * - FIFO count in big endian + */ + val = INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_ENDIAN; + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0, + GENMASK(7, 5), val); + if (ret) + return ret; + + /* + * Enable FIFO partial read and continuous watermark interrupt. + * Disable all FIFO EN bits. + */ + val = INV_ICM42600_FIFO_CONFIG1_RESUME_PARTIAL_RD | + INV_ICM42600_FIFO_CONFIG1_WM_GT_TH; + return regmap_update_bits(st->map, INV_ICM42600_REG_FIFO_CONFIG1, + GENMASK(6, 5) | GENMASK(3, 0), val); +} diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h new file mode 100644 index 000000000..8b85ee333 --- /dev/null +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h @@ -0,0 +1,98 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ +/* + * Copyright (C) 2020 Invensense, Inc. + */ + +#ifndef INV_ICM42600_BUFFER_H_ +#define INV_ICM42600_BUFFER_H_ + +#include +#include + +struct inv_icm42600_state; + +#define INV_ICM42600_SENSOR_GYRO BIT(0) +#define INV_ICM42600_SENSOR_ACCEL BIT(1) +#define INV_ICM42600_SENSOR_TEMP BIT(2) + +/** + * struct inv_icm42600_fifo - FIFO state variables + * @on: reference counter for FIFO on. + * @en: bits field of INV_ICM42600_SENSOR_* for FIFO EN bits. + * @period: FIFO internal period. + * @watermark: watermark configuration values for accel and gyro. + * @count: number of bytes in the FIFO data buffer. + * @nb: gyro, accel and total samples in the FIFO data buffer. + * @data: FIFO data buffer aligned for DMA (2kB + 32 bytes of read cache). + */ +struct inv_icm42600_fifo { + unsigned int on; + unsigned int en; + uint32_t period; + struct { + unsigned int gyro; + unsigned int accel; + } watermark; + size_t count; + struct { + size_t gyro; + size_t accel; + size_t total; + } nb; + uint8_t data[2080] __aligned(IIO_DMA_MINALIGN); +}; + +/* FIFO data packet */ +struct inv_icm42600_fifo_sensor_data { + __be16 x; + __be16 y; + __be16 z; +} __packed; +#define INV_ICM42600_FIFO_DATA_INVALID -32768 + +static inline int16_t inv_icm42600_fifo_get_sensor_data(__be16 d) +{ + return be16_to_cpu(d); +} + +static inline bool +inv_icm42600_fifo_is_data_valid(const struct inv_icm42600_fifo_sensor_data *s) +{ + int16_t x, y, z; + + x = inv_icm42600_fifo_get_sensor_data(s->x); + y = inv_icm42600_fifo_get_sensor_data(s->y); + z = inv_icm42600_fifo_get_sensor_data(s->z); + + if (x == INV_ICM42600_FIFO_DATA_INVALID && + y == INV_ICM42600_FIFO_DATA_INVALID && + z == INV_ICM42600_FIFO_DATA_INVALID) + return false; + + return true; +} + +ssize_t inv_icm42600_fifo_decode_packet(const void *packet, const void **accel, + const void **gyro, const int8_t **temp, + const void **timestamp, unsigned int *odr); + +extern const struct iio_buffer_setup_ops inv_icm42600_buffer_ops; + +int inv_icm42600_buffer_init(struct inv_icm42600_state *st); + +void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st); + +int inv_icm42600_buffer_set_fifo_en(struct inv_icm42600_state *st, + unsigned int fifo_en); + +int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st); + +int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st, + unsigned int max); + +int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st); + +int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st, + unsigned int count); + +#endif diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c new file mode 100644 index 000000000..ca85fccc9 --- /dev/null +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c @@ -0,0 +1,786 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * Copyright (C) 2020 Invensense, Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "inv_icm42600.h" +#include "inv_icm42600_buffer.h" +#include "inv_icm42600_timestamp.h" + +static const struct regmap_range_cfg inv_icm42600_regmap_ranges[] = { + { + .name = "user banks", + .range_min = 0x0000, + .range_max = 0x4FFF, + .selector_reg = INV_ICM42600_REG_BANK_SEL, + .selector_mask = INV_ICM42600_BANK_SEL_MASK, + .selector_shift = 0, + .window_start = 0, + .window_len = 0x1000, + }, +}; + +const struct regmap_config inv_icm42600_regmap_config = { + .reg_bits = 8, + .val_bits = 8, + .max_register = 0x4FFF, + .ranges = inv_icm42600_regmap_ranges, + .num_ranges = ARRAY_SIZE(inv_icm42600_regmap_ranges), +}; +EXPORT_SYMBOL_GPL(inv_icm42600_regmap_config); + +struct inv_icm42600_hw { + uint8_t whoami; + const char *name; + const struct inv_icm42600_conf *conf; +}; + +/* chip initial default configuration */ +static const struct inv_icm42600_conf inv_icm42600_default_conf = { + .gyro = { + .mode = INV_ICM42600_SENSOR_MODE_OFF, + .fs = INV_ICM42600_GYRO_FS_2000DPS, + .odr = INV_ICM42600_ODR_50HZ, + .filter = INV_ICM42600_FILTER_BW_ODR_DIV_2, + }, + .accel = { + .mode = INV_ICM42600_SENSOR_MODE_OFF, + .fs = INV_ICM42600_ACCEL_FS_16G, + .odr = INV_ICM42600_ODR_50HZ, + .filter = INV_ICM42600_FILTER_BW_ODR_DIV_2, + }, + .temp_en = false, +}; + +static const struct inv_icm42600_hw inv_icm42600_hw[INV_CHIP_NB] = { + [INV_CHIP_ICM42600] = { + .whoami = INV_ICM42600_WHOAMI_ICM42600, + .name = "icm42600", + .conf = &inv_icm42600_default_conf, + }, + [INV_CHIP_ICM42602] = { + .whoami = INV_ICM42600_WHOAMI_ICM42602, + .name = "icm42602", + .conf = &inv_icm42600_default_conf, + }, + [INV_CHIP_ICM42605] = { + .whoami = INV_ICM42600_WHOAMI_ICM42605, + .name = "icm42605", + .conf = &inv_icm42600_default_conf, + }, + [INV_CHIP_ICM42622] = { + .whoami = INV_ICM42600_WHOAMI_ICM42622, + .name = "icm42622", + .conf = &inv_icm42600_default_conf, + }, +}; + +const struct iio_mount_matrix * +inv_icm42600_get_mount_matrix(const struct iio_dev *indio_dev, + const struct iio_chan_spec *chan) +{ + const struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + + return &st->orientation; +} + +uint32_t inv_icm42600_odr_to_period(enum inv_icm42600_odr odr) +{ + static uint32_t odr_periods[INV_ICM42600_ODR_NB] = { + /* reserved values */ + 0, 0, 0, + /* 8kHz */ + 125000, + /* 4kHz */ + 250000, + /* 2kHz */ + 500000, + /* 1kHz */ + 1000000, + /* 200Hz */ + 5000000, + /* 100Hz */ + 10000000, + /* 50Hz */ + 20000000, + /* 25Hz */ + 40000000, + /* 12.5Hz */ + 80000000, + /* 6.25Hz */ + 160000000, + /* 3.125Hz */ + 320000000, + /* 1.5625Hz */ + 640000000, + /* 500Hz */ + 2000000, + }; + + return odr_periods[odr]; +} + +static int inv_icm42600_set_pwr_mgmt0(struct inv_icm42600_state *st, + enum inv_icm42600_sensor_mode gyro, + enum inv_icm42600_sensor_mode accel, + bool temp, unsigned int *sleep_ms) +{ + enum inv_icm42600_sensor_mode oldgyro = st->conf.gyro.mode; + enum inv_icm42600_sensor_mode oldaccel = st->conf.accel.mode; + bool oldtemp = st->conf.temp_en; + unsigned int sleepval; + unsigned int val; + int ret; + + /* if nothing changed, exit */ + if (gyro == oldgyro && accel == oldaccel && temp == oldtemp) + return 0; + + val = INV_ICM42600_PWR_MGMT0_GYRO(gyro) | + INV_ICM42600_PWR_MGMT0_ACCEL(accel); + if (!temp) + val |= INV_ICM42600_PWR_MGMT0_TEMP_DIS; + ret = regmap_write(st->map, INV_ICM42600_REG_PWR_MGMT0, val); + if (ret) + return ret; + + st->conf.gyro.mode = gyro; + st->conf.accel.mode = accel; + st->conf.temp_en = temp; + + /* compute required wait time for sensors to stabilize */ + sleepval = 0; + /* temperature stabilization time */ + if (temp && !oldtemp) { + if (sleepval < INV_ICM42600_TEMP_STARTUP_TIME_MS) + sleepval = INV_ICM42600_TEMP_STARTUP_TIME_MS; + } + /* accel startup time */ + if (accel != oldaccel && oldaccel == INV_ICM42600_SENSOR_MODE_OFF) { + /* block any register write for at least 200 µs */ + usleep_range(200, 300); + if (sleepval < INV_ICM42600_ACCEL_STARTUP_TIME_MS) + sleepval = INV_ICM42600_ACCEL_STARTUP_TIME_MS; + } + if (gyro != oldgyro) { + /* gyro startup time */ + if (oldgyro == INV_ICM42600_SENSOR_MODE_OFF) { + /* block any register write for at least 200 µs */ + usleep_range(200, 300); + if (sleepval < INV_ICM42600_GYRO_STARTUP_TIME_MS) + sleepval = INV_ICM42600_GYRO_STARTUP_TIME_MS; + /* gyro stop time */ + } else if (gyro == INV_ICM42600_SENSOR_MODE_OFF) { + if (sleepval < INV_ICM42600_GYRO_STOP_TIME_MS) + sleepval = INV_ICM42600_GYRO_STOP_TIME_MS; + } + } + + /* deferred sleep value if sleep pointer is provided or direct sleep */ + if (sleep_ms) + *sleep_ms = sleepval; + else if (sleepval) + msleep(sleepval); + + return 0; +} + +int inv_icm42600_set_accel_conf(struct inv_icm42600_state *st, + struct inv_icm42600_sensor_conf *conf, + unsigned int *sleep_ms) +{ + struct inv_icm42600_sensor_conf *oldconf = &st->conf.accel; + unsigned int val; + int ret; + + /* Sanitize missing values with current values */ + if (conf->mode < 0) + conf->mode = oldconf->mode; + if (conf->fs < 0) + conf->fs = oldconf->fs; + if (conf->odr < 0) + conf->odr = oldconf->odr; + if (conf->filter < 0) + conf->filter = oldconf->filter; + + /* set ACCEL_CONFIG0 register (accel fullscale & odr) */ + if (conf->fs != oldconf->fs || conf->odr != oldconf->odr) { + val = INV_ICM42600_ACCEL_CONFIG0_FS(conf->fs) | + INV_ICM42600_ACCEL_CONFIG0_ODR(conf->odr); + ret = regmap_write(st->map, INV_ICM42600_REG_ACCEL_CONFIG0, val); + if (ret) + return ret; + oldconf->fs = conf->fs; + oldconf->odr = conf->odr; + } + + /* set GYRO_ACCEL_CONFIG0 register (accel filter) */ + if (conf->filter != oldconf->filter) { + val = INV_ICM42600_GYRO_ACCEL_CONFIG0_ACCEL_FILT(conf->filter) | + INV_ICM42600_GYRO_ACCEL_CONFIG0_GYRO_FILT(st->conf.gyro.filter); + ret = regmap_write(st->map, INV_ICM42600_REG_GYRO_ACCEL_CONFIG0, val); + if (ret) + return ret; + oldconf->filter = conf->filter; + } + + /* set PWR_MGMT0 register (accel sensor mode) */ + return inv_icm42600_set_pwr_mgmt0(st, st->conf.gyro.mode, conf->mode, + st->conf.temp_en, sleep_ms); +} + +int inv_icm42600_set_gyro_conf(struct inv_icm42600_state *st, + struct inv_icm42600_sensor_conf *conf, + unsigned int *sleep_ms) +{ + struct inv_icm42600_sensor_conf *oldconf = &st->conf.gyro; + unsigned int val; + int ret; + + /* sanitize missing values with current values */ + if (conf->mode < 0) + conf->mode = oldconf->mode; + if (conf->fs < 0) + conf->fs = oldconf->fs; + if (conf->odr < 0) + conf->odr = oldconf->odr; + if (conf->filter < 0) + conf->filter = oldconf->filter; + + /* set GYRO_CONFIG0 register (gyro fullscale & odr) */ + if (conf->fs != oldconf->fs || conf->odr != oldconf->odr) { + val = INV_ICM42600_GYRO_CONFIG0_FS(conf->fs) | + INV_ICM42600_GYRO_CONFIG0_ODR(conf->odr); + ret = regmap_write(st->map, INV_ICM42600_REG_GYRO_CONFIG0, val); + if (ret) + return ret; + oldconf->fs = conf->fs; + oldconf->odr = conf->odr; + } + + /* set GYRO_ACCEL_CONFIG0 register (gyro filter) */ + if (conf->filter != oldconf->filter) { + val = INV_ICM42600_GYRO_ACCEL_CONFIG0_ACCEL_FILT(st->conf.accel.filter) | + INV_ICM42600_GYRO_ACCEL_CONFIG0_GYRO_FILT(conf->filter); + ret = regmap_write(st->map, INV_ICM42600_REG_GYRO_ACCEL_CONFIG0, val); + if (ret) + return ret; + oldconf->filter = conf->filter; + } + + /* set PWR_MGMT0 register (gyro sensor mode) */ + return inv_icm42600_set_pwr_mgmt0(st, conf->mode, st->conf.accel.mode, + st->conf.temp_en, sleep_ms); + + return 0; +} + +int inv_icm42600_set_temp_conf(struct inv_icm42600_state *st, bool enable, + unsigned int *sleep_ms) +{ + return inv_icm42600_set_pwr_mgmt0(st, st->conf.gyro.mode, + st->conf.accel.mode, enable, + sleep_ms); +} + +int inv_icm42600_debugfs_reg(struct iio_dev *indio_dev, unsigned int reg, + unsigned int writeval, unsigned int *readval) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + int ret; + + mutex_lock(&st->lock); + + if (readval) + ret = regmap_read(st->map, reg, readval); + else + ret = regmap_write(st->map, reg, writeval); + + mutex_unlock(&st->lock); + + return ret; +} + +static int inv_icm42600_set_conf(struct inv_icm42600_state *st, + const struct inv_icm42600_conf *conf) +{ + unsigned int val; + int ret; + + /* set PWR_MGMT0 register (gyro & accel sensor mode, temp enabled) */ + val = INV_ICM42600_PWR_MGMT0_GYRO(conf->gyro.mode) | + INV_ICM42600_PWR_MGMT0_ACCEL(conf->accel.mode); + if (!conf->temp_en) + val |= INV_ICM42600_PWR_MGMT0_TEMP_DIS; + ret = regmap_write(st->map, INV_ICM42600_REG_PWR_MGMT0, val); + if (ret) + return ret; + + /* set GYRO_CONFIG0 register (gyro fullscale & odr) */ + val = INV_ICM42600_GYRO_CONFIG0_FS(conf->gyro.fs) | + INV_ICM42600_GYRO_CONFIG0_ODR(conf->gyro.odr); + ret = regmap_write(st->map, INV_ICM42600_REG_GYRO_CONFIG0, val); + if (ret) + return ret; + + /* set ACCEL_CONFIG0 register (accel fullscale & odr) */ + val = INV_ICM42600_ACCEL_CONFIG0_FS(conf->accel.fs) | + INV_ICM42600_ACCEL_CONFIG0_ODR(conf->accel.odr); + ret = regmap_write(st->map, INV_ICM42600_REG_ACCEL_CONFIG0, val); + if (ret) + return ret; + + /* set GYRO_ACCEL_CONFIG0 register (gyro & accel filters) */ + val = INV_ICM42600_GYRO_ACCEL_CONFIG0_ACCEL_FILT(conf->accel.filter) | + INV_ICM42600_GYRO_ACCEL_CONFIG0_GYRO_FILT(conf->gyro.filter); + ret = regmap_write(st->map, INV_ICM42600_REG_GYRO_ACCEL_CONFIG0, val); + if (ret) + return ret; + + /* update internal conf */ + st->conf = *conf; + + return 0; +} + +/** + * inv_icm42600_setup() - check and setup chip + * @st: driver internal state + * @bus_setup: callback for setting up bus specific registers + * + * Returns 0 on success, a negative error code otherwise. + */ +static int inv_icm42600_setup(struct inv_icm42600_state *st, + inv_icm42600_bus_setup bus_setup) +{ + const struct inv_icm42600_hw *hw = &inv_icm42600_hw[st->chip]; + const struct device *dev = regmap_get_device(st->map); + unsigned int val; + int ret; + + /* check chip self-identification value */ + ret = regmap_read(st->map, INV_ICM42600_REG_WHOAMI, &val); + if (ret) + return ret; + if (val != hw->whoami) { + dev_err(dev, "invalid whoami %#02x expected %#02x (%s)\n", + val, hw->whoami, hw->name); + return -ENODEV; + } + st->name = hw->name; + + /* reset to make sure previous state are not there */ + ret = regmap_write(st->map, INV_ICM42600_REG_DEVICE_CONFIG, + INV_ICM42600_DEVICE_CONFIG_SOFT_RESET); + if (ret) + return ret; + msleep(INV_ICM42600_RESET_TIME_MS); + + ret = regmap_read(st->map, INV_ICM42600_REG_INT_STATUS, &val); + if (ret) + return ret; + if (!(val & INV_ICM42600_INT_STATUS_RESET_DONE)) { + dev_err(dev, "reset error, reset done bit not set\n"); + return -ENODEV; + } + + /* set chip bus configuration */ + ret = bus_setup(st); + if (ret) + return ret; + + /* sensor data in big-endian (default) */ + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0, + INV_ICM42600_INTF_CONFIG0_SENSOR_DATA_ENDIAN, + INV_ICM42600_INTF_CONFIG0_SENSOR_DATA_ENDIAN); + if (ret) + return ret; + + return inv_icm42600_set_conf(st, hw->conf); +} + +static irqreturn_t inv_icm42600_irq_timestamp(int irq, void *_data) +{ + struct inv_icm42600_state *st = _data; + + st->timestamp.gyro = iio_get_time_ns(st->indio_gyro); + st->timestamp.accel = iio_get_time_ns(st->indio_accel); + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t inv_icm42600_irq_handler(int irq, void *_data) +{ + struct inv_icm42600_state *st = _data; + struct device *dev = regmap_get_device(st->map); + unsigned int status; + int ret; + + mutex_lock(&st->lock); + + ret = regmap_read(st->map, INV_ICM42600_REG_INT_STATUS, &status); + if (ret) + goto out_unlock; + + /* FIFO full */ + if (status & INV_ICM42600_INT_STATUS_FIFO_FULL) + dev_warn(dev, "FIFO full data lost!\n"); + + /* FIFO threshold reached */ + if (status & INV_ICM42600_INT_STATUS_FIFO_THS) { + ret = inv_icm42600_buffer_fifo_read(st, 0); + if (ret) { + dev_err(dev, "FIFO read error %d\n", ret); + goto out_unlock; + } + ret = inv_icm42600_buffer_fifo_parse(st); + if (ret) + dev_err(dev, "FIFO parsing error %d\n", ret); + } + +out_unlock: + mutex_unlock(&st->lock); + return IRQ_HANDLED; +} + +/** + * inv_icm42600_irq_init() - initialize int pin and interrupt handler + * @st: driver internal state + * @irq: irq number + * @irq_type: irq trigger type + * @open_drain: true if irq is open drain, false for push-pull + * + * Returns 0 on success, a negative error code otherwise. + */ +static int inv_icm42600_irq_init(struct inv_icm42600_state *st, int irq, + int irq_type, bool open_drain) +{ + struct device *dev = regmap_get_device(st->map); + unsigned int val; + int ret; + + /* configure INT1 interrupt: default is active low on edge */ + switch (irq_type) { + case IRQF_TRIGGER_RISING: + case IRQF_TRIGGER_HIGH: + val = INV_ICM42600_INT_CONFIG_INT1_ACTIVE_HIGH; + break; + default: + val = INV_ICM42600_INT_CONFIG_INT1_ACTIVE_LOW; + break; + } + + switch (irq_type) { + case IRQF_TRIGGER_LOW: + case IRQF_TRIGGER_HIGH: + val |= INV_ICM42600_INT_CONFIG_INT1_LATCHED; + break; + default: + break; + } + + if (!open_drain) + val |= INV_ICM42600_INT_CONFIG_INT1_PUSH_PULL; + + ret = regmap_write(st->map, INV_ICM42600_REG_INT_CONFIG, val); + if (ret) + return ret; + + /* Deassert async reset for proper INT pin operation (cf datasheet) */ + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_CONFIG1, + INV_ICM42600_INT_CONFIG1_ASYNC_RESET, 0); + if (ret) + return ret; + + return devm_request_threaded_irq(dev, irq, inv_icm42600_irq_timestamp, + inv_icm42600_irq_handler, irq_type, + "inv_icm42600", st); +} + +static int inv_icm42600_enable_regulator_vddio(struct inv_icm42600_state *st) +{ + int ret; + + ret = regulator_enable(st->vddio_supply); + if (ret) + return ret; + + /* wait a little for supply ramp */ + usleep_range(3000, 4000); + + return 0; +} + +static void inv_icm42600_disable_vdd_reg(void *_data) +{ + struct inv_icm42600_state *st = _data; + const struct device *dev = regmap_get_device(st->map); + int ret; + + ret = regulator_disable(st->vdd_supply); + if (ret) + dev_err(dev, "failed to disable vdd error %d\n", ret); +} + +static void inv_icm42600_disable_vddio_reg(void *_data) +{ + struct inv_icm42600_state *st = _data; + const struct device *dev = regmap_get_device(st->map); + int ret; + + ret = regulator_disable(st->vddio_supply); + if (ret) + dev_err(dev, "failed to disable vddio error %d\n", ret); +} + +static void inv_icm42600_disable_pm(void *_data) +{ + struct device *dev = _data; + + pm_runtime_put_sync(dev); + pm_runtime_disable(dev); +} + +int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq, + inv_icm42600_bus_setup bus_setup) +{ + struct device *dev = regmap_get_device(regmap); + struct inv_icm42600_state *st; + struct irq_data *irq_desc; + int irq_type; + bool open_drain; + int ret; + + if (chip <= INV_CHIP_INVALID || chip >= INV_CHIP_NB) { + dev_err(dev, "invalid chip = %d\n", chip); + return -ENODEV; + } + + /* get irq properties, set trigger falling by default */ + irq_desc = irq_get_irq_data(irq); + if (!irq_desc) { + dev_err(dev, "could not find IRQ %d\n", irq); + return -EINVAL; + } + + irq_type = irqd_get_trigger_type(irq_desc); + if (!irq_type) + irq_type = IRQF_TRIGGER_FALLING; + + open_drain = device_property_read_bool(dev, "drive-open-drain"); + + st = devm_kzalloc(dev, sizeof(*st), GFP_KERNEL); + if (!st) + return -ENOMEM; + + dev_set_drvdata(dev, st); + mutex_init(&st->lock); + st->chip = chip; + st->map = regmap; + + ret = iio_read_mount_matrix(dev, &st->orientation); + if (ret) { + dev_err(dev, "failed to retrieve mounting matrix %d\n", ret); + return ret; + } + + st->vdd_supply = devm_regulator_get(dev, "vdd"); + if (IS_ERR(st->vdd_supply)) + return PTR_ERR(st->vdd_supply); + + st->vddio_supply = devm_regulator_get(dev, "vddio"); + if (IS_ERR(st->vddio_supply)) + return PTR_ERR(st->vddio_supply); + + ret = regulator_enable(st->vdd_supply); + if (ret) + return ret; + msleep(INV_ICM42600_POWER_UP_TIME_MS); + + ret = devm_add_action_or_reset(dev, inv_icm42600_disable_vdd_reg, st); + if (ret) + return ret; + + ret = inv_icm42600_enable_regulator_vddio(st); + if (ret) + return ret; + + ret = devm_add_action_or_reset(dev, inv_icm42600_disable_vddio_reg, st); + if (ret) + return ret; + + /* setup chip registers */ + ret = inv_icm42600_setup(st, bus_setup); + if (ret) + return ret; + + ret = inv_icm42600_timestamp_setup(st); + if (ret) + return ret; + + ret = inv_icm42600_buffer_init(st); + if (ret) + return ret; + + st->indio_gyro = inv_icm42600_gyro_init(st); + if (IS_ERR(st->indio_gyro)) + return PTR_ERR(st->indio_gyro); + + st->indio_accel = inv_icm42600_accel_init(st); + if (IS_ERR(st->indio_accel)) + return PTR_ERR(st->indio_accel); + + ret = inv_icm42600_irq_init(st, irq, irq_type, open_drain); + if (ret) + return ret; + + /* setup runtime power management */ + ret = pm_runtime_set_active(dev); + if (ret) + return ret; + pm_runtime_get_noresume(dev); + pm_runtime_enable(dev); + pm_runtime_set_autosuspend_delay(dev, INV_ICM42600_SUSPEND_DELAY_MS); + pm_runtime_use_autosuspend(dev); + pm_runtime_put(dev); + + return devm_add_action_or_reset(dev, inv_icm42600_disable_pm, dev); +} +EXPORT_SYMBOL_GPL(inv_icm42600_core_probe); + +/* + * Suspend saves sensors state and turns everything off. + * Check first if runtime suspend has not already done the job. + */ +static int __maybe_unused inv_icm42600_suspend(struct device *dev) +{ + struct inv_icm42600_state *st = dev_get_drvdata(dev); + int ret; + + mutex_lock(&st->lock); + + st->suspended.gyro = st->conf.gyro.mode; + st->suspended.accel = st->conf.accel.mode; + st->suspended.temp = st->conf.temp_en; + if (pm_runtime_suspended(dev)) { + ret = 0; + goto out_unlock; + } + + /* disable FIFO data streaming */ + if (st->fifo.on) { + ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG, + INV_ICM42600_FIFO_CONFIG_BYPASS); + if (ret) + goto out_unlock; + } + + ret = inv_icm42600_set_pwr_mgmt0(st, INV_ICM42600_SENSOR_MODE_OFF, + INV_ICM42600_SENSOR_MODE_OFF, false, + NULL); + if (ret) + goto out_unlock; + + regulator_disable(st->vddio_supply); + +out_unlock: + mutex_unlock(&st->lock); + return ret; +} + +/* + * System resume gets the system back on and restores the sensors state. + * Manually put runtime power management in system active state. + */ +static int __maybe_unused inv_icm42600_resume(struct device *dev) +{ + struct inv_icm42600_state *st = dev_get_drvdata(dev); + int ret; + + mutex_lock(&st->lock); + + ret = inv_icm42600_enable_regulator_vddio(st); + if (ret) + goto out_unlock; + + pm_runtime_disable(dev); + pm_runtime_set_active(dev); + pm_runtime_enable(dev); + + /* restore sensors state */ + ret = inv_icm42600_set_pwr_mgmt0(st, st->suspended.gyro, + st->suspended.accel, + st->suspended.temp, NULL); + if (ret) + goto out_unlock; + + /* restore FIFO data streaming */ + if (st->fifo.on) + ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG, + INV_ICM42600_FIFO_CONFIG_STREAM); + +out_unlock: + mutex_unlock(&st->lock); + return ret; +} + +/* Runtime suspend will turn off sensors that are enabled by iio devices. */ +static int __maybe_unused inv_icm42600_runtime_suspend(struct device *dev) +{ + struct inv_icm42600_state *st = dev_get_drvdata(dev); + int ret; + + mutex_lock(&st->lock); + + /* disable all sensors */ + ret = inv_icm42600_set_pwr_mgmt0(st, INV_ICM42600_SENSOR_MODE_OFF, + INV_ICM42600_SENSOR_MODE_OFF, false, + NULL); + if (ret) + goto error_unlock; + + regulator_disable(st->vddio_supply); + +error_unlock: + mutex_unlock(&st->lock); + return ret; +} + +/* Sensors are enabled by iio devices, no need to turn them back on here. */ +static int __maybe_unused inv_icm42600_runtime_resume(struct device *dev) +{ + struct inv_icm42600_state *st = dev_get_drvdata(dev); + int ret; + + mutex_lock(&st->lock); + + ret = inv_icm42600_enable_regulator_vddio(st); + + mutex_unlock(&st->lock); + return ret; +} + +const struct dev_pm_ops inv_icm42600_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(inv_icm42600_suspend, inv_icm42600_resume) + SET_RUNTIME_PM_OPS(inv_icm42600_runtime_suspend, + inv_icm42600_runtime_resume, NULL) +}; +EXPORT_SYMBOL_GPL(inv_icm42600_pm_ops); + +MODULE_AUTHOR("InvenSense, Inc."); +MODULE_DESCRIPTION("InvenSense ICM-426xx device driver"); +MODULE_LICENSE("GPL"); diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c new file mode 100644 index 000000000..9d94a8518 --- /dev/null +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c @@ -0,0 +1,796 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * Copyright (C) 2020 Invensense, Inc. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "inv_icm42600.h" +#include "inv_icm42600_temp.h" +#include "inv_icm42600_buffer.h" +#include "inv_icm42600_timestamp.h" + +#define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \ + { \ + .type = IIO_ANGL_VEL, \ + .modified = 1, \ + .channel2 = _modifier, \ + .info_mask_separate = \ + BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_CALIBBIAS), \ + .info_mask_shared_by_type = \ + BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_shared_by_type_available = \ + BIT(IIO_CHAN_INFO_SCALE) | \ + BIT(IIO_CHAN_INFO_CALIBBIAS), \ + .info_mask_shared_by_all = \ + BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .info_mask_shared_by_all_available = \ + BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .scan_index = _index, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_BE, \ + }, \ + .ext_info = _ext_info, \ + } + +enum inv_icm42600_gyro_scan { + INV_ICM42600_GYRO_SCAN_X, + INV_ICM42600_GYRO_SCAN_Y, + INV_ICM42600_GYRO_SCAN_Z, + INV_ICM42600_GYRO_SCAN_TEMP, + INV_ICM42600_GYRO_SCAN_TIMESTAMP, +}; + +static const struct iio_chan_spec_ext_info inv_icm42600_gyro_ext_infos[] = { + IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm42600_get_mount_matrix), + {}, +}; + +static const struct iio_chan_spec inv_icm42600_gyro_channels[] = { + INV_ICM42600_GYRO_CHAN(IIO_MOD_X, INV_ICM42600_GYRO_SCAN_X, + inv_icm42600_gyro_ext_infos), + INV_ICM42600_GYRO_CHAN(IIO_MOD_Y, INV_ICM42600_GYRO_SCAN_Y, + inv_icm42600_gyro_ext_infos), + INV_ICM42600_GYRO_CHAN(IIO_MOD_Z, INV_ICM42600_GYRO_SCAN_Z, + inv_icm42600_gyro_ext_infos), + INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP), + IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_GYRO_SCAN_TIMESTAMP), +}; + +/* + * IIO buffer data: size must be a power of 2 and timestamp aligned + * 16 bytes: 6 bytes angular velocity, 2 bytes temperature, 8 bytes timestamp + */ +struct inv_icm42600_gyro_buffer { + struct inv_icm42600_fifo_sensor_data gyro; + int16_t temp; + int64_t timestamp __aligned(8); +}; + +#define INV_ICM42600_SCAN_MASK_GYRO_3AXIS \ + (BIT(INV_ICM42600_GYRO_SCAN_X) | \ + BIT(INV_ICM42600_GYRO_SCAN_Y) | \ + BIT(INV_ICM42600_GYRO_SCAN_Z)) + +#define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_GYRO_SCAN_TEMP) + +static const unsigned long inv_icm42600_gyro_scan_masks[] = { + /* 3-axis gyro + temperature */ + INV_ICM42600_SCAN_MASK_GYRO_3AXIS | INV_ICM42600_SCAN_MASK_TEMP, + 0, +}; + +/* enable gyroscope sensor and FIFO write */ +static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev, + const unsigned long *scan_mask) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm42600_timestamp *ts = iio_priv(indio_dev); + struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; + unsigned int fifo_en = 0; + unsigned int sleep_gyro = 0; + unsigned int sleep_temp = 0; + unsigned int sleep; + int ret; + + mutex_lock(&st->lock); + + if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) { + /* enable temp sensor */ + ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp); + if (ret) + goto out_unlock; + fifo_en |= INV_ICM42600_SENSOR_TEMP; + } + + if (*scan_mask & INV_ICM42600_SCAN_MASK_GYRO_3AXIS) { + /* enable gyro sensor */ + conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE; + ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep_gyro); + if (ret) + goto out_unlock; + fifo_en |= INV_ICM42600_SENSOR_GYRO; + } + + /* update data FIFO write */ + inv_icm42600_timestamp_apply_odr(ts, 0, 0, 0); + ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en); + if (ret) + goto out_unlock; + + ret = inv_icm42600_buffer_update_watermark(st); + +out_unlock: + mutex_unlock(&st->lock); + /* sleep maximum required time */ + if (sleep_gyro > sleep_temp) + sleep = sleep_gyro; + else + sleep = sleep_temp; + if (sleep) + msleep(sleep); + return ret; +} + +static int inv_icm42600_gyro_read_sensor(struct inv_icm42600_state *st, + struct iio_chan_spec const *chan, + int16_t *val) +{ + struct device *dev = regmap_get_device(st->map); + struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; + unsigned int reg; + __be16 *data; + int ret; + + if (chan->type != IIO_ANGL_VEL) + return -EINVAL; + + switch (chan->channel2) { + case IIO_MOD_X: + reg = INV_ICM42600_REG_GYRO_DATA_X; + break; + case IIO_MOD_Y: + reg = INV_ICM42600_REG_GYRO_DATA_Y; + break; + case IIO_MOD_Z: + reg = INV_ICM42600_REG_GYRO_DATA_Z; + break; + default: + return -EINVAL; + } + + pm_runtime_get_sync(dev); + mutex_lock(&st->lock); + + /* enable gyro sensor */ + conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE; + ret = inv_icm42600_set_gyro_conf(st, &conf, NULL); + if (ret) + goto exit; + + /* read gyro register data */ + data = (__be16 *)&st->buffer[0]; + ret = regmap_bulk_read(st->map, reg, data, sizeof(*data)); + if (ret) + goto exit; + + *val = (int16_t)be16_to_cpup(data); + if (*val == INV_ICM42600_DATA_INVALID) + ret = -EINVAL; +exit: + mutex_unlock(&st->lock); + pm_runtime_mark_last_busy(dev); + pm_runtime_put_autosuspend(dev); + return ret; +} + +/* IIO format int + nano */ +static const int inv_icm42600_gyro_scale[] = { + /* +/- 2000dps => 0.001065264 rad/s */ + [2 * INV_ICM42600_GYRO_FS_2000DPS] = 0, + [2 * INV_ICM42600_GYRO_FS_2000DPS + 1] = 1065264, + /* +/- 1000dps => 0.000532632 rad/s */ + [2 * INV_ICM42600_GYRO_FS_1000DPS] = 0, + [2 * INV_ICM42600_GYRO_FS_1000DPS + 1] = 532632, + /* +/- 500dps => 0.000266316 rad/s */ + [2 * INV_ICM42600_GYRO_FS_500DPS] = 0, + [2 * INV_ICM42600_GYRO_FS_500DPS + 1] = 266316, + /* +/- 250dps => 0.000133158 rad/s */ + [2 * INV_ICM42600_GYRO_FS_250DPS] = 0, + [2 * INV_ICM42600_GYRO_FS_250DPS + 1] = 133158, + /* +/- 125dps => 0.000066579 rad/s */ + [2 * INV_ICM42600_GYRO_FS_125DPS] = 0, + [2 * INV_ICM42600_GYRO_FS_125DPS + 1] = 66579, + /* +/- 62.5dps => 0.000033290 rad/s */ + [2 * INV_ICM42600_GYRO_FS_62_5DPS] = 0, + [2 * INV_ICM42600_GYRO_FS_62_5DPS + 1] = 33290, + /* +/- 31.25dps => 0.000016645 rad/s */ + [2 * INV_ICM42600_GYRO_FS_31_25DPS] = 0, + [2 * INV_ICM42600_GYRO_FS_31_25DPS + 1] = 16645, + /* +/- 15.625dps => 0.000008322 rad/s */ + [2 * INV_ICM42600_GYRO_FS_15_625DPS] = 0, + [2 * INV_ICM42600_GYRO_FS_15_625DPS + 1] = 8322, +}; + +static int inv_icm42600_gyro_read_scale(struct inv_icm42600_state *st, + int *val, int *val2) +{ + unsigned int idx; + + idx = st->conf.gyro.fs; + + *val = inv_icm42600_gyro_scale[2 * idx]; + *val2 = inv_icm42600_gyro_scale[2 * idx + 1]; + return IIO_VAL_INT_PLUS_NANO; +} + +static int inv_icm42600_gyro_write_scale(struct inv_icm42600_state *st, + int val, int val2) +{ + struct device *dev = regmap_get_device(st->map); + unsigned int idx; + struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; + int ret; + + for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_gyro_scale); idx += 2) { + if (val == inv_icm42600_gyro_scale[idx] && + val2 == inv_icm42600_gyro_scale[idx + 1]) + break; + } + if (idx >= ARRAY_SIZE(inv_icm42600_gyro_scale)) + return -EINVAL; + + conf.fs = idx / 2; + + pm_runtime_get_sync(dev); + mutex_lock(&st->lock); + + ret = inv_icm42600_set_gyro_conf(st, &conf, NULL); + + mutex_unlock(&st->lock); + pm_runtime_mark_last_busy(dev); + pm_runtime_put_autosuspend(dev); + + return ret; +} + +/* IIO format int + micro */ +static const int inv_icm42600_gyro_odr[] = { + /* 12.5Hz */ + 12, 500000, + /* 25Hz */ + 25, 0, + /* 50Hz */ + 50, 0, + /* 100Hz */ + 100, 0, + /* 200Hz */ + 200, 0, + /* 1kHz */ + 1000, 0, + /* 2kHz */ + 2000, 0, + /* 4kHz */ + 4000, 0, +}; + +static const int inv_icm42600_gyro_odr_conv[] = { + INV_ICM42600_ODR_12_5HZ, + INV_ICM42600_ODR_25HZ, + INV_ICM42600_ODR_50HZ, + INV_ICM42600_ODR_100HZ, + INV_ICM42600_ODR_200HZ, + INV_ICM42600_ODR_1KHZ_LN, + INV_ICM42600_ODR_2KHZ_LN, + INV_ICM42600_ODR_4KHZ_LN, +}; + +static int inv_icm42600_gyro_read_odr(struct inv_icm42600_state *st, + int *val, int *val2) +{ + unsigned int odr; + unsigned int i; + + odr = st->conf.gyro.odr; + + for (i = 0; i < ARRAY_SIZE(inv_icm42600_gyro_odr_conv); ++i) { + if (inv_icm42600_gyro_odr_conv[i] == odr) + break; + } + if (i >= ARRAY_SIZE(inv_icm42600_gyro_odr_conv)) + return -EINVAL; + + *val = inv_icm42600_gyro_odr[2 * i]; + *val2 = inv_icm42600_gyro_odr[2 * i + 1]; + + return IIO_VAL_INT_PLUS_MICRO; +} + +static int inv_icm42600_gyro_write_odr(struct iio_dev *indio_dev, + int val, int val2) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm42600_timestamp *ts = iio_priv(indio_dev); + struct device *dev = regmap_get_device(st->map); + unsigned int idx; + struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; + int ret; + + for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_gyro_odr); idx += 2) { + if (val == inv_icm42600_gyro_odr[idx] && + val2 == inv_icm42600_gyro_odr[idx + 1]) + break; + } + if (idx >= ARRAY_SIZE(inv_icm42600_gyro_odr)) + return -EINVAL; + + conf.odr = inv_icm42600_gyro_odr_conv[idx / 2]; + + pm_runtime_get_sync(dev); + mutex_lock(&st->lock); + + ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr), + iio_buffer_enabled(indio_dev)); + if (ret) + goto out_unlock; + + ret = inv_icm42600_set_gyro_conf(st, &conf, NULL); + if (ret) + goto out_unlock; + inv_icm42600_buffer_update_fifo_period(st); + inv_icm42600_buffer_update_watermark(st); + +out_unlock: + mutex_unlock(&st->lock); + pm_runtime_mark_last_busy(dev); + pm_runtime_put_autosuspend(dev); + + return ret; +} + +/* + * Calibration bias values, IIO range format int + nano. + * Value is limited to +/-64dps coded on 12 bits signed. Step is 1/32 dps. + */ +static int inv_icm42600_gyro_calibbias[] = { + -1, 117010721, /* min: -1.117010721 rad/s */ + 0, 545415, /* step: 0.000545415 rad/s */ + 1, 116465306, /* max: 1.116465306 rad/s */ +}; + +static int inv_icm42600_gyro_read_offset(struct inv_icm42600_state *st, + struct iio_chan_spec const *chan, + int *val, int *val2) +{ + struct device *dev = regmap_get_device(st->map); + int64_t val64; + int32_t bias; + unsigned int reg; + int16_t offset; + uint8_t data[2]; + int ret; + + if (chan->type != IIO_ANGL_VEL) + return -EINVAL; + + switch (chan->channel2) { + case IIO_MOD_X: + reg = INV_ICM42600_REG_OFFSET_USER0; + break; + case IIO_MOD_Y: + reg = INV_ICM42600_REG_OFFSET_USER1; + break; + case IIO_MOD_Z: + reg = INV_ICM42600_REG_OFFSET_USER3; + break; + default: + return -EINVAL; + } + + pm_runtime_get_sync(dev); + mutex_lock(&st->lock); + + ret = regmap_bulk_read(st->map, reg, st->buffer, sizeof(data)); + memcpy(data, st->buffer, sizeof(data)); + + mutex_unlock(&st->lock); + pm_runtime_mark_last_busy(dev); + pm_runtime_put_autosuspend(dev); + if (ret) + return ret; + + /* 12 bits signed value */ + switch (chan->channel2) { + case IIO_MOD_X: + offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11); + break; + case IIO_MOD_Y: + offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11); + break; + case IIO_MOD_Z: + offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11); + break; + default: + return -EINVAL; + } + + /* + * convert raw offset to dps then to rad/s + * 12 bits signed raw max 64 to dps: 64 / 2048 + * dps to rad: Pi / 180 + * result in nano (1000000000) + * (offset * 64 * Pi * 1000000000) / (2048 * 180) + */ + val64 = (int64_t)offset * 64LL * 3141592653LL; + /* for rounding, add + or - divisor (2048 * 180) divided by 2 */ + if (val64 >= 0) + val64 += 2048 * 180 / 2; + else + val64 -= 2048 * 180 / 2; + bias = div_s64(val64, 2048 * 180); + *val = bias / 1000000000L; + *val2 = bias % 1000000000L; + + return IIO_VAL_INT_PLUS_NANO; +} + +static int inv_icm42600_gyro_write_offset(struct inv_icm42600_state *st, + struct iio_chan_spec const *chan, + int val, int val2) +{ + struct device *dev = regmap_get_device(st->map); + int64_t val64, min, max; + unsigned int reg, regval; + int16_t offset; + int ret; + + if (chan->type != IIO_ANGL_VEL) + return -EINVAL; + + switch (chan->channel2) { + case IIO_MOD_X: + reg = INV_ICM42600_REG_OFFSET_USER0; + break; + case IIO_MOD_Y: + reg = INV_ICM42600_REG_OFFSET_USER1; + break; + case IIO_MOD_Z: + reg = INV_ICM42600_REG_OFFSET_USER3; + break; + default: + return -EINVAL; + } + + /* inv_icm42600_gyro_calibbias: min - step - max in nano */ + min = (int64_t)inv_icm42600_gyro_calibbias[0] * 1000000000LL + + (int64_t)inv_icm42600_gyro_calibbias[1]; + max = (int64_t)inv_icm42600_gyro_calibbias[4] * 1000000000LL + + (int64_t)inv_icm42600_gyro_calibbias[5]; + val64 = (int64_t)val * 1000000000LL + (int64_t)val2; + if (val64 < min || val64 > max) + return -EINVAL; + + /* + * convert rad/s to dps then to raw value + * rad to dps: 180 / Pi + * dps to raw 12 bits signed, max 64: 2048 / 64 + * val in nano (1000000000) + * val * 180 * 2048 / (Pi * 1000000000 * 64) + */ + val64 = val64 * 180LL * 2048LL; + /* for rounding, add + or - divisor (3141592653 * 64) divided by 2 */ + if (val64 >= 0) + val64 += 3141592653LL * 64LL / 2LL; + else + val64 -= 3141592653LL * 64LL / 2LL; + offset = div64_s64(val64, 3141592653LL * 64LL); + + /* clamp value limited to 12 bits signed */ + if (offset < -2048) + offset = -2048; + else if (offset > 2047) + offset = 2047; + + pm_runtime_get_sync(dev); + mutex_lock(&st->lock); + + switch (chan->channel2) { + case IIO_MOD_X: + /* OFFSET_USER1 register is shared */ + ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER1, + ®val); + if (ret) + goto out_unlock; + st->buffer[0] = offset & 0xFF; + st->buffer[1] = (regval & 0xF0) | ((offset & 0xF00) >> 8); + break; + case IIO_MOD_Y: + /* OFFSET_USER1 register is shared */ + ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER1, + ®val); + if (ret) + goto out_unlock; + st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F); + st->buffer[1] = offset & 0xFF; + break; + case IIO_MOD_Z: + /* OFFSET_USER4 register is shared */ + ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4, + ®val); + if (ret) + goto out_unlock; + st->buffer[0] = offset & 0xFF; + st->buffer[1] = (regval & 0xF0) | ((offset & 0xF00) >> 8); + break; + default: + ret = -EINVAL; + goto out_unlock; + } + + ret = regmap_bulk_write(st->map, reg, st->buffer, 2); + +out_unlock: + mutex_unlock(&st->lock); + pm_runtime_mark_last_busy(dev); + pm_runtime_put_autosuspend(dev); + return ret; +} + +static int inv_icm42600_gyro_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + int16_t data; + int ret; + + switch (chan->type) { + case IIO_ANGL_VEL: + break; + case IIO_TEMP: + return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask); + default: + return -EINVAL; + } + + switch (mask) { + case IIO_CHAN_INFO_RAW: + ret = iio_device_claim_direct_mode(indio_dev); + if (ret) + return ret; + ret = inv_icm42600_gyro_read_sensor(st, chan, &data); + iio_device_release_direct_mode(indio_dev); + if (ret) + return ret; + *val = data; + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + return inv_icm42600_gyro_read_scale(st, val, val2); + case IIO_CHAN_INFO_SAMP_FREQ: + return inv_icm42600_gyro_read_odr(st, val, val2); + case IIO_CHAN_INFO_CALIBBIAS: + return inv_icm42600_gyro_read_offset(st, chan, val, val2); + default: + return -EINVAL; + } +} + +static int inv_icm42600_gyro_read_avail(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + const int **vals, + int *type, int *length, long mask) +{ + if (chan->type != IIO_ANGL_VEL) + return -EINVAL; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + *vals = inv_icm42600_gyro_scale; + *type = IIO_VAL_INT_PLUS_NANO; + *length = ARRAY_SIZE(inv_icm42600_gyro_scale); + return IIO_AVAIL_LIST; + case IIO_CHAN_INFO_SAMP_FREQ: + *vals = inv_icm42600_gyro_odr; + *type = IIO_VAL_INT_PLUS_MICRO; + *length = ARRAY_SIZE(inv_icm42600_gyro_odr); + return IIO_AVAIL_LIST; + case IIO_CHAN_INFO_CALIBBIAS: + *vals = inv_icm42600_gyro_calibbias; + *type = IIO_VAL_INT_PLUS_NANO; + return IIO_AVAIL_RANGE; + default: + return -EINVAL; + } +} + +static int inv_icm42600_gyro_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + int ret; + + if (chan->type != IIO_ANGL_VEL) + return -EINVAL; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + ret = iio_device_claim_direct_mode(indio_dev); + if (ret) + return ret; + ret = inv_icm42600_gyro_write_scale(st, val, val2); + iio_device_release_direct_mode(indio_dev); + return ret; + case IIO_CHAN_INFO_SAMP_FREQ: + return inv_icm42600_gyro_write_odr(indio_dev, val, val2); + case IIO_CHAN_INFO_CALIBBIAS: + ret = iio_device_claim_direct_mode(indio_dev); + if (ret) + return ret; + ret = inv_icm42600_gyro_write_offset(st, chan, val, val2); + iio_device_release_direct_mode(indio_dev); + return ret; + default: + return -EINVAL; + } +} + +static int inv_icm42600_gyro_write_raw_get_fmt(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + long mask) +{ + if (chan->type != IIO_ANGL_VEL) + return -EINVAL; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + return IIO_VAL_INT_PLUS_NANO; + case IIO_CHAN_INFO_SAMP_FREQ: + return IIO_VAL_INT_PLUS_MICRO; + case IIO_CHAN_INFO_CALIBBIAS: + return IIO_VAL_INT_PLUS_NANO; + default: + return -EINVAL; + } +} + +static int inv_icm42600_gyro_hwfifo_set_watermark(struct iio_dev *indio_dev, + unsigned int val) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + int ret; + + mutex_lock(&st->lock); + + st->fifo.watermark.gyro = val; + ret = inv_icm42600_buffer_update_watermark(st); + + mutex_unlock(&st->lock); + + return ret; +} + +static int inv_icm42600_gyro_hwfifo_flush(struct iio_dev *indio_dev, + unsigned int count) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + int ret; + + if (count == 0) + return 0; + + mutex_lock(&st->lock); + + ret = inv_icm42600_buffer_hwfifo_flush(st, count); + if (!ret) + ret = st->fifo.nb.gyro; + + mutex_unlock(&st->lock); + + return ret; +} + +static const struct iio_info inv_icm42600_gyro_info = { + .read_raw = inv_icm42600_gyro_read_raw, + .read_avail = inv_icm42600_gyro_read_avail, + .write_raw = inv_icm42600_gyro_write_raw, + .write_raw_get_fmt = inv_icm42600_gyro_write_raw_get_fmt, + .debugfs_reg_access = inv_icm42600_debugfs_reg, + .update_scan_mode = inv_icm42600_gyro_update_scan_mode, + .hwfifo_set_watermark = inv_icm42600_gyro_hwfifo_set_watermark, + .hwfifo_flush_to_buffer = inv_icm42600_gyro_hwfifo_flush, +}; + +struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st) +{ + struct device *dev = regmap_get_device(st->map); + const char *name; + struct inv_icm42600_timestamp *ts; + struct iio_dev *indio_dev; + int ret; + + name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name); + if (!name) + return ERR_PTR(-ENOMEM); + + indio_dev = devm_iio_device_alloc(dev, sizeof(*ts)); + if (!indio_dev) + return ERR_PTR(-ENOMEM); + + ts = iio_priv(indio_dev); + inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.gyro.odr)); + + iio_device_set_drvdata(indio_dev, st); + indio_dev->name = name; + indio_dev->info = &inv_icm42600_gyro_info; + indio_dev->modes = INDIO_DIRECT_MODE; + indio_dev->channels = inv_icm42600_gyro_channels; + indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_gyro_channels); + indio_dev->available_scan_masks = inv_icm42600_gyro_scan_masks; + indio_dev->setup_ops = &inv_icm42600_buffer_ops; + + ret = devm_iio_kfifo_buffer_setup(dev, indio_dev, + &inv_icm42600_buffer_ops); + if (ret) + return ERR_PTR(ret); + + ret = devm_iio_device_register(dev, indio_dev); + if (ret) + return ERR_PTR(ret); + + return indio_dev; +} + +int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm42600_timestamp *ts = iio_priv(indio_dev); + ssize_t i, size; + unsigned int no; + const void *accel, *gyro, *timestamp; + const int8_t *temp; + unsigned int odr; + int64_t ts_val; + struct inv_icm42600_gyro_buffer buffer; + + /* parse all fifo packets */ + for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) { + size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i], + &accel, &gyro, &temp, ×tamp, &odr); + /* quit if error or FIFO is empty */ + if (size <= 0) + return size; + + /* skip packet if no gyro data or data is invalid */ + if (gyro == NULL || !inv_icm42600_fifo_is_data_valid(gyro)) + continue; + + /* update odr */ + if (odr & INV_ICM42600_SENSOR_GYRO) + inv_icm42600_timestamp_apply_odr(ts, st->fifo.period, + st->fifo.nb.total, no); + + /* buffer is copied to userspace, zeroing it to avoid any data leak */ + memset(&buffer, 0, sizeof(buffer)); + memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro)); + /* convert 8 bits FIFO temperature in high resolution format */ + buffer.temp = temp ? (*temp * 64) : 0; + ts_val = inv_icm42600_timestamp_pop(ts); + iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val); + } + + return 0; +} diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c new file mode 100644 index 000000000..d4a692b83 --- /dev/null +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c @@ -0,0 +1,104 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * Copyright (C) 2020 InvenSense, Inc. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "inv_icm42600.h" + +static int inv_icm42600_i2c_bus_setup(struct inv_icm42600_state *st) +{ + unsigned int mask, val; + int ret; + + /* + * setup interface registers + * This register write to REG_INTF_CONFIG6 enables a spike filter that + * is impacting the line and can prevent the I2C ACK to be seen by the + * controller. So we don't test the return value. + */ + regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG6, + INV_ICM42600_INTF_CONFIG6_MASK, + INV_ICM42600_INTF_CONFIG6_I3C_EN); + + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG4, + INV_ICM42600_INTF_CONFIG4_I3C_BUS_ONLY, 0); + if (ret) + return ret; + + /* set slew rates for I2C and SPI */ + mask = INV_ICM42600_DRIVE_CONFIG_I2C_MASK | + INV_ICM42600_DRIVE_CONFIG_SPI_MASK; + val = INV_ICM42600_DRIVE_CONFIG_I2C(INV_ICM42600_SLEW_RATE_12_36NS) | + INV_ICM42600_DRIVE_CONFIG_SPI(INV_ICM42600_SLEW_RATE_12_36NS); + ret = regmap_update_bits(st->map, INV_ICM42600_REG_DRIVE_CONFIG, + mask, val); + if (ret) + return ret; + + /* disable SPI bus */ + return regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0, + INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_MASK, + INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_SPI_DIS); +} + +static int inv_icm42600_probe(struct i2c_client *client) +{ + const void *match; + enum inv_icm42600_chip chip; + struct regmap *regmap; + + if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_I2C_BLOCK)) + return -ENOTSUPP; + + match = device_get_match_data(&client->dev); + if (!match) + return -EINVAL; + chip = (uintptr_t)match; + + regmap = devm_regmap_init_i2c(client, &inv_icm42600_regmap_config); + if (IS_ERR(regmap)) + return PTR_ERR(regmap); + + return inv_icm42600_core_probe(regmap, chip, client->irq, + inv_icm42600_i2c_bus_setup); +} + +static const struct of_device_id inv_icm42600_of_matches[] = { + { + .compatible = "invensense,icm42600", + .data = (void *)INV_CHIP_ICM42600, + }, { + .compatible = "invensense,icm42602", + .data = (void *)INV_CHIP_ICM42602, + }, { + .compatible = "invensense,icm42605", + .data = (void *)INV_CHIP_ICM42605, + }, { + .compatible = "invensense,icm42622", + .data = (void *)INV_CHIP_ICM42622, + }, + {} +}; +MODULE_DEVICE_TABLE(of, inv_icm42600_of_matches); + +static struct i2c_driver inv_icm42600_driver = { + .driver = { + .name = "inv-icm42600-i2c", + .of_match_table = inv_icm42600_of_matches, + .pm = &inv_icm42600_pm_ops, + }, + .probe_new = inv_icm42600_probe, +}; +module_i2c_driver(inv_icm42600_driver); + +MODULE_AUTHOR("InvenSense, Inc."); +MODULE_DESCRIPTION("InvenSense ICM-426xx I2C driver"); +MODULE_LICENSE("GPL"); diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c new file mode 100644 index 000000000..e6305e5fa --- /dev/null +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c @@ -0,0 +1,100 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * Copyright (C) 2020 InvenSense, Inc. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "inv_icm42600.h" + +static int inv_icm42600_spi_bus_setup(struct inv_icm42600_state *st) +{ + unsigned int mask, val; + int ret; + + /* setup interface registers */ + val = INV_ICM42600_INTF_CONFIG6_I3C_EN | + INV_ICM42600_INTF_CONFIG6_I3C_SDR_EN | + INV_ICM42600_INTF_CONFIG6_I3C_DDR_EN; + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG6, + INV_ICM42600_INTF_CONFIG6_MASK, val); + if (ret) + return ret; + + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG4, + INV_ICM42600_INTF_CONFIG4_I3C_BUS_ONLY, 0); + if (ret) + return ret; + + /* set slew rates for I2C and SPI */ + mask = INV_ICM42600_DRIVE_CONFIG_I2C_MASK | + INV_ICM42600_DRIVE_CONFIG_SPI_MASK; + val = INV_ICM42600_DRIVE_CONFIG_I2C(INV_ICM42600_SLEW_RATE_20_60NS) | + INV_ICM42600_DRIVE_CONFIG_SPI(INV_ICM42600_SLEW_RATE_INF_2NS); + ret = regmap_update_bits(st->map, INV_ICM42600_REG_DRIVE_CONFIG, + mask, val); + if (ret) + return ret; + + /* disable i2c bus */ + return regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0, + INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_MASK, + INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_I2C_DIS); +} + +static int inv_icm42600_probe(struct spi_device *spi) +{ + const void *match; + enum inv_icm42600_chip chip; + struct regmap *regmap; + + match = device_get_match_data(&spi->dev); + if (!match) + return -EINVAL; + chip = (uintptr_t)match; + + regmap = devm_regmap_init_spi(spi, &inv_icm42600_regmap_config); + if (IS_ERR(regmap)) + return PTR_ERR(regmap); + + return inv_icm42600_core_probe(regmap, chip, spi->irq, + inv_icm42600_spi_bus_setup); +} + +static const struct of_device_id inv_icm42600_of_matches[] = { + { + .compatible = "invensense,icm42600", + .data = (void *)INV_CHIP_ICM42600, + }, { + .compatible = "invensense,icm42602", + .data = (void *)INV_CHIP_ICM42602, + }, { + .compatible = "invensense,icm42605", + .data = (void *)INV_CHIP_ICM42605, + }, { + .compatible = "invensense,icm42622", + .data = (void *)INV_CHIP_ICM42622, + }, + {} +}; +MODULE_DEVICE_TABLE(of, inv_icm42600_of_matches); + +static struct spi_driver inv_icm42600_driver = { + .driver = { + .name = "inv-icm42600-spi", + .of_match_table = inv_icm42600_of_matches, + .pm = &inv_icm42600_pm_ops, + }, + .probe = inv_icm42600_probe, +}; +module_spi_driver(inv_icm42600_driver); + +MODULE_AUTHOR("InvenSense, Inc."); +MODULE_DESCRIPTION("InvenSense ICM-426xx SPI driver"); +MODULE_LICENSE("GPL"); diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_temp.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_temp.c new file mode 100644 index 000000000..213cce1c3 --- /dev/null +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_temp.c @@ -0,0 +1,84 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * Copyright (C) 2020 Invensense, Inc. + */ + +#include +#include +#include +#include +#include +#include + +#include "inv_icm42600.h" +#include "inv_icm42600_temp.h" + +static int inv_icm42600_temp_read(struct inv_icm42600_state *st, int16_t *temp) +{ + struct device *dev = regmap_get_device(st->map); + __be16 *raw; + int ret; + + pm_runtime_get_sync(dev); + mutex_lock(&st->lock); + + ret = inv_icm42600_set_temp_conf(st, true, NULL); + if (ret) + goto exit; + + raw = (__be16 *)&st->buffer[0]; + ret = regmap_bulk_read(st->map, INV_ICM42600_REG_TEMP_DATA, raw, sizeof(*raw)); + if (ret) + goto exit; + + *temp = (int16_t)be16_to_cpup(raw); + if (*temp == INV_ICM42600_DATA_INVALID) + ret = -EINVAL; + +exit: + mutex_unlock(&st->lock); + pm_runtime_mark_last_busy(dev); + pm_runtime_put_autosuspend(dev); + + return ret; +} + +int inv_icm42600_temp_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask) +{ + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); + int16_t temp; + int ret; + + if (chan->type != IIO_TEMP) + return -EINVAL; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + ret = iio_device_claim_direct_mode(indio_dev); + if (ret) + return ret; + ret = inv_icm42600_temp_read(st, &temp); + iio_device_release_direct_mode(indio_dev); + if (ret) + return ret; + *val = temp; + return IIO_VAL_INT; + /* + * T°C = (temp / 132.48) + 25 + * Tm°C = 1000 * ((temp * 100 / 13248) + 25) + * scale: 100000 / 13248 ~= 7.548309 + * offset: 25000 + */ + case IIO_CHAN_INFO_SCALE: + *val = 7; + *val2 = 548309; + return IIO_VAL_INT_PLUS_MICRO; + case IIO_CHAN_INFO_OFFSET: + *val = 25000; + return IIO_VAL_INT; + default: + return -EINVAL; + } +} diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_temp.h b/drivers/iio/imu/inv_icm42600/inv_icm42600_temp.h new file mode 100644 index 000000000..394118651 --- /dev/null +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_temp.h @@ -0,0 +1,30 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ +/* + * Copyright (C) 2020 Invensense, Inc. + */ + +#ifndef INV_ICM42600_TEMP_H_ +#define INV_ICM42600_TEMP_H_ + +#include + +#define INV_ICM42600_TEMP_CHAN(_index) \ + { \ + .type = IIO_TEMP, \ + .info_mask_separate = \ + BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_OFFSET) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .scan_index = _index, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + }, \ + } + +int inv_icm42600_temp_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask); + +#endif diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c new file mode 100644 index 000000000..7f2dc41f8 --- /dev/null +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c @@ -0,0 +1,195 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * Copyright (C) 2020 Invensense, Inc. + */ + +#include +#include +#include + +#include "inv_icm42600.h" +#include "inv_icm42600_timestamp.h" + +/* internal chip period is 32kHz, 31250ns */ +#define INV_ICM42600_TIMESTAMP_PERIOD 31250 +/* allow a jitter of +/- 2% */ +#define INV_ICM42600_TIMESTAMP_JITTER 2 +/* compute min and max periods accepted */ +#define INV_ICM42600_TIMESTAMP_MIN_PERIOD(_p) \ + (((_p) * (100 - INV_ICM42600_TIMESTAMP_JITTER)) / 100) +#define INV_ICM42600_TIMESTAMP_MAX_PERIOD(_p) \ + (((_p) * (100 + INV_ICM42600_TIMESTAMP_JITTER)) / 100) + +/* Add a new value inside an accumulator and update the estimate value */ +static void inv_update_acc(struct inv_icm42600_timestamp_acc *acc, uint32_t val) +{ + uint64_t sum = 0; + size_t i; + + acc->values[acc->idx++] = val; + if (acc->idx >= ARRAY_SIZE(acc->values)) + acc->idx = 0; + + /* compute the mean of all stored values, use 0 as empty slot */ + for (i = 0; i < ARRAY_SIZE(acc->values); ++i) { + if (acc->values[i] == 0) + break; + sum += acc->values[i]; + } + + acc->val = div_u64(sum, i); +} + +void inv_icm42600_timestamp_init(struct inv_icm42600_timestamp *ts, + uint32_t period) +{ + /* initial odr for sensor after reset is 1kHz */ + const uint32_t default_period = 1000000; + + /* current multiplier and period values after reset */ + ts->mult = default_period / INV_ICM42600_TIMESTAMP_PERIOD; + ts->period = default_period; + /* new set multiplier is the one from chip initialization */ + ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD; + + /* use theoretical value for chip period */ + inv_update_acc(&ts->chip_period, INV_ICM42600_TIMESTAMP_PERIOD); +} + +int inv_icm42600_timestamp_setup(struct inv_icm42600_state *st) +{ + unsigned int val; + + /* enable timestamp register */ + val = INV_ICM42600_TMST_CONFIG_TMST_TO_REGS_EN | + INV_ICM42600_TMST_CONFIG_TMST_EN; + return regmap_update_bits(st->map, INV_ICM42600_REG_TMST_CONFIG, + INV_ICM42600_TMST_CONFIG_MASK, val); +} + +int inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts, + uint32_t period, bool fifo) +{ + /* when FIFO is on, prevent odr change if one is already pending */ + if (fifo && ts->new_mult != 0) + return -EAGAIN; + + ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD; + + return 0; +} + +static bool inv_validate_period(uint32_t period, uint32_t mult) +{ + const uint32_t chip_period = INV_ICM42600_TIMESTAMP_PERIOD; + uint32_t period_min, period_max; + + /* check that period is acceptable */ + period_min = INV_ICM42600_TIMESTAMP_MIN_PERIOD(chip_period) * mult; + period_max = INV_ICM42600_TIMESTAMP_MAX_PERIOD(chip_period) * mult; + if (period > period_min && period < period_max) + return true; + else + return false; +} + +static bool inv_compute_chip_period(struct inv_icm42600_timestamp *ts, + uint32_t mult, uint32_t period) +{ + uint32_t new_chip_period; + + if (!inv_validate_period(period, mult)) + return false; + + /* update chip internal period estimation */ + new_chip_period = period / mult; + inv_update_acc(&ts->chip_period, new_chip_period); + + return true; +} + +void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts, + uint32_t fifo_period, size_t fifo_nb, + size_t sensor_nb, int64_t timestamp) +{ + struct inv_icm42600_timestamp_interval *it; + int64_t delta, interval; + const uint32_t fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD; + uint32_t period = ts->period; + int32_t m; + bool valid = false; + + if (fifo_nb == 0) + return; + + /* update interrupt timestamp and compute chip and sensor periods */ + it = &ts->it; + it->lo = it->up; + it->up = timestamp; + delta = it->up - it->lo; + if (it->lo != 0) { + /* compute period: delta time divided by number of samples */ + period = div_s64(delta, fifo_nb); + valid = inv_compute_chip_period(ts, fifo_mult, period); + /* update sensor period if chip internal period is updated */ + if (valid) + ts->period = ts->mult * ts->chip_period.val; + } + + /* no previous data, compute theoritical value from interrupt */ + if (ts->timestamp == 0) { + /* elapsed time: sensor period * sensor samples number */ + interval = (int64_t)ts->period * (int64_t)sensor_nb; + ts->timestamp = it->up - interval; + return; + } + + /* if interrupt interval is valid, sync with interrupt timestamp */ + if (valid) { + /* compute measured fifo_period */ + fifo_period = fifo_mult * ts->chip_period.val; + /* delta time between last sample and last interrupt */ + delta = it->lo - ts->timestamp; + /* if there are multiple samples, go back to first one */ + while (delta >= (fifo_period * 3 / 2)) + delta -= fifo_period; + /* compute maximal adjustment value */ + m = INV_ICM42600_TIMESTAMP_MAX_PERIOD(ts->period) - ts->period; + if (delta > m) + delta = m; + else if (delta < -m) + delta = -m; + ts->timestamp += delta; + } +} + +void inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp *ts, + uint32_t fifo_period, size_t fifo_nb, + unsigned int fifo_no) +{ + int64_t interval; + uint32_t fifo_mult; + + if (ts->new_mult == 0) + return; + + /* update to new multiplier and update period */ + ts->mult = ts->new_mult; + ts->new_mult = 0; + ts->period = ts->mult * ts->chip_period.val; + + /* + * After ODR change the time interval with the previous sample is + * undertermined (depends when the change occures). So we compute the + * timestamp from the current interrupt using the new FIFO period, the + * total number of samples and the current sample numero. + */ + if (ts->timestamp != 0) { + /* compute measured fifo period */ + fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD; + fifo_period = fifo_mult * ts->chip_period.val; + /* computes time interval between interrupt and this sample */ + interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period; + ts->timestamp = ts->it.up - interval; + } +} diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h new file mode 100644 index 000000000..4e4f331d4 --- /dev/null +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h @@ -0,0 +1,85 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ +/* + * Copyright (C) 2020 Invensense, Inc. + */ + +#ifndef INV_ICM42600_TIMESTAMP_H_ +#define INV_ICM42600_TIMESTAMP_H_ + +#include + +struct inv_icm42600_state; + +/** + * struct inv_icm42600_timestamp_interval - timestamps interval + * @lo: interval lower bound + * @up: interval upper bound + */ +struct inv_icm42600_timestamp_interval { + int64_t lo; + int64_t up; +}; + +/** + * struct inv_icm42600_timestamp_acc - accumulator for computing an estimation + * @val: current estimation of the value, the mean of all values + * @idx: current index of the next free place in values table + * @values: table of all measured values, use for computing the mean + */ +struct inv_icm42600_timestamp_acc { + uint32_t val; + size_t idx; + uint32_t values[32]; +}; + +/** + * struct inv_icm42600_timestamp - timestamp management states + * @it: interrupts interval timestamps + * @timestamp: store last timestamp for computing next data timestamp + * @mult: current internal period multiplier + * @new_mult: new set internal period multiplier (not yet effective) + * @period: measured current period of the sensor + * @chip_period: accumulator for computing internal chip period + */ +struct inv_icm42600_timestamp { + struct inv_icm42600_timestamp_interval it; + int64_t timestamp; + uint32_t mult; + uint32_t new_mult; + uint32_t period; + struct inv_icm42600_timestamp_acc chip_period; +}; + +void inv_icm42600_timestamp_init(struct inv_icm42600_timestamp *ts, + uint32_t period); + +int inv_icm42600_timestamp_setup(struct inv_icm42600_state *st); + +int inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts, + uint32_t period, bool fifo); + +void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts, + uint32_t fifo_period, size_t fifo_nb, + size_t sensor_nb, int64_t timestamp); + +static inline int64_t +inv_icm42600_timestamp_pop(struct inv_icm42600_timestamp *ts) +{ + ts->timestamp += ts->period; + return ts->timestamp; +} + +void inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp *ts, + uint32_t fifo_period, size_t fifo_nb, + unsigned int fifo_no); + +static inline void +inv_icm42600_timestamp_reset(struct inv_icm42600_timestamp *ts) +{ + const struct inv_icm42600_timestamp_interval interval_init = {0LL, 0LL}; + + ts->it = interval_init; + ts->timestamp = 0; +} + +#endif diff --git a/drivers/iio/imu/inv_mpu6050/Kconfig b/drivers/iio/imu/inv_mpu6050/Kconfig new file mode 100644 index 000000000..3636b1bc9 --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/Kconfig @@ -0,0 +1,34 @@ +# SPDX-License-Identifier: GPL-2.0-only +# +# inv-mpu6050 drivers for Invensense MPU devices and combos +# + +config INV_MPU6050_IIO + tristate + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + +config INV_MPU6050_I2C + tristate "Invensense MPU6050 devices (I2C)" + depends on I2C + select I2C_MUX + select INV_MPU6050_IIO + select REGMAP_I2C + help + This driver supports the Invensense MPU6050/9150, + MPU6500/6515/6880/9250/9255, ICM20608(D)/20609/20689, ICM20602/ICM20690 + and IAM20680 motion tracking devices over I2C. + This driver can be built as a module. The module will be called + inv-mpu6050-i2c. + +config INV_MPU6050_SPI + tristate "Invensense MPU6050 devices (SPI)" + depends on SPI_MASTER + select INV_MPU6050_IIO + select REGMAP_SPI + help + This driver supports the Invensense MPU6000, + MPU6500/6515/6880/9250/9255, ICM20608(D)/20609/20689, ICM20602/ICM20690 + and IAM20680 motion tracking devices over SPI. + This driver can be built as a module. The module will be called + inv-mpu6050-spi. diff --git a/drivers/iio/imu/inv_mpu6050/Makefile b/drivers/iio/imu/inv_mpu6050/Makefile new file mode 100644 index 000000000..c103441a9 --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/Makefile @@ -0,0 +1,14 @@ +# SPDX-License-Identifier: GPL-2.0 +# +# Makefile for Invensense MPU6050 device. +# + +obj-$(CONFIG_INV_MPU6050_IIO) += inv-mpu6050.o +inv-mpu6050-y := inv_mpu_core.o inv_mpu_ring.o inv_mpu_trigger.o \ + inv_mpu_aux.o inv_mpu_magn.o + +obj-$(CONFIG_INV_MPU6050_I2C) += inv-mpu6050-i2c.o +inv-mpu6050-i2c-y := inv_mpu_i2c.o inv_mpu_acpi.o + +obj-$(CONFIG_INV_MPU6050_SPI) += inv-mpu6050-spi.o +inv-mpu6050-spi-y := inv_mpu_spi.o diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_acpi.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_acpi.c new file mode 100644 index 000000000..f7bce428d --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_acpi.c @@ -0,0 +1,198 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * inv_mpu_acpi: ACPI processing for creating client devices + * Copyright (c) 2015, Intel Corporation. + */ + +#ifdef CONFIG_ACPI + +#include +#include +#include +#include +#include "inv_mpu_iio.h" + +enum inv_mpu_product_name { + INV_MPU_NOT_MATCHED, + INV_MPU_ASUS_T100TA, +}; + +static enum inv_mpu_product_name matched_product_name; + +static int __init asus_t100_matched(const struct dmi_system_id *d) +{ + matched_product_name = INV_MPU_ASUS_T100TA; + + return 0; +} + +static const struct dmi_system_id inv_mpu_dev_list[] = { + { + .callback = asus_t100_matched, + .ident = "Asus Transformer Book T100", + .matches = { + DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC"), + DMI_MATCH(DMI_PRODUCT_NAME, "T100TA"), + DMI_MATCH(DMI_PRODUCT_VERSION, "1.0"), + }, + }, + /* Add more matching tables here..*/ + {} +}; + +static int asus_acpi_get_sensor_info(struct acpi_device *adev, + struct i2c_client *client, + struct i2c_board_info *info) +{ + struct acpi_buffer buffer = {ACPI_ALLOCATE_BUFFER, NULL}; + int i; + acpi_status status; + union acpi_object *cpm; + int ret; + + status = acpi_evaluate_object(adev->handle, "CNF0", NULL, &buffer); + if (ACPI_FAILURE(status)) + return -ENODEV; + + cpm = buffer.pointer; + for (i = 0; i < cpm->package.count; ++i) { + union acpi_object *elem; + int j; + + elem = &cpm->package.elements[i]; + for (j = 0; j < elem->package.count; ++j) { + union acpi_object *sub_elem; + + sub_elem = &elem->package.elements[j]; + if (sub_elem->type == ACPI_TYPE_STRING) + strscpy(info->type, sub_elem->string.pointer, + sizeof(info->type)); + else if (sub_elem->type == ACPI_TYPE_INTEGER) { + if (sub_elem->integer.value != client->addr) { + info->addr = sub_elem->integer.value; + break; /* Not a MPU6500 primary */ + } + } + } + } + ret = cpm->package.count; + kfree(buffer.pointer); + + return ret; +} + +static int acpi_i2c_check_resource(struct acpi_resource *ares, void *data) +{ + struct acpi_resource_i2c_serialbus *sb; + u32 *addr = data; + + if (i2c_acpi_get_i2c_resource(ares, &sb)) { + if (*addr) + *addr |= (sb->slave_address << 16); + else + *addr = sb->slave_address; + } + + /* Tell the ACPI core that we already copied this address */ + return 1; +} + +static int inv_mpu_process_acpi_config(struct i2c_client *client, + unsigned short *primary_addr, + unsigned short *secondary_addr) +{ + struct acpi_device *adev = ACPI_COMPANION(&client->dev); + const struct acpi_device_id *id; + u32 i2c_addr = 0; + LIST_HEAD(resources); + int ret; + + id = acpi_match_device(client->dev.driver->acpi_match_table, + &client->dev); + if (!id) + return -ENODEV; + + ret = acpi_dev_get_resources(adev, &resources, + acpi_i2c_check_resource, &i2c_addr); + if (ret < 0) + return ret; + + acpi_dev_free_resource_list(&resources); + *primary_addr = i2c_addr & 0x0000ffff; + *secondary_addr = (i2c_addr & 0xffff0000) >> 16; + + return 0; +} + +int inv_mpu_acpi_create_mux_client(struct i2c_client *client) +{ + struct inv_mpu6050_state *st = iio_priv(dev_get_drvdata(&client->dev)); + struct acpi_device *adev = ACPI_COMPANION(&client->dev); + + st->mux_client = NULL; + if (adev) { + struct i2c_board_info info; + struct i2c_client *mux_client; + int ret = -1; + + memset(&info, 0, sizeof(info)); + + dmi_check_system(inv_mpu_dev_list); + switch (matched_product_name) { + case INV_MPU_ASUS_T100TA: + ret = asus_acpi_get_sensor_info(adev, client, + &info); + break; + /* Add more matched product processing here */ + default: + break; + } + + if (ret < 0) { + /* No matching DMI, so create device on INV6XX type */ + unsigned short primary, secondary; + + ret = inv_mpu_process_acpi_config(client, &primary, + &secondary); + if (!ret && secondary) { + char *name; + + info.addr = secondary; + strscpy(info.type, dev_name(&adev->dev), + sizeof(info.type)); + name = strchr(info.type, ':'); + if (name) + *name = '\0'; + strlcat(info.type, "-client", + sizeof(info.type)); + } else + return 0; /* no secondary addr, which is OK */ + } + mux_client = i2c_new_client_device(st->muxc->adapter[0], &info); + if (IS_ERR(mux_client)) + return PTR_ERR(mux_client); + st->mux_client = mux_client; + } + + return 0; +} + +void inv_mpu_acpi_delete_mux_client(struct i2c_client *client) +{ + struct inv_mpu6050_state *st = iio_priv(dev_get_drvdata(&client->dev)); + + i2c_unregister_device(st->mux_client); +} +#else + +#include "inv_mpu_iio.h" + +int inv_mpu_acpi_create_mux_client(struct i2c_client *client) +{ + return 0; +} + +void inv_mpu_acpi_delete_mux_client(struct i2c_client *client) +{ +} +#endif diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_aux.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_aux.c new file mode 100644 index 000000000..7327e5723 --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_aux.c @@ -0,0 +1,204 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Copyright (C) 2019 TDK-InvenSense, Inc. + */ + +#include +#include +#include +#include + +#include "inv_mpu_aux.h" +#include "inv_mpu_iio.h" + +/* + * i2c master auxiliary bus transfer function. + * Requires the i2c operations to be correctly setup before. + */ +static int inv_mpu_i2c_master_xfer(const struct inv_mpu6050_state *st) +{ + /* use 50hz frequency for xfer */ + const unsigned int freq = 50; + const unsigned int period_ms = 1000 / freq; + uint8_t d; + unsigned int user_ctrl; + int ret; + + /* set sample rate */ + d = INV_MPU6050_FIFO_RATE_TO_DIVIDER(freq); + ret = regmap_write(st->map, st->reg->sample_rate_div, d); + if (ret) + return ret; + + /* start i2c master */ + user_ctrl = st->chip_config.user_ctrl | INV_MPU6050_BIT_I2C_MST_EN; + ret = regmap_write(st->map, st->reg->user_ctrl, user_ctrl); + if (ret) + goto error_restore_rate; + + /* wait for xfer: 1 period + half-period margin */ + msleep(period_ms + period_ms / 2); + + /* stop i2c master */ + user_ctrl = st->chip_config.user_ctrl; + ret = regmap_write(st->map, st->reg->user_ctrl, user_ctrl); + if (ret) + goto error_stop_i2c; + + /* restore sample rate */ + d = st->chip_config.divider; + ret = regmap_write(st->map, st->reg->sample_rate_div, d); + if (ret) + goto error_restore_rate; + + return 0; + +error_stop_i2c: + regmap_write(st->map, st->reg->user_ctrl, st->chip_config.user_ctrl); +error_restore_rate: + regmap_write(st->map, st->reg->sample_rate_div, st->chip_config.divider); + return ret; +} + +/** + * inv_mpu_aux_init() - init i2c auxiliary bus + * @st: driver internal state + * + * Returns 0 on success, a negative error code otherwise. + */ +int inv_mpu_aux_init(const struct inv_mpu6050_state *st) +{ + unsigned int val; + int ret; + + /* configure i2c master */ + val = INV_MPU6050_BITS_I2C_MST_CLK_400KHZ | + INV_MPU6050_BIT_WAIT_FOR_ES; + ret = regmap_write(st->map, INV_MPU6050_REG_I2C_MST_CTRL, val); + if (ret) + return ret; + + /* configure i2c master delay */ + ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV4_CTRL, 0); + if (ret) + return ret; + + val = INV_MPU6050_BIT_I2C_SLV0_DLY_EN | + INV_MPU6050_BIT_I2C_SLV1_DLY_EN | + INV_MPU6050_BIT_I2C_SLV2_DLY_EN | + INV_MPU6050_BIT_I2C_SLV3_DLY_EN | + INV_MPU6050_BIT_DELAY_ES_SHADOW; + return regmap_write(st->map, INV_MPU6050_REG_I2C_MST_DELAY_CTRL, val); +} + +/** + * inv_mpu_aux_read() - read register function for i2c auxiliary bus + * @st: driver internal state. + * @addr: chip i2c Address + * @reg: chip register address + * @val: buffer for storing read bytes + * @size: number of bytes to read + * + * Returns 0 on success, a negative error code otherwise. + */ +int inv_mpu_aux_read(const struct inv_mpu6050_state *st, uint8_t addr, + uint8_t reg, uint8_t *val, size_t size) +{ + unsigned int status; + int ret; + + if (size > 0x0F) + return -EINVAL; + + /* setup i2c SLV0 control: i2c addr, register, enable + size */ + ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_ADDR(0), + INV_MPU6050_BIT_I2C_SLV_RNW | addr); + if (ret) + return ret; + ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_REG(0), reg); + if (ret) + return ret; + ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0), + INV_MPU6050_BIT_SLV_EN | size); + if (ret) + return ret; + + /* do i2c xfer */ + ret = inv_mpu_i2c_master_xfer(st); + if (ret) + goto error_disable_i2c; + + /* disable i2c slave */ + ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0), 0); + if (ret) + goto error_disable_i2c; + + /* check i2c status */ + ret = regmap_read(st->map, INV_MPU6050_REG_I2C_MST_STATUS, &status); + if (ret) + return ret; + if (status & INV_MPU6050_BIT_I2C_SLV0_NACK) + return -EIO; + + /* read data in registers */ + return regmap_bulk_read(st->map, INV_MPU6050_REG_EXT_SENS_DATA, + val, size); + +error_disable_i2c: + regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0), 0); + return ret; +} + +/** + * inv_mpu_aux_write() - write register function for i2c auxiliary bus + * @st: driver internal state. + * @addr: chip i2c Address + * @reg: chip register address + * @val: 1 byte value to write + * + * Returns 0 on success, a negative error code otherwise. + */ +int inv_mpu_aux_write(const struct inv_mpu6050_state *st, uint8_t addr, + uint8_t reg, uint8_t val) +{ + unsigned int status; + int ret; + + /* setup i2c SLV0 control: i2c addr, register, value, enable + size */ + ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_ADDR(0), addr); + if (ret) + return ret; + ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_REG(0), reg); + if (ret) + return ret; + ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_DO(0), val); + if (ret) + return ret; + ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0), + INV_MPU6050_BIT_SLV_EN | 1); + if (ret) + return ret; + + /* do i2c xfer */ + ret = inv_mpu_i2c_master_xfer(st); + if (ret) + goto error_disable_i2c; + + /* disable i2c slave */ + ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0), 0); + if (ret) + goto error_disable_i2c; + + /* check i2c status */ + ret = regmap_read(st->map, INV_MPU6050_REG_I2C_MST_STATUS, &status); + if (ret) + return ret; + if (status & INV_MPU6050_BIT_I2C_SLV0_NACK) + return -EIO; + + return 0; + +error_disable_i2c: + regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0), 0); + return ret; +} diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_aux.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_aux.h new file mode 100644 index 000000000..b66997545 --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_aux.h @@ -0,0 +1,19 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +/* + * Copyright (C) 2019 TDK-InvenSense, Inc. + */ + +#ifndef INV_MPU_AUX_H_ +#define INV_MPU_AUX_H_ + +#include "inv_mpu_iio.h" + +int inv_mpu_aux_init(const struct inv_mpu6050_state *st); + +int inv_mpu_aux_read(const struct inv_mpu6050_state *st, uint8_t addr, + uint8_t reg, uint8_t *val, size_t size); + +int inv_mpu_aux_write(const struct inv_mpu6050_state *st, uint8_t addr, + uint8_t reg, uint8_t val); + +#endif /* INV_MPU_AUX_H_ */ diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c new file mode 100644 index 000000000..19a1ef535 --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c @@ -0,0 +1,1778 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* +* Copyright (C) 2012 Invensense, Inc. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "inv_mpu_iio.h" +#include "inv_mpu_magn.h" + +/* + * this is the gyro scale translated from dynamic range plus/minus + * {250, 500, 1000, 2000} to rad/s + */ +static const int gyro_scale_6050[] = {133090, 266181, 532362, 1064724}; + +/* + * this is the accel scale translated from dynamic range plus/minus + * {2, 4, 8, 16} to m/s^2 + */ +static const int accel_scale[] = {598, 1196, 2392, 4785}; + +static const struct inv_mpu6050_reg_map reg_set_icm20602 = { + .sample_rate_div = INV_MPU6050_REG_SAMPLE_RATE_DIV, + .lpf = INV_MPU6050_REG_CONFIG, + .accel_lpf = INV_MPU6500_REG_ACCEL_CONFIG_2, + .user_ctrl = INV_MPU6050_REG_USER_CTRL, + .fifo_en = INV_MPU6050_REG_FIFO_EN, + .gyro_config = INV_MPU6050_REG_GYRO_CONFIG, + .accl_config = INV_MPU6050_REG_ACCEL_CONFIG, + .fifo_count_h = INV_MPU6050_REG_FIFO_COUNT_H, + .fifo_r_w = INV_MPU6050_REG_FIFO_R_W, + .raw_gyro = INV_MPU6050_REG_RAW_GYRO, + .raw_accl = INV_MPU6050_REG_RAW_ACCEL, + .temperature = INV_MPU6050_REG_TEMPERATURE, + .int_enable = INV_MPU6050_REG_INT_ENABLE, + .int_status = INV_MPU6050_REG_INT_STATUS, + .pwr_mgmt_1 = INV_MPU6050_REG_PWR_MGMT_1, + .pwr_mgmt_2 = INV_MPU6050_REG_PWR_MGMT_2, + .int_pin_cfg = INV_MPU6050_REG_INT_PIN_CFG, + .accl_offset = INV_MPU6500_REG_ACCEL_OFFSET, + .gyro_offset = INV_MPU6050_REG_GYRO_OFFSET, + .i2c_if = INV_ICM20602_REG_I2C_IF, +}; + +static const struct inv_mpu6050_reg_map reg_set_6500 = { + .sample_rate_div = INV_MPU6050_REG_SAMPLE_RATE_DIV, + .lpf = INV_MPU6050_REG_CONFIG, + .accel_lpf = INV_MPU6500_REG_ACCEL_CONFIG_2, + .user_ctrl = INV_MPU6050_REG_USER_CTRL, + .fifo_en = INV_MPU6050_REG_FIFO_EN, + .gyro_config = INV_MPU6050_REG_GYRO_CONFIG, + .accl_config = INV_MPU6050_REG_ACCEL_CONFIG, + .fifo_count_h = INV_MPU6050_REG_FIFO_COUNT_H, + .fifo_r_w = INV_MPU6050_REG_FIFO_R_W, + .raw_gyro = INV_MPU6050_REG_RAW_GYRO, + .raw_accl = INV_MPU6050_REG_RAW_ACCEL, + .temperature = INV_MPU6050_REG_TEMPERATURE, + .int_enable = INV_MPU6050_REG_INT_ENABLE, + .int_status = INV_MPU6050_REG_INT_STATUS, + .pwr_mgmt_1 = INV_MPU6050_REG_PWR_MGMT_1, + .pwr_mgmt_2 = INV_MPU6050_REG_PWR_MGMT_2, + .int_pin_cfg = INV_MPU6050_REG_INT_PIN_CFG, + .accl_offset = INV_MPU6500_REG_ACCEL_OFFSET, + .gyro_offset = INV_MPU6050_REG_GYRO_OFFSET, + .i2c_if = 0, +}; + +static const struct inv_mpu6050_reg_map reg_set_6050 = { + .sample_rate_div = INV_MPU6050_REG_SAMPLE_RATE_DIV, + .lpf = INV_MPU6050_REG_CONFIG, + .user_ctrl = INV_MPU6050_REG_USER_CTRL, + .fifo_en = INV_MPU6050_REG_FIFO_EN, + .gyro_config = INV_MPU6050_REG_GYRO_CONFIG, + .accl_config = INV_MPU6050_REG_ACCEL_CONFIG, + .fifo_count_h = INV_MPU6050_REG_FIFO_COUNT_H, + .fifo_r_w = INV_MPU6050_REG_FIFO_R_W, + .raw_gyro = INV_MPU6050_REG_RAW_GYRO, + .raw_accl = INV_MPU6050_REG_RAW_ACCEL, + .temperature = INV_MPU6050_REG_TEMPERATURE, + .int_enable = INV_MPU6050_REG_INT_ENABLE, + .pwr_mgmt_1 = INV_MPU6050_REG_PWR_MGMT_1, + .pwr_mgmt_2 = INV_MPU6050_REG_PWR_MGMT_2, + .int_pin_cfg = INV_MPU6050_REG_INT_PIN_CFG, + .accl_offset = INV_MPU6050_REG_ACCEL_OFFSET, + .gyro_offset = INV_MPU6050_REG_GYRO_OFFSET, + .i2c_if = 0, +}; + +static const struct inv_mpu6050_chip_config chip_config_6050 = { + .clk = INV_CLK_INTERNAL, + .fsr = INV_MPU6050_FSR_2000DPS, + .lpf = INV_MPU6050_FILTER_20HZ, + .divider = INV_MPU6050_FIFO_RATE_TO_DIVIDER(50), + .gyro_en = true, + .accl_en = true, + .temp_en = true, + .magn_en = false, + .gyro_fifo_enable = false, + .accl_fifo_enable = false, + .temp_fifo_enable = false, + .magn_fifo_enable = false, + .accl_fs = INV_MPU6050_FS_02G, + .user_ctrl = 0, +}; + +static const struct inv_mpu6050_chip_config chip_config_6500 = { + .clk = INV_CLK_PLL, + .fsr = INV_MPU6050_FSR_2000DPS, + .lpf = INV_MPU6050_FILTER_20HZ, + .divider = INV_MPU6050_FIFO_RATE_TO_DIVIDER(50), + .gyro_en = true, + .accl_en = true, + .temp_en = true, + .magn_en = false, + .gyro_fifo_enable = false, + .accl_fifo_enable = false, + .temp_fifo_enable = false, + .magn_fifo_enable = false, + .accl_fs = INV_MPU6050_FS_02G, + .user_ctrl = 0, +}; + +/* Indexed by enum inv_devices */ +static const struct inv_mpu6050_hw hw_info[] = { + { + .whoami = INV_MPU6050_WHOAMI_VALUE, + .name = "MPU6050", + .reg = ®_set_6050, + .config = &chip_config_6050, + .fifo_size = 1024, + .temp = {INV_MPU6050_TEMP_OFFSET, INV_MPU6050_TEMP_SCALE}, + .startup_time = {INV_MPU6050_GYRO_STARTUP_TIME, INV_MPU6050_ACCEL_STARTUP_TIME}, + }, + { + .whoami = INV_MPU6500_WHOAMI_VALUE, + .name = "MPU6500", + .reg = ®_set_6500, + .config = &chip_config_6500, + .fifo_size = 512, + .temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE}, + .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME}, + }, + { + .whoami = INV_MPU6515_WHOAMI_VALUE, + .name = "MPU6515", + .reg = ®_set_6500, + .config = &chip_config_6500, + .fifo_size = 512, + .temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE}, + .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME}, + }, + { + .whoami = INV_MPU6880_WHOAMI_VALUE, + .name = "MPU6880", + .reg = ®_set_6500, + .config = &chip_config_6500, + .fifo_size = 4096, + .temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE}, + .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME}, + }, + { + .whoami = INV_MPU6000_WHOAMI_VALUE, + .name = "MPU6000", + .reg = ®_set_6050, + .config = &chip_config_6050, + .fifo_size = 1024, + .temp = {INV_MPU6050_TEMP_OFFSET, INV_MPU6050_TEMP_SCALE}, + .startup_time = {INV_MPU6050_GYRO_STARTUP_TIME, INV_MPU6050_ACCEL_STARTUP_TIME}, + }, + { + .whoami = INV_MPU9150_WHOAMI_VALUE, + .name = "MPU9150", + .reg = ®_set_6050, + .config = &chip_config_6050, + .fifo_size = 1024, + .temp = {INV_MPU6050_TEMP_OFFSET, INV_MPU6050_TEMP_SCALE}, + .startup_time = {INV_MPU6050_GYRO_STARTUP_TIME, INV_MPU6050_ACCEL_STARTUP_TIME}, + }, + { + .whoami = INV_MPU9250_WHOAMI_VALUE, + .name = "MPU9250", + .reg = ®_set_6500, + .config = &chip_config_6500, + .fifo_size = 512, + .temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE}, + .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME}, + }, + { + .whoami = INV_MPU9255_WHOAMI_VALUE, + .name = "MPU9255", + .reg = ®_set_6500, + .config = &chip_config_6500, + .fifo_size = 512, + .temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE}, + .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME}, + }, + { + .whoami = INV_ICM20608_WHOAMI_VALUE, + .name = "ICM20608", + .reg = ®_set_6500, + .config = &chip_config_6500, + .fifo_size = 512, + .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE}, + .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME}, + }, + { + .whoami = INV_ICM20608D_WHOAMI_VALUE, + .name = "ICM20608D", + .reg = ®_set_6500, + .config = &chip_config_6500, + .fifo_size = 512, + .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE}, + .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME}, + }, + { + .whoami = INV_ICM20609_WHOAMI_VALUE, + .name = "ICM20609", + .reg = ®_set_6500, + .config = &chip_config_6500, + .fifo_size = 4 * 1024, + .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE}, + .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME}, + }, + { + .whoami = INV_ICM20689_WHOAMI_VALUE, + .name = "ICM20689", + .reg = ®_set_6500, + .config = &chip_config_6500, + .fifo_size = 4 * 1024, + .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE}, + .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME}, + }, + { + .whoami = INV_ICM20602_WHOAMI_VALUE, + .name = "ICM20602", + .reg = ®_set_icm20602, + .config = &chip_config_6500, + .fifo_size = 1008, + .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE}, + .startup_time = {INV_ICM20602_GYRO_STARTUP_TIME, INV_ICM20602_ACCEL_STARTUP_TIME}, + }, + { + .whoami = INV_ICM20690_WHOAMI_VALUE, + .name = "ICM20690", + .reg = ®_set_6500, + .config = &chip_config_6500, + .fifo_size = 1024, + .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE}, + .startup_time = {INV_ICM20690_GYRO_STARTUP_TIME, INV_ICM20690_ACCEL_STARTUP_TIME}, + }, + { + .whoami = INV_IAM20680_WHOAMI_VALUE, + .name = "IAM20680", + .reg = ®_set_6500, + .config = &chip_config_6500, + .fifo_size = 512, + .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE}, + .startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME}, + }, +}; + +static int inv_mpu6050_pwr_mgmt_1_write(struct inv_mpu6050_state *st, bool sleep, + int clock, int temp_dis) +{ + u8 val; + + if (clock < 0) + clock = st->chip_config.clk; + if (temp_dis < 0) + temp_dis = !st->chip_config.temp_en; + + val = clock & INV_MPU6050_BIT_CLK_MASK; + if (temp_dis) + val |= INV_MPU6050_BIT_TEMP_DIS; + if (sleep) + val |= INV_MPU6050_BIT_SLEEP; + + dev_dbg(regmap_get_device(st->map), "pwr_mgmt_1: 0x%x\n", val); + return regmap_write(st->map, st->reg->pwr_mgmt_1, val); +} + +static int inv_mpu6050_clock_switch(struct inv_mpu6050_state *st, + unsigned int clock) +{ + int ret; + + switch (st->chip_type) { + case INV_MPU6050: + case INV_MPU6000: + case INV_MPU9150: + /* old chips: switch clock manually */ + ret = inv_mpu6050_pwr_mgmt_1_write(st, false, clock, -1); + if (ret) + return ret; + st->chip_config.clk = clock; + break; + default: + /* automatic clock switching, nothing to do */ + break; + } + + return 0; +} + +int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, + unsigned int mask) +{ + unsigned int sleep; + u8 pwr_mgmt2, user_ctrl; + int ret; + + /* delete useless requests */ + if (mask & INV_MPU6050_SENSOR_ACCL && en == st->chip_config.accl_en) + mask &= ~INV_MPU6050_SENSOR_ACCL; + if (mask & INV_MPU6050_SENSOR_GYRO && en == st->chip_config.gyro_en) + mask &= ~INV_MPU6050_SENSOR_GYRO; + if (mask & INV_MPU6050_SENSOR_TEMP && en == st->chip_config.temp_en) + mask &= ~INV_MPU6050_SENSOR_TEMP; + if (mask & INV_MPU6050_SENSOR_MAGN && en == st->chip_config.magn_en) + mask &= ~INV_MPU6050_SENSOR_MAGN; + if (mask == 0) + return 0; + + /* turn on/off temperature sensor */ + if (mask & INV_MPU6050_SENSOR_TEMP) { + ret = inv_mpu6050_pwr_mgmt_1_write(st, false, -1, !en); + if (ret) + return ret; + st->chip_config.temp_en = en; + } + + /* update user_crtl for driving magnetometer */ + if (mask & INV_MPU6050_SENSOR_MAGN) { + user_ctrl = st->chip_config.user_ctrl; + if (en) + user_ctrl |= INV_MPU6050_BIT_I2C_MST_EN; + else + user_ctrl &= ~INV_MPU6050_BIT_I2C_MST_EN; + ret = regmap_write(st->map, st->reg->user_ctrl, user_ctrl); + if (ret) + return ret; + st->chip_config.user_ctrl = user_ctrl; + st->chip_config.magn_en = en; + } + + /* manage accel & gyro engines */ + if (mask & (INV_MPU6050_SENSOR_ACCL | INV_MPU6050_SENSOR_GYRO)) { + /* compute power management 2 current value */ + pwr_mgmt2 = 0; + if (!st->chip_config.accl_en) + pwr_mgmt2 |= INV_MPU6050_BIT_PWR_ACCL_STBY; + if (!st->chip_config.gyro_en) + pwr_mgmt2 |= INV_MPU6050_BIT_PWR_GYRO_STBY; + + /* update to new requested value */ + if (mask & INV_MPU6050_SENSOR_ACCL) { + if (en) + pwr_mgmt2 &= ~INV_MPU6050_BIT_PWR_ACCL_STBY; + else + pwr_mgmt2 |= INV_MPU6050_BIT_PWR_ACCL_STBY; + } + if (mask & INV_MPU6050_SENSOR_GYRO) { + if (en) + pwr_mgmt2 &= ~INV_MPU6050_BIT_PWR_GYRO_STBY; + else + pwr_mgmt2 |= INV_MPU6050_BIT_PWR_GYRO_STBY; + } + + /* switch clock to internal when turning gyro off */ + if (mask & INV_MPU6050_SENSOR_GYRO && !en) { + ret = inv_mpu6050_clock_switch(st, INV_CLK_INTERNAL); + if (ret) + return ret; + } + + /* update sensors engine */ + dev_dbg(regmap_get_device(st->map), "pwr_mgmt_2: 0x%x\n", + pwr_mgmt2); + ret = regmap_write(st->map, st->reg->pwr_mgmt_2, pwr_mgmt2); + if (ret) + return ret; + if (mask & INV_MPU6050_SENSOR_ACCL) + st->chip_config.accl_en = en; + if (mask & INV_MPU6050_SENSOR_GYRO) + st->chip_config.gyro_en = en; + + /* compute required time to have sensors stabilized */ + sleep = 0; + if (en) { + if (mask & INV_MPU6050_SENSOR_ACCL) { + if (sleep < st->hw->startup_time.accel) + sleep = st->hw->startup_time.accel; + } + if (mask & INV_MPU6050_SENSOR_GYRO) { + if (sleep < st->hw->startup_time.gyro) + sleep = st->hw->startup_time.gyro; + } + } else { + if (mask & INV_MPU6050_SENSOR_GYRO) { + if (sleep < INV_MPU6050_GYRO_DOWN_TIME) + sleep = INV_MPU6050_GYRO_DOWN_TIME; + } + } + if (sleep) + msleep(sleep); + + /* switch clock to PLL when turning gyro on */ + if (mask & INV_MPU6050_SENSOR_GYRO && en) { + ret = inv_mpu6050_clock_switch(st, INV_CLK_PLL); + if (ret) + return ret; + } + } + + return 0; +} + +static int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, + bool power_on) +{ + int result; + + result = inv_mpu6050_pwr_mgmt_1_write(st, !power_on, -1, -1); + if (result) + return result; + + if (power_on) + usleep_range(INV_MPU6050_REG_UP_TIME_MIN, + INV_MPU6050_REG_UP_TIME_MAX); + + return 0; +} + +static int inv_mpu6050_set_gyro_fsr(struct inv_mpu6050_state *st, + enum inv_mpu6050_fsr_e val) +{ + unsigned int gyro_shift; + u8 data; + + switch (st->chip_type) { + case INV_ICM20690: + gyro_shift = INV_ICM20690_GYRO_CONFIG_FSR_SHIFT; + break; + default: + gyro_shift = INV_MPU6050_GYRO_CONFIG_FSR_SHIFT; + break; + } + + data = val << gyro_shift; + return regmap_write(st->map, st->reg->gyro_config, data); +} + +/* + * inv_mpu6050_set_lpf_regs() - set low pass filter registers, chip dependent + * + * MPU60xx/MPU9150 use only 1 register for accelerometer + gyroscope + * MPU6500 and above have a dedicated register for accelerometer + */ +static int inv_mpu6050_set_lpf_regs(struct inv_mpu6050_state *st, + enum inv_mpu6050_filter_e val) +{ + int result; + + result = regmap_write(st->map, st->reg->lpf, val); + if (result) + return result; + + /* set accel lpf */ + switch (st->chip_type) { + case INV_MPU6050: + case INV_MPU6000: + case INV_MPU9150: + /* old chips, nothing to do */ + return 0; + case INV_ICM20689: + case INV_ICM20690: + /* set FIFO size to maximum value */ + val |= INV_ICM20689_BITS_FIFO_SIZE_MAX; + break; + default: + break; + } + + return regmap_write(st->map, st->reg->accel_lpf, val); +} + +/* + * inv_mpu6050_init_config() - Initialize hardware, disable FIFO. + * + * Initial configuration: + * FSR: ± 2000DPS + * DLPF: 20Hz + * FIFO rate: 50Hz + * Clock source: Gyro PLL + */ +static int inv_mpu6050_init_config(struct iio_dev *indio_dev) +{ + int result; + u8 d; + struct inv_mpu6050_state *st = iio_priv(indio_dev); + + result = inv_mpu6050_set_gyro_fsr(st, st->chip_config.fsr); + if (result) + return result; + + result = inv_mpu6050_set_lpf_regs(st, st->chip_config.lpf); + if (result) + return result; + + d = st->chip_config.divider; + result = regmap_write(st->map, st->reg->sample_rate_div, d); + if (result) + return result; + + d = (st->chip_config.accl_fs << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT); + result = regmap_write(st->map, st->reg->accl_config, d); + if (result) + return result; + + result = regmap_write(st->map, st->reg->int_pin_cfg, st->irq_mask); + if (result) + return result; + + /* + * Internal chip period is 1ms (1kHz). + * Let's use at the beginning the theorical value before measuring + * with interrupt timestamps. + */ + st->chip_period = NSEC_PER_MSEC; + + /* magn chip init, noop if not present in the chip */ + result = inv_mpu_magn_probe(st); + if (result) + return result; + + return 0; +} + +static int inv_mpu6050_sensor_set(struct inv_mpu6050_state *st, int reg, + int axis, int val) +{ + int ind, result; + __be16 d = cpu_to_be16(val); + + ind = (axis - IIO_MOD_X) * 2; + result = regmap_bulk_write(st->map, reg + ind, &d, sizeof(d)); + if (result) + return -EINVAL; + + return 0; +} + +static int inv_mpu6050_sensor_show(struct inv_mpu6050_state *st, int reg, + int axis, int *val) +{ + int ind, result; + __be16 d; + + ind = (axis - IIO_MOD_X) * 2; + result = regmap_bulk_read(st->map, reg + ind, &d, sizeof(d)); + if (result) + return -EINVAL; + *val = (short)be16_to_cpup(&d); + + return IIO_VAL_INT; +} + +static int inv_mpu6050_read_channel_data(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val) +{ + struct inv_mpu6050_state *st = iio_priv(indio_dev); + struct device *pdev = regmap_get_device(st->map); + unsigned int freq_hz, period_us, min_sleep_us, max_sleep_us; + int result; + int ret; + + /* compute sample period */ + freq_hz = INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider); + period_us = 1000000 / freq_hz; + + result = pm_runtime_resume_and_get(pdev); + if (result) + return result; + + switch (chan->type) { + case IIO_ANGL_VEL: + if (!st->chip_config.gyro_en) { + result = inv_mpu6050_switch_engine(st, true, + INV_MPU6050_SENSOR_GYRO); + if (result) + goto error_power_off; + /* need to wait 2 periods to have first valid sample */ + min_sleep_us = 2 * period_us; + max_sleep_us = 2 * (period_us + period_us / 2); + usleep_range(min_sleep_us, max_sleep_us); + } + ret = inv_mpu6050_sensor_show(st, st->reg->raw_gyro, + chan->channel2, val); + break; + case IIO_ACCEL: + if (!st->chip_config.accl_en) { + result = inv_mpu6050_switch_engine(st, true, + INV_MPU6050_SENSOR_ACCL); + if (result) + goto error_power_off; + /* wait 1 period for first sample availability */ + min_sleep_us = period_us; + max_sleep_us = period_us + period_us / 2; + usleep_range(min_sleep_us, max_sleep_us); + } + ret = inv_mpu6050_sensor_show(st, st->reg->raw_accl, + chan->channel2, val); + break; + case IIO_TEMP: + /* temperature sensor work only with accel and/or gyro */ + if (!st->chip_config.accl_en && !st->chip_config.gyro_en) { + result = -EBUSY; + goto error_power_off; + } + if (!st->chip_config.temp_en) { + result = inv_mpu6050_switch_engine(st, true, + INV_MPU6050_SENSOR_TEMP); + if (result) + goto error_power_off; + /* wait 1 period for first sample availability */ + min_sleep_us = period_us; + max_sleep_us = period_us + period_us / 2; + usleep_range(min_sleep_us, max_sleep_us); + } + ret = inv_mpu6050_sensor_show(st, st->reg->temperature, + IIO_MOD_X, val); + break; + case IIO_MAGN: + if (!st->chip_config.magn_en) { + result = inv_mpu6050_switch_engine(st, true, + INV_MPU6050_SENSOR_MAGN); + if (result) + goto error_power_off; + /* frequency is limited for magnetometer */ + if (freq_hz > INV_MPU_MAGN_FREQ_HZ_MAX) { + freq_hz = INV_MPU_MAGN_FREQ_HZ_MAX; + period_us = 1000000 / freq_hz; + } + /* need to wait 2 periods to have first valid sample */ + min_sleep_us = 2 * period_us; + max_sleep_us = 2 * (period_us + period_us / 2); + usleep_range(min_sleep_us, max_sleep_us); + } + ret = inv_mpu_magn_read(st, chan->channel2, val); + break; + default: + ret = -EINVAL; + break; + } + + pm_runtime_mark_last_busy(pdev); + pm_runtime_put_autosuspend(pdev); + + return ret; + +error_power_off: + pm_runtime_put_autosuspend(pdev); + return result; +} + +static int +inv_mpu6050_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask) +{ + struct inv_mpu6050_state *st = iio_priv(indio_dev); + int ret = 0; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + ret = iio_device_claim_direct_mode(indio_dev); + if (ret) + return ret; + mutex_lock(&st->lock); + ret = inv_mpu6050_read_channel_data(indio_dev, chan, val); + mutex_unlock(&st->lock); + iio_device_release_direct_mode(indio_dev); + return ret; + case IIO_CHAN_INFO_SCALE: + switch (chan->type) { + case IIO_ANGL_VEL: + mutex_lock(&st->lock); + *val = 0; + *val2 = gyro_scale_6050[st->chip_config.fsr]; + mutex_unlock(&st->lock); + + return IIO_VAL_INT_PLUS_NANO; + case IIO_ACCEL: + mutex_lock(&st->lock); + *val = 0; + *val2 = accel_scale[st->chip_config.accl_fs]; + mutex_unlock(&st->lock); + + return IIO_VAL_INT_PLUS_MICRO; + case IIO_TEMP: + *val = st->hw->temp.scale / 1000000; + *val2 = st->hw->temp.scale % 1000000; + return IIO_VAL_INT_PLUS_MICRO; + case IIO_MAGN: + return inv_mpu_magn_get_scale(st, chan, val, val2); + default: + return -EINVAL; + } + case IIO_CHAN_INFO_OFFSET: + switch (chan->type) { + case IIO_TEMP: + *val = st->hw->temp.offset; + return IIO_VAL_INT; + default: + return -EINVAL; + } + case IIO_CHAN_INFO_CALIBBIAS: + switch (chan->type) { + case IIO_ANGL_VEL: + mutex_lock(&st->lock); + ret = inv_mpu6050_sensor_show(st, st->reg->gyro_offset, + chan->channel2, val); + mutex_unlock(&st->lock); + return ret; + case IIO_ACCEL: + mutex_lock(&st->lock); + ret = inv_mpu6050_sensor_show(st, st->reg->accl_offset, + chan->channel2, val); + mutex_unlock(&st->lock); + return ret; + + default: + return -EINVAL; + } + default: + return -EINVAL; + } +} + +static int inv_mpu6050_write_gyro_scale(struct inv_mpu6050_state *st, int val, + int val2) +{ + int result, i; + + if (val != 0) + return -EINVAL; + + for (i = 0; i < ARRAY_SIZE(gyro_scale_6050); ++i) { + if (gyro_scale_6050[i] == val2) { + result = inv_mpu6050_set_gyro_fsr(st, i); + if (result) + return result; + + st->chip_config.fsr = i; + return 0; + } + } + + return -EINVAL; +} + +static int inv_write_raw_get_fmt(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, long mask) +{ + switch (mask) { + case IIO_CHAN_INFO_SCALE: + switch (chan->type) { + case IIO_ANGL_VEL: + return IIO_VAL_INT_PLUS_NANO; + default: + return IIO_VAL_INT_PLUS_MICRO; + } + default: + return IIO_VAL_INT_PLUS_MICRO; + } + + return -EINVAL; +} + +static int inv_mpu6050_write_accel_scale(struct inv_mpu6050_state *st, int val, + int val2) +{ + int result, i; + u8 d; + + if (val != 0) + return -EINVAL; + + for (i = 0; i < ARRAY_SIZE(accel_scale); ++i) { + if (accel_scale[i] == val2) { + d = (i << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT); + result = regmap_write(st->map, st->reg->accl_config, d); + if (result) + return result; + + st->chip_config.accl_fs = i; + return 0; + } + } + + return -EINVAL; +} + +static int inv_mpu6050_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct inv_mpu6050_state *st = iio_priv(indio_dev); + struct device *pdev = regmap_get_device(st->map); + int result; + + /* + * we should only update scale when the chip is disabled, i.e. + * not running + */ + result = iio_device_claim_direct_mode(indio_dev); + if (result) + return result; + + mutex_lock(&st->lock); + result = pm_runtime_resume_and_get(pdev); + if (result) + goto error_write_raw_unlock; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + switch (chan->type) { + case IIO_ANGL_VEL: + result = inv_mpu6050_write_gyro_scale(st, val, val2); + break; + case IIO_ACCEL: + result = inv_mpu6050_write_accel_scale(st, val, val2); + break; + default: + result = -EINVAL; + break; + } + break; + case IIO_CHAN_INFO_CALIBBIAS: + switch (chan->type) { + case IIO_ANGL_VEL: + result = inv_mpu6050_sensor_set(st, + st->reg->gyro_offset, + chan->channel2, val); + break; + case IIO_ACCEL: + result = inv_mpu6050_sensor_set(st, + st->reg->accl_offset, + chan->channel2, val); + break; + default: + result = -EINVAL; + break; + } + break; + default: + result = -EINVAL; + break; + } + + pm_runtime_mark_last_busy(pdev); + pm_runtime_put_autosuspend(pdev); +error_write_raw_unlock: + mutex_unlock(&st->lock); + iio_device_release_direct_mode(indio_dev); + + return result; +} + +/* + * inv_mpu6050_set_lpf() - set low pass filer based on fifo rate. + * + * Based on the Nyquist principle, the bandwidth of the low + * pass filter must not exceed the signal sampling rate divided + * by 2, or there would be aliasing. + * This function basically search for the correct low pass + * parameters based on the fifo rate, e.g, sampling frequency. + * + * lpf is set automatically when setting sampling rate to avoid any aliases. + */ +static int inv_mpu6050_set_lpf(struct inv_mpu6050_state *st, int rate) +{ + static const int hz[] = {400, 200, 90, 40, 20, 10}; + static const int d[] = { + INV_MPU6050_FILTER_200HZ, INV_MPU6050_FILTER_100HZ, + INV_MPU6050_FILTER_45HZ, INV_MPU6050_FILTER_20HZ, + INV_MPU6050_FILTER_10HZ, INV_MPU6050_FILTER_5HZ + }; + int i, result; + u8 data; + + data = INV_MPU6050_FILTER_5HZ; + for (i = 0; i < ARRAY_SIZE(hz); ++i) { + if (rate >= hz[i]) { + data = d[i]; + break; + } + } + result = inv_mpu6050_set_lpf_regs(st, data); + if (result) + return result; + st->chip_config.lpf = data; + + return 0; +} + +/* + * inv_mpu6050_fifo_rate_store() - Set fifo rate. + */ +static ssize_t +inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr, + const char *buf, size_t count) +{ + int fifo_rate; + u8 d; + int result; + struct iio_dev *indio_dev = dev_to_iio_dev(dev); + struct inv_mpu6050_state *st = iio_priv(indio_dev); + struct device *pdev = regmap_get_device(st->map); + + if (kstrtoint(buf, 10, &fifo_rate)) + return -EINVAL; + if (fifo_rate < INV_MPU6050_MIN_FIFO_RATE || + fifo_rate > INV_MPU6050_MAX_FIFO_RATE) + return -EINVAL; + + /* compute the chip sample rate divider */ + d = INV_MPU6050_FIFO_RATE_TO_DIVIDER(fifo_rate); + /* compute back the fifo rate to handle truncation cases */ + fifo_rate = INV_MPU6050_DIVIDER_TO_FIFO_RATE(d); + + mutex_lock(&st->lock); + if (d == st->chip_config.divider) { + result = 0; + goto fifo_rate_fail_unlock; + } + result = pm_runtime_resume_and_get(pdev); + if (result) + goto fifo_rate_fail_unlock; + + result = regmap_write(st->map, st->reg->sample_rate_div, d); + if (result) + goto fifo_rate_fail_power_off; + st->chip_config.divider = d; + + result = inv_mpu6050_set_lpf(st, fifo_rate); + if (result) + goto fifo_rate_fail_power_off; + + /* update rate for magn, noop if not present in chip */ + result = inv_mpu_magn_set_rate(st, fifo_rate); + if (result) + goto fifo_rate_fail_power_off; + + pm_runtime_mark_last_busy(pdev); +fifo_rate_fail_power_off: + pm_runtime_put_autosuspend(pdev); +fifo_rate_fail_unlock: + mutex_unlock(&st->lock); + if (result) + return result; + + return count; +} + +/* + * inv_fifo_rate_show() - Get the current sampling rate. + */ +static ssize_t +inv_fifo_rate_show(struct device *dev, struct device_attribute *attr, + char *buf) +{ + struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev)); + unsigned fifo_rate; + + mutex_lock(&st->lock); + fifo_rate = INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider); + mutex_unlock(&st->lock); + + return scnprintf(buf, PAGE_SIZE, "%u\n", fifo_rate); +} + +/* + * inv_attr_show() - calling this function will show current + * parameters. + * + * Deprecated in favor of IIO mounting matrix API. + * + * See inv_get_mount_matrix() + */ +static ssize_t inv_attr_show(struct device *dev, struct device_attribute *attr, + char *buf) +{ + struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev)); + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); + s8 *m; + + switch (this_attr->address) { + /* + * In MPU6050, the two matrix are the same because gyro and accel + * are integrated in one chip + */ + case ATTR_GYRO_MATRIX: + case ATTR_ACCL_MATRIX: + m = st->plat_data.orientation; + + return scnprintf(buf, PAGE_SIZE, + "%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]); + default: + return -EINVAL; + } +} + +/** + * inv_mpu6050_validate_trigger() - validate_trigger callback for invensense + * MPU6050 device. + * @indio_dev: The IIO device + * @trig: The new trigger + * + * Returns: 0 if the 'trig' matches the trigger registered by the MPU6050 + * device, -EINVAL otherwise. + */ +static int inv_mpu6050_validate_trigger(struct iio_dev *indio_dev, + struct iio_trigger *trig) +{ + struct inv_mpu6050_state *st = iio_priv(indio_dev); + + if (st->trig != trig) + return -EINVAL; + + return 0; +} + +static const struct iio_mount_matrix * +inv_get_mount_matrix(const struct iio_dev *indio_dev, + const struct iio_chan_spec *chan) +{ + struct inv_mpu6050_state *data = iio_priv(indio_dev); + const struct iio_mount_matrix *matrix; + + if (chan->type == IIO_MAGN) + matrix = &data->magn_orient; + else + matrix = &data->orientation; + + return matrix; +} + +static const struct iio_chan_spec_ext_info inv_ext_info[] = { + IIO_MOUNT_MATRIX(IIO_SHARED_BY_TYPE, inv_get_mount_matrix), + { } +}; + +#define INV_MPU6050_CHAN(_type, _channel2, _index) \ + { \ + .type = _type, \ + .modified = 1, \ + .channel2 = _channel2, \ + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_CALIBBIAS), \ + .scan_index = _index, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .shift = 0, \ + .endianness = IIO_BE, \ + }, \ + .ext_info = inv_ext_info, \ + } + +#define INV_MPU6050_TEMP_CHAN(_index) \ + { \ + .type = IIO_TEMP, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) \ + | BIT(IIO_CHAN_INFO_OFFSET) \ + | BIT(IIO_CHAN_INFO_SCALE), \ + .scan_index = _index, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .shift = 0, \ + .endianness = IIO_BE, \ + }, \ + } + +static const struct iio_chan_spec inv_mpu_channels[] = { + IIO_CHAN_SOFT_TIMESTAMP(INV_MPU6050_SCAN_TIMESTAMP), + + INV_MPU6050_TEMP_CHAN(INV_MPU6050_SCAN_TEMP), + + INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_X, INV_MPU6050_SCAN_GYRO_X), + INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Y, INV_MPU6050_SCAN_GYRO_Y), + INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Z, INV_MPU6050_SCAN_GYRO_Z), + + INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_X, INV_MPU6050_SCAN_ACCL_X), + INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Y, INV_MPU6050_SCAN_ACCL_Y), + INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Z, INV_MPU6050_SCAN_ACCL_Z), +}; + +#define INV_MPU6050_SCAN_MASK_3AXIS_ACCEL \ + (BIT(INV_MPU6050_SCAN_ACCL_X) \ + | BIT(INV_MPU6050_SCAN_ACCL_Y) \ + | BIT(INV_MPU6050_SCAN_ACCL_Z)) + +#define INV_MPU6050_SCAN_MASK_3AXIS_GYRO \ + (BIT(INV_MPU6050_SCAN_GYRO_X) \ + | BIT(INV_MPU6050_SCAN_GYRO_Y) \ + | BIT(INV_MPU6050_SCAN_GYRO_Z)) + +#define INV_MPU6050_SCAN_MASK_TEMP (BIT(INV_MPU6050_SCAN_TEMP)) + +static const unsigned long inv_mpu_scan_masks[] = { + /* 3-axis accel */ + INV_MPU6050_SCAN_MASK_3AXIS_ACCEL, + INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_TEMP, + /* 3-axis gyro */ + INV_MPU6050_SCAN_MASK_3AXIS_GYRO, + INV_MPU6050_SCAN_MASK_3AXIS_GYRO | INV_MPU6050_SCAN_MASK_TEMP, + /* 6-axis accel + gyro */ + INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_3AXIS_GYRO, + INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_3AXIS_GYRO + | INV_MPU6050_SCAN_MASK_TEMP, + 0, +}; + +#define INV_MPU9X50_MAGN_CHAN(_chan2, _bits, _index) \ + { \ + .type = IIO_MAGN, \ + .modified = 1, \ + .channel2 = _chan2, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_SCALE) | \ + BIT(IIO_CHAN_INFO_RAW), \ + .scan_index = _index, \ + .scan_type = { \ + .sign = 's', \ + .realbits = _bits, \ + .storagebits = 16, \ + .shift = 0, \ + .endianness = IIO_BE, \ + }, \ + .ext_info = inv_ext_info, \ + } + +static const struct iio_chan_spec inv_mpu9150_channels[] = { + IIO_CHAN_SOFT_TIMESTAMP(INV_MPU9X50_SCAN_TIMESTAMP), + + INV_MPU6050_TEMP_CHAN(INV_MPU6050_SCAN_TEMP), + + INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_X, INV_MPU6050_SCAN_GYRO_X), + INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Y, INV_MPU6050_SCAN_GYRO_Y), + INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Z, INV_MPU6050_SCAN_GYRO_Z), + + INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_X, INV_MPU6050_SCAN_ACCL_X), + INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Y, INV_MPU6050_SCAN_ACCL_Y), + INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Z, INV_MPU6050_SCAN_ACCL_Z), + + /* Magnetometer resolution is 13 bits */ + INV_MPU9X50_MAGN_CHAN(IIO_MOD_X, 13, INV_MPU9X50_SCAN_MAGN_X), + INV_MPU9X50_MAGN_CHAN(IIO_MOD_Y, 13, INV_MPU9X50_SCAN_MAGN_Y), + INV_MPU9X50_MAGN_CHAN(IIO_MOD_Z, 13, INV_MPU9X50_SCAN_MAGN_Z), +}; + +static const struct iio_chan_spec inv_mpu9250_channels[] = { + IIO_CHAN_SOFT_TIMESTAMP(INV_MPU9X50_SCAN_TIMESTAMP), + + INV_MPU6050_TEMP_CHAN(INV_MPU6050_SCAN_TEMP), + + INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_X, INV_MPU6050_SCAN_GYRO_X), + INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Y, INV_MPU6050_SCAN_GYRO_Y), + INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Z, INV_MPU6050_SCAN_GYRO_Z), + + INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_X, INV_MPU6050_SCAN_ACCL_X), + INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Y, INV_MPU6050_SCAN_ACCL_Y), + INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Z, INV_MPU6050_SCAN_ACCL_Z), + + /* Magnetometer resolution is 16 bits */ + INV_MPU9X50_MAGN_CHAN(IIO_MOD_X, 16, INV_MPU9X50_SCAN_MAGN_X), + INV_MPU9X50_MAGN_CHAN(IIO_MOD_Y, 16, INV_MPU9X50_SCAN_MAGN_Y), + INV_MPU9X50_MAGN_CHAN(IIO_MOD_Z, 16, INV_MPU9X50_SCAN_MAGN_Z), +}; + +#define INV_MPU9X50_SCAN_MASK_3AXIS_MAGN \ + (BIT(INV_MPU9X50_SCAN_MAGN_X) \ + | BIT(INV_MPU9X50_SCAN_MAGN_Y) \ + | BIT(INV_MPU9X50_SCAN_MAGN_Z)) + +static const unsigned long inv_mpu9x50_scan_masks[] = { + /* 3-axis accel */ + INV_MPU6050_SCAN_MASK_3AXIS_ACCEL, + INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_TEMP, + /* 3-axis gyro */ + INV_MPU6050_SCAN_MASK_3AXIS_GYRO, + INV_MPU6050_SCAN_MASK_3AXIS_GYRO | INV_MPU6050_SCAN_MASK_TEMP, + /* 3-axis magn */ + INV_MPU9X50_SCAN_MASK_3AXIS_MAGN, + INV_MPU9X50_SCAN_MASK_3AXIS_MAGN | INV_MPU6050_SCAN_MASK_TEMP, + /* 6-axis accel + gyro */ + INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_3AXIS_GYRO, + INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_3AXIS_GYRO + | INV_MPU6050_SCAN_MASK_TEMP, + /* 6-axis accel + magn */ + INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU9X50_SCAN_MASK_3AXIS_MAGN, + INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU9X50_SCAN_MASK_3AXIS_MAGN + | INV_MPU6050_SCAN_MASK_TEMP, + /* 6-axis gyro + magn */ + INV_MPU6050_SCAN_MASK_3AXIS_GYRO | INV_MPU9X50_SCAN_MASK_3AXIS_MAGN, + INV_MPU6050_SCAN_MASK_3AXIS_GYRO | INV_MPU9X50_SCAN_MASK_3AXIS_MAGN + | INV_MPU6050_SCAN_MASK_TEMP, + /* 9-axis accel + gyro + magn */ + INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_3AXIS_GYRO + | INV_MPU9X50_SCAN_MASK_3AXIS_MAGN, + INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_3AXIS_GYRO + | INV_MPU9X50_SCAN_MASK_3AXIS_MAGN + | INV_MPU6050_SCAN_MASK_TEMP, + 0, +}; + +static const unsigned long inv_icm20602_scan_masks[] = { + /* 3-axis accel + temp (mandatory) */ + INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_TEMP, + /* 3-axis gyro + temp (mandatory) */ + INV_MPU6050_SCAN_MASK_3AXIS_GYRO | INV_MPU6050_SCAN_MASK_TEMP, + /* 6-axis accel + gyro + temp (mandatory) */ + INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_3AXIS_GYRO + | INV_MPU6050_SCAN_MASK_TEMP, + 0, +}; + +/* + * The user can choose any frequency between INV_MPU6050_MIN_FIFO_RATE and + * INV_MPU6050_MAX_FIFO_RATE, but only these frequencies are matched by the + * low-pass filter. Specifically, each of these sampling rates are about twice + * the bandwidth of a corresponding low-pass filter, which should eliminate + * aliasing following the Nyquist principle. By picking a frequency different + * from these, the user risks aliasing effects. + */ +static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("10 20 50 100 200 500"); +static IIO_CONST_ATTR(in_anglvel_scale_available, + "0.000133090 0.000266181 0.000532362 0.001064724"); +static IIO_CONST_ATTR(in_accel_scale_available, + "0.000598 0.001196 0.002392 0.004785"); +static IIO_DEV_ATTR_SAMP_FREQ(S_IRUGO | S_IWUSR, inv_fifo_rate_show, + inv_mpu6050_fifo_rate_store); + +/* Deprecated: kept for userspace backward compatibility. */ +static IIO_DEVICE_ATTR(in_gyro_matrix, S_IRUGO, inv_attr_show, NULL, + ATTR_GYRO_MATRIX); +static IIO_DEVICE_ATTR(in_accel_matrix, S_IRUGO, inv_attr_show, NULL, + ATTR_ACCL_MATRIX); + +static struct attribute *inv_attributes[] = { + &iio_dev_attr_in_gyro_matrix.dev_attr.attr, /* deprecated */ + &iio_dev_attr_in_accel_matrix.dev_attr.attr, /* deprecated */ + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_const_attr_sampling_frequency_available.dev_attr.attr, + &iio_const_attr_in_accel_scale_available.dev_attr.attr, + &iio_const_attr_in_anglvel_scale_available.dev_attr.attr, + NULL, +}; + +static const struct attribute_group inv_attribute_group = { + .attrs = inv_attributes +}; + +static int inv_mpu6050_reg_access(struct iio_dev *indio_dev, + unsigned int reg, + unsigned int writeval, + unsigned int *readval) +{ + struct inv_mpu6050_state *st = iio_priv(indio_dev); + int ret; + + mutex_lock(&st->lock); + if (readval) + ret = regmap_read(st->map, reg, readval); + else + ret = regmap_write(st->map, reg, writeval); + mutex_unlock(&st->lock); + + return ret; +} + +static const struct iio_info mpu_info = { + .read_raw = &inv_mpu6050_read_raw, + .write_raw = &inv_mpu6050_write_raw, + .write_raw_get_fmt = &inv_write_raw_get_fmt, + .attrs = &inv_attribute_group, + .validate_trigger = inv_mpu6050_validate_trigger, + .debugfs_reg_access = &inv_mpu6050_reg_access, +}; + +/* + * inv_check_and_setup_chip() - check and setup chip. + */ +static int inv_check_and_setup_chip(struct inv_mpu6050_state *st) +{ + int result; + unsigned int regval, mask; + int i; + + st->hw = &hw_info[st->chip_type]; + st->reg = hw_info[st->chip_type].reg; + memcpy(&st->chip_config, hw_info[st->chip_type].config, + sizeof(st->chip_config)); + + /* check chip self-identification */ + result = regmap_read(st->map, INV_MPU6050_REG_WHOAMI, ®val); + if (result) + return result; + if (regval != st->hw->whoami) { + /* check whoami against all possible values */ + for (i = 0; i < INV_NUM_PARTS; ++i) { + if (regval == hw_info[i].whoami) { + dev_warn(regmap_get_device(st->map), + "whoami mismatch got 0x%02x (%s) expected 0x%02x (%s)\n", + regval, hw_info[i].name, + st->hw->whoami, st->hw->name); + break; + } + } + if (i >= INV_NUM_PARTS) { + dev_err(regmap_get_device(st->map), + "invalid whoami 0x%02x expected 0x%02x (%s)\n", + regval, st->hw->whoami, st->hw->name); + return -ENODEV; + } + } + + /* reset to make sure previous state are not there */ + result = regmap_write(st->map, st->reg->pwr_mgmt_1, + INV_MPU6050_BIT_H_RESET); + if (result) + return result; + msleep(INV_MPU6050_POWER_UP_TIME); + switch (st->chip_type) { + case INV_MPU6000: + case INV_MPU6500: + case INV_MPU6515: + case INV_MPU6880: + case INV_MPU9250: + case INV_MPU9255: + /* reset signal path (required for spi connection) */ + regval = INV_MPU6050_BIT_TEMP_RST | INV_MPU6050_BIT_ACCEL_RST | + INV_MPU6050_BIT_GYRO_RST; + result = regmap_write(st->map, INV_MPU6050_REG_SIGNAL_PATH_RESET, + regval); + if (result) + return result; + msleep(INV_MPU6050_POWER_UP_TIME); + break; + default: + break; + } + + /* + * Turn power on. After reset, the sleep bit could be on + * or off depending on the OTP settings. Turning power on + * make it in a definite state as well as making the hardware + * state align with the software state + */ + result = inv_mpu6050_set_power_itg(st, true); + if (result) + return result; + mask = INV_MPU6050_SENSOR_ACCL | INV_MPU6050_SENSOR_GYRO | + INV_MPU6050_SENSOR_TEMP | INV_MPU6050_SENSOR_MAGN; + result = inv_mpu6050_switch_engine(st, false, mask); + if (result) + goto error_power_off; + + return 0; + +error_power_off: + inv_mpu6050_set_power_itg(st, false); + return result; +} + +static int inv_mpu_core_enable_regulator_vddio(struct inv_mpu6050_state *st) +{ + int result; + + result = regulator_enable(st->vddio_supply); + if (result) { + dev_err(regmap_get_device(st->map), + "Failed to enable vddio regulator: %d\n", result); + } else { + /* Give the device a little bit of time to start up. */ + usleep_range(3000, 5000); + } + + return result; +} + +static int inv_mpu_core_disable_regulator_vddio(struct inv_mpu6050_state *st) +{ + int result; + + result = regulator_disable(st->vddio_supply); + if (result) + dev_err(regmap_get_device(st->map), + "Failed to disable vddio regulator: %d\n", result); + + return result; +} + +static void inv_mpu_core_disable_regulator_action(void *_data) +{ + struct inv_mpu6050_state *st = _data; + int result; + + result = regulator_disable(st->vdd_supply); + if (result) + dev_err(regmap_get_device(st->map), + "Failed to disable vdd regulator: %d\n", result); + + inv_mpu_core_disable_regulator_vddio(st); +} + +static void inv_mpu_pm_disable(void *data) +{ + struct device *dev = data; + + pm_runtime_disable(dev); +} + +int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name, + int (*inv_mpu_bus_setup)(struct iio_dev *), int chip_type) +{ + struct inv_mpu6050_state *st; + struct iio_dev *indio_dev; + struct inv_mpu6050_platform_data *pdata; + struct device *dev = regmap_get_device(regmap); + int result; + struct irq_data *desc; + int irq_type; + + indio_dev = devm_iio_device_alloc(dev, sizeof(*st)); + if (!indio_dev) + return -ENOMEM; + + BUILD_BUG_ON(ARRAY_SIZE(hw_info) != INV_NUM_PARTS); + if (chip_type < 0 || chip_type >= INV_NUM_PARTS) { + dev_err(dev, "Bad invensense chip_type=%d name=%s\n", + chip_type, name); + return -ENODEV; + } + st = iio_priv(indio_dev); + mutex_init(&st->lock); + st->chip_type = chip_type; + st->irq = irq; + st->map = regmap; + + pdata = dev_get_platdata(dev); + if (!pdata) { + result = iio_read_mount_matrix(dev, &st->orientation); + if (result) { + dev_err(dev, "Failed to retrieve mounting matrix %d\n", + result); + return result; + } + } else { + st->plat_data = *pdata; + } + + if (irq > 0) { + desc = irq_get_irq_data(irq); + if (!desc) { + dev_err(dev, "Could not find IRQ %d\n", irq); + return -EINVAL; + } + + irq_type = irqd_get_trigger_type(desc); + if (!irq_type) + irq_type = IRQF_TRIGGER_RISING; + } else { + /* Doesn't really matter, use the default */ + irq_type = IRQF_TRIGGER_RISING; + } + + if (irq_type & IRQF_TRIGGER_RISING) // rising or both-edge + st->irq_mask = INV_MPU6050_ACTIVE_HIGH; + else if (irq_type == IRQF_TRIGGER_FALLING) + st->irq_mask = INV_MPU6050_ACTIVE_LOW; + else if (irq_type == IRQF_TRIGGER_HIGH) + st->irq_mask = INV_MPU6050_ACTIVE_HIGH | + INV_MPU6050_LATCH_INT_EN; + else if (irq_type == IRQF_TRIGGER_LOW) + st->irq_mask = INV_MPU6050_ACTIVE_LOW | + INV_MPU6050_LATCH_INT_EN; + else { + dev_err(dev, "Invalid interrupt type 0x%x specified\n", + irq_type); + return -EINVAL; + } + + st->vdd_supply = devm_regulator_get(dev, "vdd"); + if (IS_ERR(st->vdd_supply)) + return dev_err_probe(dev, PTR_ERR(st->vdd_supply), + "Failed to get vdd regulator\n"); + + st->vddio_supply = devm_regulator_get(dev, "vddio"); + if (IS_ERR(st->vddio_supply)) + return dev_err_probe(dev, PTR_ERR(st->vddio_supply), + "Failed to get vddio regulator\n"); + + result = regulator_enable(st->vdd_supply); + if (result) { + dev_err(dev, "Failed to enable vdd regulator: %d\n", result); + return result; + } + msleep(INV_MPU6050_POWER_UP_TIME); + + result = inv_mpu_core_enable_regulator_vddio(st); + if (result) { + regulator_disable(st->vdd_supply); + return result; + } + + result = devm_add_action_or_reset(dev, inv_mpu_core_disable_regulator_action, + st); + if (result) { + dev_err(dev, "Failed to setup regulator cleanup action %d\n", + result); + return result; + } + + /* fill magnetometer orientation */ + result = inv_mpu_magn_set_orient(st); + if (result) + return result; + + /* power is turned on inside check chip type*/ + result = inv_check_and_setup_chip(st); + if (result) + return result; + + result = inv_mpu6050_init_config(indio_dev); + if (result) { + dev_err(dev, "Could not initialize device.\n"); + goto error_power_off; + } + + dev_set_drvdata(dev, indio_dev); + /* name will be NULL when enumerated via ACPI */ + if (name) + indio_dev->name = name; + else + indio_dev->name = dev_name(dev); + + /* requires parent device set in indio_dev */ + if (inv_mpu_bus_setup) { + result = inv_mpu_bus_setup(indio_dev); + if (result) + goto error_power_off; + } + + /* chip init is done, turning on runtime power management */ + result = pm_runtime_set_active(dev); + if (result) + goto error_power_off; + pm_runtime_get_noresume(dev); + pm_runtime_enable(dev); + pm_runtime_set_autosuspend_delay(dev, INV_MPU6050_SUSPEND_DELAY_MS); + pm_runtime_use_autosuspend(dev); + pm_runtime_put(dev); + result = devm_add_action_or_reset(dev, inv_mpu_pm_disable, dev); + if (result) + return result; + + switch (chip_type) { + case INV_MPU9150: + indio_dev->channels = inv_mpu9150_channels; + indio_dev->num_channels = ARRAY_SIZE(inv_mpu9150_channels); + indio_dev->available_scan_masks = inv_mpu9x50_scan_masks; + break; + case INV_MPU9250: + case INV_MPU9255: + indio_dev->channels = inv_mpu9250_channels; + indio_dev->num_channels = ARRAY_SIZE(inv_mpu9250_channels); + indio_dev->available_scan_masks = inv_mpu9x50_scan_masks; + break; + case INV_ICM20602: + indio_dev->channels = inv_mpu_channels; + indio_dev->num_channels = ARRAY_SIZE(inv_mpu_channels); + indio_dev->available_scan_masks = inv_icm20602_scan_masks; + break; + default: + indio_dev->channels = inv_mpu_channels; + indio_dev->num_channels = ARRAY_SIZE(inv_mpu_channels); + indio_dev->available_scan_masks = inv_mpu_scan_masks; + break; + } + /* + * Use magnetometer inside the chip only if there is no i2c + * auxiliary device in use. Otherwise Going back to 6-axis only. + */ + if (st->magn_disabled) { + indio_dev->channels = inv_mpu_channels; + indio_dev->num_channels = ARRAY_SIZE(inv_mpu_channels); + indio_dev->available_scan_masks = inv_mpu_scan_masks; + } + + indio_dev->info = &mpu_info; + + if (irq > 0) { + /* + * The driver currently only supports buffered capture with its + * own trigger. So no IRQ, no trigger, no buffer + */ + result = devm_iio_triggered_buffer_setup(dev, indio_dev, + iio_pollfunc_store_time, + inv_mpu6050_read_fifo, + NULL); + if (result) { + dev_err(dev, "configure buffer fail %d\n", result); + return result; + } + + result = inv_mpu6050_probe_trigger(indio_dev, irq_type); + if (result) { + dev_err(dev, "trigger probe fail %d\n", result); + return result; + } + } + + result = devm_iio_device_register(dev, indio_dev); + if (result) { + dev_err(dev, "IIO register fail %d\n", result); + return result; + } + + return 0; + +error_power_off: + inv_mpu6050_set_power_itg(st, false); + return result; +} +EXPORT_SYMBOL_GPL(inv_mpu_core_probe); + +static int __maybe_unused inv_mpu_resume(struct device *dev) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu6050_state *st = iio_priv(indio_dev); + int result; + + mutex_lock(&st->lock); + result = inv_mpu_core_enable_regulator_vddio(st); + if (result) + goto out_unlock; + + result = inv_mpu6050_set_power_itg(st, true); + if (result) + goto out_unlock; + + pm_runtime_disable(dev); + pm_runtime_set_active(dev); + pm_runtime_enable(dev); + + result = inv_mpu6050_switch_engine(st, true, st->suspended_sensors); + if (result) + goto out_unlock; + + if (iio_buffer_enabled(indio_dev)) + result = inv_mpu6050_prepare_fifo(st, true); + +out_unlock: + mutex_unlock(&st->lock); + + return result; +} + +static int __maybe_unused inv_mpu_suspend(struct device *dev) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu6050_state *st = iio_priv(indio_dev); + int result; + + mutex_lock(&st->lock); + + st->suspended_sensors = 0; + if (pm_runtime_suspended(dev)) { + result = 0; + goto out_unlock; + } + + if (iio_buffer_enabled(indio_dev)) { + result = inv_mpu6050_prepare_fifo(st, false); + if (result) + goto out_unlock; + } + + if (st->chip_config.accl_en) + st->suspended_sensors |= INV_MPU6050_SENSOR_ACCL; + if (st->chip_config.gyro_en) + st->suspended_sensors |= INV_MPU6050_SENSOR_GYRO; + if (st->chip_config.temp_en) + st->suspended_sensors |= INV_MPU6050_SENSOR_TEMP; + if (st->chip_config.magn_en) + st->suspended_sensors |= INV_MPU6050_SENSOR_MAGN; + result = inv_mpu6050_switch_engine(st, false, st->suspended_sensors); + if (result) + goto out_unlock; + + result = inv_mpu6050_set_power_itg(st, false); + if (result) + goto out_unlock; + + inv_mpu_core_disable_regulator_vddio(st); +out_unlock: + mutex_unlock(&st->lock); + + return result; +} + +static int __maybe_unused inv_mpu_runtime_suspend(struct device *dev) +{ + struct inv_mpu6050_state *st = iio_priv(dev_get_drvdata(dev)); + unsigned int sensors; + int ret; + + mutex_lock(&st->lock); + + sensors = INV_MPU6050_SENSOR_ACCL | INV_MPU6050_SENSOR_GYRO | + INV_MPU6050_SENSOR_TEMP | INV_MPU6050_SENSOR_MAGN; + ret = inv_mpu6050_switch_engine(st, false, sensors); + if (ret) + goto out_unlock; + + ret = inv_mpu6050_set_power_itg(st, false); + if (ret) + goto out_unlock; + + inv_mpu_core_disable_regulator_vddio(st); + +out_unlock: + mutex_unlock(&st->lock); + return ret; +} + +static int __maybe_unused inv_mpu_runtime_resume(struct device *dev) +{ + struct inv_mpu6050_state *st = iio_priv(dev_get_drvdata(dev)); + int ret; + + ret = inv_mpu_core_enable_regulator_vddio(st); + if (ret) + return ret; + + return inv_mpu6050_set_power_itg(st, true); +} + +const struct dev_pm_ops inv_mpu_pmops = { + SET_SYSTEM_SLEEP_PM_OPS(inv_mpu_suspend, inv_mpu_resume) + SET_RUNTIME_PM_OPS(inv_mpu_runtime_suspend, inv_mpu_runtime_resume, NULL) +}; +EXPORT_SYMBOL_GPL(inv_mpu_pmops); + +MODULE_AUTHOR("Invensense Corporation"); +MODULE_DESCRIPTION("Invensense device MPU6050 driver"); +MODULE_LICENSE("GPL"); diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c new file mode 100644 index 000000000..14255a918 --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c @@ -0,0 +1,278 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* +* Copyright (C) 2012 Invensense, Inc. +*/ + +#include +#include +#include +#include +#include +#include +#include + +#include "inv_mpu_iio.h" + +static const struct regmap_config inv_mpu_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int inv_mpu6050_select_bypass(struct i2c_mux_core *muxc, u32 chan_id) +{ + return 0; +} + +static bool inv_mpu_i2c_aux_bus(struct device *dev) +{ + struct inv_mpu6050_state *st = iio_priv(dev_get_drvdata(dev)); + + switch (st->chip_type) { + case INV_ICM20608: + case INV_ICM20608D: + case INV_ICM20609: + case INV_ICM20689: + case INV_ICM20602: + case INV_IAM20680: + /* no i2c auxiliary bus on the chip */ + return false; + case INV_MPU9150: + case INV_MPU9250: + case INV_MPU9255: + if (st->magn_disabled) + return true; + else + return false; + default: + return true; + } +} + +static int inv_mpu_i2c_aux_setup(struct iio_dev *indio_dev) +{ + struct inv_mpu6050_state *st = iio_priv(indio_dev); + struct device *dev = indio_dev->dev.parent; + struct fwnode_handle *mux_node; + int ret; + + /* + * MPU9xxx magnetometer support requires to disable i2c auxiliary bus. + * To ensure backward compatibility with existing setups, do not disable + * i2c auxiliary bus if it used. + * Check for i2c-gate node in devicetree and set magnetometer disabled. + * Only MPU6500 is supported by ACPI, no need to check. + */ + switch (st->chip_type) { + case INV_MPU9150: + case INV_MPU9250: + case INV_MPU9255: + mux_node = device_get_named_child_node(dev, "i2c-gate"); + if (mux_node != NULL) { + st->magn_disabled = true; + dev_warn(dev, "disable internal use of magnetometer\n"); + } + fwnode_handle_put(mux_node); + break; + default: + break; + } + + /* enable i2c bypass when using i2c auxiliary bus */ + if (inv_mpu_i2c_aux_bus(dev)) { + ret = regmap_write(st->map, st->reg->int_pin_cfg, + st->irq_mask | INV_MPU6050_BIT_BYPASS_EN); + if (ret) + return ret; + } + + return 0; +} + +/** + * inv_mpu_probe() - probe function. + * @client: i2c client. + * @id: i2c device id. + * + * Returns 0 on success, a negative error code otherwise. + */ +static int inv_mpu_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + const void *match; + struct inv_mpu6050_state *st; + int result; + enum inv_devices chip_type; + struct regmap *regmap; + const char *name; + + if (!i2c_check_functionality(client->adapter, + I2C_FUNC_SMBUS_I2C_BLOCK)) + return -EOPNOTSUPP; + + match = device_get_match_data(&client->dev); + if (match) { + chip_type = (uintptr_t)match; + name = client->name; + } else if (id) { + chip_type = (enum inv_devices) + id->driver_data; + name = id->name; + } else { + return -ENOSYS; + } + + regmap = devm_regmap_init_i2c(client, &inv_mpu_regmap_config); + if (IS_ERR(regmap)) { + dev_err(&client->dev, "Failed to register i2c regmap: %pe\n", + regmap); + return PTR_ERR(regmap); + } + + result = inv_mpu_core_probe(regmap, client->irq, name, + inv_mpu_i2c_aux_setup, chip_type); + if (result < 0) + return result; + + st = iio_priv(dev_get_drvdata(&client->dev)); + if (inv_mpu_i2c_aux_bus(&client->dev)) { + /* declare i2c auxiliary bus */ + st->muxc = i2c_mux_alloc(client->adapter, &client->dev, + 1, 0, I2C_MUX_LOCKED | I2C_MUX_GATE, + inv_mpu6050_select_bypass, NULL); + if (!st->muxc) + return -ENOMEM; + st->muxc->priv = dev_get_drvdata(&client->dev); + result = i2c_mux_add_adapter(st->muxc, 0, 0, 0); + if (result) + return result; + result = inv_mpu_acpi_create_mux_client(client); + if (result) + goto out_del_mux; + } + + return 0; + +out_del_mux: + i2c_mux_del_adapters(st->muxc); + return result; +} + +static void inv_mpu_remove(struct i2c_client *client) +{ + struct iio_dev *indio_dev = i2c_get_clientdata(client); + struct inv_mpu6050_state *st = iio_priv(indio_dev); + + if (st->muxc) { + inv_mpu_acpi_delete_mux_client(client); + i2c_mux_del_adapters(st->muxc); + } +} + +/* + * device id table is used to identify what device can be + * supported by this driver + */ +static const struct i2c_device_id inv_mpu_id[] = { + {"mpu6050", INV_MPU6050}, + {"mpu6500", INV_MPU6500}, + {"mpu6515", INV_MPU6515}, + {"mpu6880", INV_MPU6880}, + {"mpu9150", INV_MPU9150}, + {"mpu9250", INV_MPU9250}, + {"mpu9255", INV_MPU9255}, + {"icm20608", INV_ICM20608}, + {"icm20608d", INV_ICM20608D}, + {"icm20609", INV_ICM20609}, + {"icm20689", INV_ICM20689}, + {"icm20602", INV_ICM20602}, + {"icm20690", INV_ICM20690}, + {"iam20680", INV_IAM20680}, + {} +}; + +MODULE_DEVICE_TABLE(i2c, inv_mpu_id); + +static const struct of_device_id inv_of_match[] = { + { + .compatible = "invensense,mpu6050", + .data = (void *)INV_MPU6050 + }, + { + .compatible = "invensense,mpu6500", + .data = (void *)INV_MPU6500 + }, + { + .compatible = "invensense,mpu6515", + .data = (void *)INV_MPU6515 + }, + { + .compatible = "invensense,mpu6880", + .data = (void *)INV_MPU6880 + }, + { + .compatible = "invensense,mpu9150", + .data = (void *)INV_MPU9150 + }, + { + .compatible = "invensense,mpu9250", + .data = (void *)INV_MPU9250 + }, + { + .compatible = "invensense,mpu9255", + .data = (void *)INV_MPU9255 + }, + { + .compatible = "invensense,icm20608", + .data = (void *)INV_ICM20608 + }, + { + .compatible = "invensense,icm20608d", + .data = (void *)INV_ICM20608D + }, + { + .compatible = "invensense,icm20609", + .data = (void *)INV_ICM20609 + }, + { + .compatible = "invensense,icm20689", + .data = (void *)INV_ICM20689 + }, + { + .compatible = "invensense,icm20602", + .data = (void *)INV_ICM20602 + }, + { + .compatible = "invensense,icm20690", + .data = (void *)INV_ICM20690 + }, + { + .compatible = "invensense,iam20680", + .data = (void *)INV_IAM20680 + }, + { } +}; +MODULE_DEVICE_TABLE(of, inv_of_match); + +static const struct acpi_device_id inv_acpi_match[] = { + {"INVN6500", INV_MPU6500}, + { }, +}; +MODULE_DEVICE_TABLE(acpi, inv_acpi_match); + +static struct i2c_driver inv_mpu_driver = { + .probe = inv_mpu_probe, + .remove = inv_mpu_remove, + .id_table = inv_mpu_id, + .driver = { + .of_match_table = inv_of_match, + .acpi_match_table = inv_acpi_match, + .name = "inv-mpu6050-i2c", + .pm = &inv_mpu_pmops, + }, +}; + +module_i2c_driver(inv_mpu_driver); + +MODULE_AUTHOR("Invensense Corporation"); +MODULE_DESCRIPTION("Invensense device MPU6050 driver"); +MODULE_LICENSE("GPL"); diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h new file mode 100644 index 000000000..94b54c501 --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h @@ -0,0 +1,474 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* +* Copyright (C) 2012 Invensense, Inc. +*/ + +#ifndef INV_MPU_IIO_H_ +#define INV_MPU_IIO_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * struct inv_mpu6050_reg_map - Notable registers. + * @sample_rate_div: Divider applied to gyro output rate. + * @lpf: Configures internal low pass filter. + * @accel_lpf: Configures accelerometer low pass filter. + * @user_ctrl: Enables/resets the FIFO. + * @fifo_en: Determines which data will appear in FIFO. + * @gyro_config: gyro config register. + * @accl_config: accel config register + * @fifo_count_h: Upper byte of FIFO count. + * @fifo_r_w: FIFO register. + * @raw_gyro: Address of first gyro register. + * @raw_accl: Address of first accel register. + * @temperature: temperature register + * @int_enable: Interrupt enable register. + * @int_status: Interrupt status register. + * @pwr_mgmt_1: Controls chip's power state and clock source. + * @pwr_mgmt_2: Controls power state of individual sensors. + * @int_pin_cfg; Controls interrupt pin configuration. + * @accl_offset: Controls the accelerometer calibration offset. + * @gyro_offset: Controls the gyroscope calibration offset. + * @i2c_if: Controls the i2c interface + */ +struct inv_mpu6050_reg_map { + u8 sample_rate_div; + u8 lpf; + u8 accel_lpf; + u8 user_ctrl; + u8 fifo_en; + u8 gyro_config; + u8 accl_config; + u8 fifo_count_h; + u8 fifo_r_w; + u8 raw_gyro; + u8 raw_accl; + u8 temperature; + u8 int_enable; + u8 int_status; + u8 pwr_mgmt_1; + u8 pwr_mgmt_2; + u8 int_pin_cfg; + u8 accl_offset; + u8 gyro_offset; + u8 i2c_if; +}; + +/*device enum */ +enum inv_devices { + INV_MPU6050, + INV_MPU6500, + INV_MPU6515, + INV_MPU6880, + INV_MPU6000, + INV_MPU9150, + INV_MPU9250, + INV_MPU9255, + INV_ICM20608, + INV_ICM20608D, + INV_ICM20609, + INV_ICM20689, + INV_ICM20602, + INV_ICM20690, + INV_IAM20680, + INV_NUM_PARTS +}; + +/* chip sensors mask: accelerometer, gyroscope, temperature, magnetometer */ +#define INV_MPU6050_SENSOR_ACCL BIT(0) +#define INV_MPU6050_SENSOR_GYRO BIT(1) +#define INV_MPU6050_SENSOR_TEMP BIT(2) +#define INV_MPU6050_SENSOR_MAGN BIT(3) + +/** + * struct inv_mpu6050_chip_config - Cached chip configuration data. + * @clk: selected chip clock + * @fsr: Full scale range. + * @lpf: Digital low pass filter frequency. + * @accl_fs: accel full scale range. + * @accl_en: accel engine enabled + * @gyro_en: gyro engine enabled + * @temp_en: temperature sensor enabled + * @magn_en: magn engine (i2c master) enabled + * @accl_fifo_enable: enable accel data output + * @gyro_fifo_enable: enable gyro data output + * @temp_fifo_enable: enable temp data output + * @magn_fifo_enable: enable magn data output + * @divider: chip sample rate divider (sample rate divider - 1) + */ +struct inv_mpu6050_chip_config { + unsigned int clk:3; + unsigned int fsr:2; + unsigned int lpf:3; + unsigned int accl_fs:2; + unsigned int accl_en:1; + unsigned int gyro_en:1; + unsigned int temp_en:1; + unsigned int magn_en:1; + unsigned int accl_fifo_enable:1; + unsigned int gyro_fifo_enable:1; + unsigned int temp_fifo_enable:1; + unsigned int magn_fifo_enable:1; + u8 divider; + u8 user_ctrl; +}; + +/* + * Maximum of 6 + 6 + 2 + 7 (for MPU9x50) = 21 round up to 24 and plus 8. + * May be less if fewer channels are enabled, as long as the timestamp + * remains 8 byte aligned + */ +#define INV_MPU6050_OUTPUT_DATA_SIZE 32 + +/** + * struct inv_mpu6050_hw - Other important hardware information. + * @whoami: Self identification byte from WHO_AM_I register + * @name: name of the chip. + * @reg: register map of the chip. + * @config: configuration of the chip. + * @fifo_size: size of the FIFO in bytes. + * @temp: offset and scale to apply to raw temperature. + */ +struct inv_mpu6050_hw { + u8 whoami; + u8 *name; + const struct inv_mpu6050_reg_map *reg; + const struct inv_mpu6050_chip_config *config; + size_t fifo_size; + struct { + int offset; + int scale; + } temp; + struct { + unsigned int accel; + unsigned int gyro; + } startup_time; +}; + +/* + * struct inv_mpu6050_state - Driver state variables. + * @lock: Chip access lock. + * @trig: IIO trigger. + * @chip_config: Cached attribute information. + * @reg: Map of important registers. + * @hw: Other hardware-specific information. + * @chip_type: chip type. + * @plat_data: platform data (deprecated in favor of @orientation). + * @orientation: sensor chip orientation relative to main hardware. + * @map regmap pointer. + * @irq interrupt number. + * @irq_mask the int_pin_cfg mask to configure interrupt type. + * @chip_period: chip internal period estimation (~1kHz). + * @it_timestamp: timestamp from previous interrupt. + * @data_timestamp: timestamp for next data sample. + * @vdd_supply: VDD voltage regulator for the chip. + * @vddio_supply I/O voltage regulator for the chip. + * @magn_disabled: magnetometer disabled for backward compatibility reason. + * @magn_raw_to_gauss: coefficient to convert mag raw value to Gauss. + * @magn_orient: magnetometer sensor chip orientation if available. + * @suspended_sensors: sensors mask of sensors turned off for suspend + * @data: dma safe buffer used for bulk reads. + */ +struct inv_mpu6050_state { + struct mutex lock; + struct iio_trigger *trig; + struct inv_mpu6050_chip_config chip_config; + const struct inv_mpu6050_reg_map *reg; + const struct inv_mpu6050_hw *hw; + enum inv_devices chip_type; + struct i2c_mux_core *muxc; + struct i2c_client *mux_client; + struct inv_mpu6050_platform_data plat_data; + struct iio_mount_matrix orientation; + struct regmap *map; + int irq; + u8 irq_mask; + unsigned skip_samples; + s64 chip_period; + s64 it_timestamp; + s64 data_timestamp; + struct regulator *vdd_supply; + struct regulator *vddio_supply; + bool magn_disabled; + s32 magn_raw_to_gauss[3]; + struct iio_mount_matrix magn_orient; + unsigned int suspended_sensors; + u8 data[INV_MPU6050_OUTPUT_DATA_SIZE] __aligned(IIO_DMA_MINALIGN); +}; + +/*register and associated bit definition*/ +#define INV_MPU6050_REG_ACCEL_OFFSET 0x06 +#define INV_MPU6050_REG_GYRO_OFFSET 0x13 + +#define INV_MPU6050_REG_SAMPLE_RATE_DIV 0x19 +#define INV_MPU6050_REG_CONFIG 0x1A +#define INV_MPU6050_REG_GYRO_CONFIG 0x1B +#define INV_MPU6050_REG_ACCEL_CONFIG 0x1C + +#define INV_MPU6050_REG_FIFO_EN 0x23 +#define INV_MPU6050_BIT_SLAVE_0 0x01 +#define INV_MPU6050_BIT_SLAVE_1 0x02 +#define INV_MPU6050_BIT_SLAVE_2 0x04 +#define INV_MPU6050_BIT_ACCEL_OUT 0x08 +#define INV_MPU6050_BITS_GYRO_OUT 0x70 +#define INV_MPU6050_BIT_TEMP_OUT 0x80 + +#define INV_MPU6050_REG_I2C_MST_CTRL 0x24 +#define INV_MPU6050_BITS_I2C_MST_CLK_400KHZ 0x0D +#define INV_MPU6050_BIT_I2C_MST_P_NSR 0x10 +#define INV_MPU6050_BIT_SLV3_FIFO_EN 0x20 +#define INV_MPU6050_BIT_WAIT_FOR_ES 0x40 +#define INV_MPU6050_BIT_MULT_MST_EN 0x80 + +/* control I2C slaves from 0 to 3 */ +#define INV_MPU6050_REG_I2C_SLV_ADDR(_x) (0x25 + 3 * (_x)) +#define INV_MPU6050_BIT_I2C_SLV_RNW 0x80 + +#define INV_MPU6050_REG_I2C_SLV_REG(_x) (0x26 + 3 * (_x)) + +#define INV_MPU6050_REG_I2C_SLV_CTRL(_x) (0x27 + 3 * (_x)) +#define INV_MPU6050_BIT_SLV_GRP 0x10 +#define INV_MPU6050_BIT_SLV_REG_DIS 0x20 +#define INV_MPU6050_BIT_SLV_BYTE_SW 0x40 +#define INV_MPU6050_BIT_SLV_EN 0x80 + +/* I2C master delay register */ +#define INV_MPU6050_REG_I2C_SLV4_CTRL 0x34 +#define INV_MPU6050_BITS_I2C_MST_DLY(_x) ((_x) & 0x1F) + +#define INV_MPU6050_REG_I2C_MST_STATUS 0x36 +#define INV_MPU6050_BIT_I2C_SLV0_NACK 0x01 +#define INV_MPU6050_BIT_I2C_SLV1_NACK 0x02 +#define INV_MPU6050_BIT_I2C_SLV2_NACK 0x04 +#define INV_MPU6050_BIT_I2C_SLV3_NACK 0x08 + +#define INV_MPU6050_REG_INT_ENABLE 0x38 +#define INV_MPU6050_BIT_DATA_RDY_EN 0x01 +#define INV_MPU6050_BIT_DMP_INT_EN 0x02 + +#define INV_MPU6050_REG_RAW_ACCEL 0x3B +#define INV_MPU6050_REG_TEMPERATURE 0x41 +#define INV_MPU6050_REG_RAW_GYRO 0x43 + +#define INV_MPU6050_REG_INT_STATUS 0x3A +#define INV_MPU6050_BIT_FIFO_OVERFLOW_INT 0x10 +#define INV_MPU6050_BIT_RAW_DATA_RDY_INT 0x01 + +#define INV_MPU6050_REG_EXT_SENS_DATA 0x49 + +/* I2C slaves data output from 0 to 3 */ +#define INV_MPU6050_REG_I2C_SLV_DO(_x) (0x63 + (_x)) + +#define INV_MPU6050_REG_I2C_MST_DELAY_CTRL 0x67 +#define INV_MPU6050_BIT_I2C_SLV0_DLY_EN 0x01 +#define INV_MPU6050_BIT_I2C_SLV1_DLY_EN 0x02 +#define INV_MPU6050_BIT_I2C_SLV2_DLY_EN 0x04 +#define INV_MPU6050_BIT_I2C_SLV3_DLY_EN 0x08 +#define INV_MPU6050_BIT_DELAY_ES_SHADOW 0x80 + +#define INV_MPU6050_REG_SIGNAL_PATH_RESET 0x68 +#define INV_MPU6050_BIT_TEMP_RST BIT(0) +#define INV_MPU6050_BIT_ACCEL_RST BIT(1) +#define INV_MPU6050_BIT_GYRO_RST BIT(2) + +#define INV_MPU6050_REG_USER_CTRL 0x6A +#define INV_MPU6050_BIT_SIG_COND_RST 0x01 +#define INV_MPU6050_BIT_FIFO_RST 0x04 +#define INV_MPU6050_BIT_DMP_RST 0x08 +#define INV_MPU6050_BIT_I2C_MST_EN 0x20 +#define INV_MPU6050_BIT_FIFO_EN 0x40 +#define INV_MPU6050_BIT_DMP_EN 0x80 +#define INV_MPU6050_BIT_I2C_IF_DIS 0x10 + +#define INV_MPU6050_REG_PWR_MGMT_1 0x6B +#define INV_MPU6050_BIT_H_RESET 0x80 +#define INV_MPU6050_BIT_SLEEP 0x40 +#define INV_MPU6050_BIT_TEMP_DIS 0x08 +#define INV_MPU6050_BIT_CLK_MASK 0x7 + +#define INV_MPU6050_REG_PWR_MGMT_2 0x6C +#define INV_MPU6050_BIT_PWR_ACCL_STBY 0x38 +#define INV_MPU6050_BIT_PWR_GYRO_STBY 0x07 + +/* ICM20602 register */ +#define INV_ICM20602_REG_I2C_IF 0x70 +#define INV_ICM20602_BIT_I2C_IF_DIS 0x40 + +#define INV_MPU6050_REG_FIFO_COUNT_H 0x72 +#define INV_MPU6050_REG_FIFO_R_W 0x74 + +#define INV_MPU6050_BYTES_PER_3AXIS_SENSOR 6 +#define INV_MPU6050_FIFO_COUNT_BYTE 2 + +/* MPU9X50 9-axis magnetometer */ +#define INV_MPU9X50_BYTES_MAGN 7 + +/* FIFO temperature sample size */ +#define INV_MPU6050_BYTES_PER_TEMP_SENSOR 2 + +/* mpu6500 registers */ +#define INV_MPU6500_REG_ACCEL_CONFIG_2 0x1D +#define INV_ICM20689_BITS_FIFO_SIZE_MAX 0xC0 +#define INV_MPU6500_REG_ACCEL_OFFSET 0x77 + +/* delay time in milliseconds */ +#define INV_MPU6050_POWER_UP_TIME 100 +#define INV_MPU6050_TEMP_UP_TIME 100 +#define INV_MPU6050_ACCEL_STARTUP_TIME 20 +#define INV_MPU6050_GYRO_STARTUP_TIME 60 +#define INV_MPU6050_GYRO_DOWN_TIME 150 +#define INV_MPU6050_SUSPEND_DELAY_MS 2000 + +#define INV_MPU6500_GYRO_STARTUP_TIME 70 +#define INV_MPU6500_ACCEL_STARTUP_TIME 30 + +#define INV_ICM20602_GYRO_STARTUP_TIME 100 +#define INV_ICM20602_ACCEL_STARTUP_TIME 20 + +#define INV_ICM20690_GYRO_STARTUP_TIME 80 +#define INV_ICM20690_ACCEL_STARTUP_TIME 10 + + +/* delay time in microseconds */ +#define INV_MPU6050_REG_UP_TIME_MIN 5000 +#define INV_MPU6050_REG_UP_TIME_MAX 10000 + +#define INV_MPU6050_TEMP_OFFSET 12420 +#define INV_MPU6050_TEMP_SCALE 2941176 +#define INV_MPU6050_MAX_GYRO_FS_PARAM 3 +#define INV_MPU6050_MAX_ACCL_FS_PARAM 3 +#define INV_MPU6050_THREE_AXIS 3 +#define INV_MPU6050_GYRO_CONFIG_FSR_SHIFT 3 +#define INV_ICM20690_GYRO_CONFIG_FSR_SHIFT 2 +#define INV_MPU6050_ACCL_CONFIG_FSR_SHIFT 3 + +#define INV_MPU6500_TEMP_OFFSET 7011 +#define INV_MPU6500_TEMP_SCALE 2995178 + +#define INV_ICM20608_TEMP_OFFSET 8170 +#define INV_ICM20608_TEMP_SCALE 3059976 + +#define INV_MPU6050_REG_INT_PIN_CFG 0x37 +#define INV_MPU6050_ACTIVE_HIGH 0x00 +#define INV_MPU6050_ACTIVE_LOW 0x80 +/* enable level triggering */ +#define INV_MPU6050_LATCH_INT_EN 0x20 +#define INV_MPU6050_BIT_BYPASS_EN 0x2 + +/* Allowed timestamp period jitter in percent */ +#define INV_MPU6050_TS_PERIOD_JITTER 4 + +/* init parameters */ +#define INV_MPU6050_MAX_FIFO_RATE 1000 +#define INV_MPU6050_MIN_FIFO_RATE 4 + +/* chip internal frequency: 1KHz */ +#define INV_MPU6050_INTERNAL_FREQ_HZ 1000 +/* return the frequency divider (chip sample rate divider + 1) */ +#define INV_MPU6050_FREQ_DIVIDER(st) \ + ((st)->chip_config.divider + 1) +/* chip sample rate divider to fifo rate */ +#define INV_MPU6050_FIFO_RATE_TO_DIVIDER(fifo_rate) \ + ((INV_MPU6050_INTERNAL_FREQ_HZ / (fifo_rate)) - 1) +#define INV_MPU6050_DIVIDER_TO_FIFO_RATE(divider) \ + (INV_MPU6050_INTERNAL_FREQ_HZ / ((divider) + 1)) + +#define INV_MPU6050_REG_WHOAMI 117 + +#define INV_MPU6000_WHOAMI_VALUE 0x68 +#define INV_MPU6050_WHOAMI_VALUE 0x68 +#define INV_MPU6500_WHOAMI_VALUE 0x70 +#define INV_MPU6880_WHOAMI_VALUE 0x78 +#define INV_MPU9150_WHOAMI_VALUE 0x68 +#define INV_MPU9250_WHOAMI_VALUE 0x71 +#define INV_MPU9255_WHOAMI_VALUE 0x73 +#define INV_MPU6515_WHOAMI_VALUE 0x74 +#define INV_ICM20608_WHOAMI_VALUE 0xAF +#define INV_ICM20608D_WHOAMI_VALUE 0xAE +#define INV_ICM20609_WHOAMI_VALUE 0xA6 +#define INV_ICM20689_WHOAMI_VALUE 0x98 +#define INV_ICM20602_WHOAMI_VALUE 0x12 +#define INV_ICM20690_WHOAMI_VALUE 0x20 +#define INV_IAM20680_WHOAMI_VALUE 0xA9 + +/* scan element definition for generic MPU6xxx devices */ +enum inv_mpu6050_scan { + INV_MPU6050_SCAN_ACCL_X, + INV_MPU6050_SCAN_ACCL_Y, + INV_MPU6050_SCAN_ACCL_Z, + INV_MPU6050_SCAN_TEMP, + INV_MPU6050_SCAN_GYRO_X, + INV_MPU6050_SCAN_GYRO_Y, + INV_MPU6050_SCAN_GYRO_Z, + INV_MPU6050_SCAN_TIMESTAMP, + + INV_MPU9X50_SCAN_MAGN_X = INV_MPU6050_SCAN_GYRO_Z + 1, + INV_MPU9X50_SCAN_MAGN_Y, + INV_MPU9X50_SCAN_MAGN_Z, + INV_MPU9X50_SCAN_TIMESTAMP, +}; + +enum inv_mpu6050_filter_e { + INV_MPU6050_FILTER_NOLPF2 = 0, + INV_MPU6050_FILTER_200HZ, + INV_MPU6050_FILTER_100HZ, + INV_MPU6050_FILTER_45HZ, + INV_MPU6050_FILTER_20HZ, + INV_MPU6050_FILTER_10HZ, + INV_MPU6050_FILTER_5HZ, + INV_MPU6050_FILTER_NOLPF, + NUM_MPU6050_FILTER +}; + +/* IIO attribute address */ +enum INV_MPU6050_IIO_ATTR_ADDR { + ATTR_GYRO_MATRIX, + ATTR_ACCL_MATRIX, +}; + +enum inv_mpu6050_accl_fs_e { + INV_MPU6050_FS_02G = 0, + INV_MPU6050_FS_04G, + INV_MPU6050_FS_08G, + INV_MPU6050_FS_16G, + NUM_ACCL_FSR +}; + +enum inv_mpu6050_fsr_e { + INV_MPU6050_FSR_250DPS = 0, + INV_MPU6050_FSR_500DPS, + INV_MPU6050_FSR_1000DPS, + INV_MPU6050_FSR_2000DPS, + NUM_MPU6050_FSR +}; + +enum inv_mpu6050_clock_sel_e { + INV_CLK_INTERNAL = 0, + INV_CLK_PLL, + NUM_CLK +}; + +irqreturn_t inv_mpu6050_read_fifo(int irq, void *p); +int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type); +int inv_mpu6050_prepare_fifo(struct inv_mpu6050_state *st, bool enable); +int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, + unsigned int mask); +int inv_mpu6050_write_reg(struct inv_mpu6050_state *st, int reg, u8 val); +int inv_mpu_acpi_create_mux_client(struct i2c_client *client); +void inv_mpu_acpi_delete_mux_client(struct i2c_client *client); +int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name, + int (*inv_mpu_bus_setup)(struct iio_dev *), int chip_type); +extern const struct dev_pm_ops inv_mpu_pmops; + +#endif diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c new file mode 100644 index 000000000..6aee6c989 --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c @@ -0,0 +1,365 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Copyright (C) 2019 TDK-InvenSense, Inc. + */ + +#include +#include +#include + +#include "inv_mpu_aux.h" +#include "inv_mpu_iio.h" +#include "inv_mpu_magn.h" + +/* + * MPU9xxx magnetometer are AKM chips on I2C aux bus + * MPU9150 is AK8975 + * MPU9250 is AK8963 + */ +#define INV_MPU_MAGN_I2C_ADDR 0x0C + +#define INV_MPU_MAGN_REG_WIA 0x00 +#define INV_MPU_MAGN_BITS_WIA 0x48 + +#define INV_MPU_MAGN_REG_ST1 0x02 +#define INV_MPU_MAGN_BIT_DRDY 0x01 +#define INV_MPU_MAGN_BIT_DOR 0x02 + +#define INV_MPU_MAGN_REG_DATA 0x03 + +#define INV_MPU_MAGN_REG_ST2 0x09 +#define INV_MPU_MAGN_BIT_HOFL 0x08 +#define INV_MPU_MAGN_BIT_BITM 0x10 + +#define INV_MPU_MAGN_REG_CNTL1 0x0A +#define INV_MPU_MAGN_BITS_MODE_PWDN 0x00 +#define INV_MPU_MAGN_BITS_MODE_SINGLE 0x01 +#define INV_MPU_MAGN_BITS_MODE_FUSE 0x0F +#define INV_MPU9250_MAGN_BIT_OUTPUT_BIT 0x10 + +#define INV_MPU9250_MAGN_REG_CNTL2 0x0B +#define INV_MPU9250_MAGN_BIT_SRST 0x01 + +#define INV_MPU_MAGN_REG_ASAX 0x10 +#define INV_MPU_MAGN_REG_ASAY 0x11 +#define INV_MPU_MAGN_REG_ASAZ 0x12 + +static bool inv_magn_supported(const struct inv_mpu6050_state *st) +{ + switch (st->chip_type) { + case INV_MPU9150: + case INV_MPU9250: + case INV_MPU9255: + return true; + default: + return false; + } +} + +/* init magnetometer chip */ +static int inv_magn_init(struct inv_mpu6050_state *st) +{ + uint8_t val; + uint8_t asa[3]; + int32_t sensitivity; + int ret; + + /* check whoami */ + ret = inv_mpu_aux_read(st, INV_MPU_MAGN_I2C_ADDR, INV_MPU_MAGN_REG_WIA, + &val, sizeof(val)); + if (ret) + return ret; + if (val != INV_MPU_MAGN_BITS_WIA) + return -ENODEV; + + /* software reset for MPU925x only */ + switch (st->chip_type) { + case INV_MPU9250: + case INV_MPU9255: + ret = inv_mpu_aux_write(st, INV_MPU_MAGN_I2C_ADDR, + INV_MPU9250_MAGN_REG_CNTL2, + INV_MPU9250_MAGN_BIT_SRST); + if (ret) + return ret; + break; + default: + break; + } + + /* read fuse ROM data */ + ret = inv_mpu_aux_write(st, INV_MPU_MAGN_I2C_ADDR, + INV_MPU_MAGN_REG_CNTL1, + INV_MPU_MAGN_BITS_MODE_FUSE); + if (ret) + return ret; + + ret = inv_mpu_aux_read(st, INV_MPU_MAGN_I2C_ADDR, INV_MPU_MAGN_REG_ASAX, + asa, sizeof(asa)); + if (ret) + return ret; + + /* switch back to power-down */ + ret = inv_mpu_aux_write(st, INV_MPU_MAGN_I2C_ADDR, + INV_MPU_MAGN_REG_CNTL1, + INV_MPU_MAGN_BITS_MODE_PWDN); + if (ret) + return ret; + + /* + * Sensor sentivity + * 1 uT = 0.01 G and value is in micron (1e6) + * sensitvity = x uT * 0.01 * 1e6 + */ + switch (st->chip_type) { + case INV_MPU9150: + /* sensor sensitivity is 0.3 uT */ + sensitivity = 3000; + break; + case INV_MPU9250: + case INV_MPU9255: + /* sensor sensitivity in 16 bits mode: 0.15 uT */ + sensitivity = 1500; + break; + default: + return -EINVAL; + } + + /* + * Sensitivity adjustement and scale to Gauss + * + * Hadj = H * (((ASA - 128) * 0.5 / 128) + 1) + * Factor simplification: + * Hadj = H * ((ASA + 128) / 256) + * + * raw_to_gauss = Hadj * sensitivity + */ + st->magn_raw_to_gauss[0] = (((int32_t)asa[0] + 128) * sensitivity) / 256; + st->magn_raw_to_gauss[1] = (((int32_t)asa[1] + 128) * sensitivity) / 256; + st->magn_raw_to_gauss[2] = (((int32_t)asa[2] + 128) * sensitivity) / 256; + + return 0; +} + +/** + * inv_mpu_magn_probe() - probe and setup magnetometer chip + * @st: driver internal state + * + * Returns 0 on success, a negative error code otherwise + * + * It is probing the chip and setting up all needed i2c transfers. + * Noop if there is no magnetometer in the chip. + */ +int inv_mpu_magn_probe(struct inv_mpu6050_state *st) +{ + uint8_t val; + int ret; + + /* quit if chip is not supported */ + if (!inv_magn_supported(st)) + return 0; + + /* configure i2c master aux port */ + ret = inv_mpu_aux_init(st); + if (ret) + return ret; + + /* check and init mag chip */ + ret = inv_magn_init(st); + if (ret) + return ret; + + /* + * configure mpu i2c master accesses + * i2c SLV0: read sensor data, 7 bytes data(6)-ST2 + * Byte swap data to store them in big-endian in impair address groups + */ + ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_ADDR(0), + INV_MPU6050_BIT_I2C_SLV_RNW | INV_MPU_MAGN_I2C_ADDR); + if (ret) + return ret; + + ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_REG(0), + INV_MPU_MAGN_REG_DATA); + if (ret) + return ret; + + ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0), + INV_MPU6050_BIT_SLV_EN | + INV_MPU6050_BIT_SLV_BYTE_SW | + INV_MPU6050_BIT_SLV_GRP | + INV_MPU9X50_BYTES_MAGN); + if (ret) + return ret; + + /* i2c SLV1: launch single measurement */ + ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_ADDR(1), + INV_MPU_MAGN_I2C_ADDR); + if (ret) + return ret; + + ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_REG(1), + INV_MPU_MAGN_REG_CNTL1); + if (ret) + return ret; + + /* add 16 bits mode for MPU925x */ + val = INV_MPU_MAGN_BITS_MODE_SINGLE; + switch (st->chip_type) { + case INV_MPU9250: + case INV_MPU9255: + val |= INV_MPU9250_MAGN_BIT_OUTPUT_BIT; + break; + default: + break; + } + ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_DO(1), val); + if (ret) + return ret; + + return regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(1), + INV_MPU6050_BIT_SLV_EN | 1); +} + +/** + * inv_mpu_magn_set_rate() - set magnetometer sampling rate + * @st: driver internal state + * @fifo_rate: mpu set fifo rate + * + * Returns 0 on success, a negative error code otherwise + * + * Limit sampling frequency to the maximum value supported by the + * magnetometer chip. Resulting in duplicated data for higher frequencies. + * Noop if there is no magnetometer in the chip. + */ +int inv_mpu_magn_set_rate(const struct inv_mpu6050_state *st, int fifo_rate) +{ + uint8_t d; + + /* quit if chip is not supported */ + if (!inv_magn_supported(st)) + return 0; + + /* + * update i2c master delay to limit mag sampling to max frequency + * compute fifo_rate divider d: rate = fifo_rate / (d + 1) + */ + if (fifo_rate > INV_MPU_MAGN_FREQ_HZ_MAX) + d = fifo_rate / INV_MPU_MAGN_FREQ_HZ_MAX - 1; + else + d = 0; + + return regmap_write(st->map, INV_MPU6050_REG_I2C_SLV4_CTRL, d); +} + +/** + * inv_mpu_magn_set_orient() - fill magnetometer mounting matrix + * @st: driver internal state + * + * Returns 0 on success, a negative error code otherwise + * + * Fill magnetometer mounting matrix using the provided chip matrix. + */ +int inv_mpu_magn_set_orient(struct inv_mpu6050_state *st) +{ + struct device *dev = regmap_get_device(st->map); + const char *orient; + char *str; + int i; + + /* fill magnetometer orientation */ + switch (st->chip_type) { + case INV_MPU9150: + case INV_MPU9250: + case INV_MPU9255: + /* x <- y */ + st->magn_orient.rotation[0] = st->orientation.rotation[3]; + st->magn_orient.rotation[1] = st->orientation.rotation[4]; + st->magn_orient.rotation[2] = st->orientation.rotation[5]; + /* y <- x */ + st->magn_orient.rotation[3] = st->orientation.rotation[0]; + st->magn_orient.rotation[4] = st->orientation.rotation[1]; + st->magn_orient.rotation[5] = st->orientation.rotation[2]; + /* z <- -z */ + for (i = 6; i < 9; ++i) { + orient = st->orientation.rotation[i]; + + /* + * The value is negated according to one of the following + * rules: + * + * 1) Drop leading minus. + * 2) Leave 0 as is. + * 3) Add leading minus. + */ + if (orient[0] == '-') + str = devm_kstrdup(dev, orient + 1, GFP_KERNEL); + else if (!strcmp(orient, "0")) + str = devm_kstrdup(dev, orient, GFP_KERNEL); + else + str = devm_kasprintf(dev, GFP_KERNEL, "-%s", orient); + if (!str) + return -ENOMEM; + + st->magn_orient.rotation[i] = str; + } + break; + default: + st->magn_orient = st->orientation; + break; + } + + return 0; +} + +/** + * inv_mpu_magn_read() - read magnetometer data + * @st: driver internal state + * @axis: IIO modifier axis value + * @val: store corresponding axis value + * + * Returns 0 on success, a negative error code otherwise + */ +int inv_mpu_magn_read(struct inv_mpu6050_state *st, int axis, int *val) +{ + unsigned int status; + __be16 data; + uint8_t addr; + int ret; + + /* quit if chip is not supported */ + if (!inv_magn_supported(st)) + return -ENODEV; + + /* Mag data: XH,XL,YH,YL,ZH,ZL */ + switch (axis) { + case IIO_MOD_X: + addr = 0; + break; + case IIO_MOD_Y: + addr = 2; + break; + case IIO_MOD_Z: + addr = 4; + break; + default: + return -EINVAL; + } + addr += INV_MPU6050_REG_EXT_SENS_DATA; + + /* check i2c status and read raw data */ + ret = regmap_read(st->map, INV_MPU6050_REG_I2C_MST_STATUS, &status); + if (ret) + return ret; + + if (status & INV_MPU6050_BIT_I2C_SLV0_NACK || + status & INV_MPU6050_BIT_I2C_SLV1_NACK) + return -EIO; + + ret = regmap_bulk_read(st->map, addr, &data, sizeof(data)); + if (ret) + return ret; + + *val = (int16_t)be16_to_cpu(data); + + return IIO_VAL_INT; +} diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h new file mode 100644 index 000000000..185c000c6 --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h @@ -0,0 +1,39 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +/* + * Copyright (C) 2019 TDK-InvenSense, Inc. + */ + +#ifndef INV_MPU_MAGN_H_ +#define INV_MPU_MAGN_H_ + +#include + +#include "inv_mpu_iio.h" + +/* Magnetometer maximum frequency */ +#define INV_MPU_MAGN_FREQ_HZ_MAX 50 + +int inv_mpu_magn_probe(struct inv_mpu6050_state *st); + +/** + * inv_mpu_magn_get_scale() - get magnetometer scale value + * @st: driver internal state + * + * Returns IIO data format. + */ +static inline int inv_mpu_magn_get_scale(const struct inv_mpu6050_state *st, + const struct iio_chan_spec *chan, + int *val, int *val2) +{ + *val = 0; + *val2 = st->magn_raw_to_gauss[chan->address]; + return IIO_VAL_INT_PLUS_MICRO; +} + +int inv_mpu_magn_set_rate(const struct inv_mpu6050_state *st, int fifo_rate); + +int inv_mpu_magn_set_orient(struct inv_mpu6050_state *st); + +int inv_mpu_magn_read(struct inv_mpu6050_state *st, int axis, int *val); + +#endif /* INV_MPU_MAGN_H_ */ diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c new file mode 100644 index 000000000..45c37525c --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c @@ -0,0 +1,208 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* +* Copyright (C) 2012 Invensense, Inc. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "inv_mpu_iio.h" + +/** + * inv_mpu6050_update_period() - Update chip internal period estimation + * + * @st: driver state + * @timestamp: the interrupt timestamp + * @nb: number of data set in the fifo + * + * This function uses interrupt timestamps to estimate the chip period and + * to choose the data timestamp to come. + */ +static void inv_mpu6050_update_period(struct inv_mpu6050_state *st, + s64 timestamp, size_t nb) +{ + /* Period boundaries for accepting timestamp */ + const s64 period_min = + (NSEC_PER_MSEC * (100 - INV_MPU6050_TS_PERIOD_JITTER)) / 100; + const s64 period_max = + (NSEC_PER_MSEC * (100 + INV_MPU6050_TS_PERIOD_JITTER)) / 100; + const s32 divider = INV_MPU6050_FREQ_DIVIDER(st); + s64 delta, interval; + bool use_it_timestamp = false; + + if (st->it_timestamp == 0) { + /* not initialized, forced to use it_timestamp */ + use_it_timestamp = true; + } else if (nb == 1) { + /* + * Validate the use of it timestamp by checking if interrupt + * has been delayed. + * nb > 1 means interrupt was delayed for more than 1 sample, + * so it's obviously not good. + * Compute the chip period between 2 interrupts for validating. + */ + delta = div_s64(timestamp - st->it_timestamp, divider); + if (delta > period_min && delta < period_max) { + /* update chip period and use it timestamp */ + st->chip_period = (st->chip_period + delta) / 2; + use_it_timestamp = true; + } + } + + if (use_it_timestamp) { + /* + * Manage case of multiple samples in the fifo (nb > 1): + * compute timestamp corresponding to the first sample using + * estimated chip period. + */ + interval = (nb - 1) * st->chip_period * divider; + st->data_timestamp = timestamp - interval; + } + + /* save it timestamp */ + st->it_timestamp = timestamp; +} + +/** + * inv_mpu6050_get_timestamp() - Return the current data timestamp + * + * @st: driver state + * @return: current data timestamp + * + * This function returns the current data timestamp and prepares for next one. + */ +static s64 inv_mpu6050_get_timestamp(struct inv_mpu6050_state *st) +{ + s64 ts; + + /* return current data timestamp and increment */ + ts = st->data_timestamp; + st->data_timestamp += st->chip_period * INV_MPU6050_FREQ_DIVIDER(st); + + return ts; +} + +static int inv_reset_fifo(struct iio_dev *indio_dev) +{ + int result; + struct inv_mpu6050_state *st = iio_priv(indio_dev); + + /* disable fifo and reenable it */ + inv_mpu6050_prepare_fifo(st, false); + result = inv_mpu6050_prepare_fifo(st, true); + if (result) + goto reset_fifo_fail; + + return 0; + +reset_fifo_fail: + dev_err(regmap_get_device(st->map), "reset fifo failed %d\n", result); + result = regmap_write(st->map, st->reg->int_enable, + INV_MPU6050_BIT_DATA_RDY_EN); + + return result; +} + +/* + * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO. + */ +irqreturn_t inv_mpu6050_read_fifo(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + struct inv_mpu6050_state *st = iio_priv(indio_dev); + size_t bytes_per_datum; + int result; + u16 fifo_count; + s64 timestamp; + int int_status; + size_t i, nb; + + mutex_lock(&st->lock); + + /* ack interrupt and check status */ + result = regmap_read(st->map, st->reg->int_status, &int_status); + if (result) { + dev_err(regmap_get_device(st->map), + "failed to ack interrupt\n"); + goto flush_fifo; + } + if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT)) + goto end_session; + + if (!(st->chip_config.accl_fifo_enable | + st->chip_config.gyro_fifo_enable | + st->chip_config.magn_fifo_enable)) + goto end_session; + bytes_per_datum = 0; + if (st->chip_config.accl_fifo_enable) + bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR; + + if (st->chip_config.gyro_fifo_enable) + bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR; + + if (st->chip_config.temp_fifo_enable) + bytes_per_datum += INV_MPU6050_BYTES_PER_TEMP_SENSOR; + + if (st->chip_config.magn_fifo_enable) + bytes_per_datum += INV_MPU9X50_BYTES_MAGN; + + /* + * read fifo_count register to know how many bytes are inside the FIFO + * right now + */ + result = regmap_bulk_read(st->map, st->reg->fifo_count_h, + st->data, INV_MPU6050_FIFO_COUNT_BYTE); + if (result) + goto end_session; + fifo_count = be16_to_cpup((__be16 *)&st->data[0]); + + /* + * Handle fifo overflow by resetting fifo. + * Reset if there is only 3 data set free remaining to mitigate + * possible delay between reading fifo count and fifo data. + */ + nb = 3 * bytes_per_datum; + if (fifo_count >= st->hw->fifo_size - nb) { + dev_warn(regmap_get_device(st->map), "fifo overflow reset\n"); + goto flush_fifo; + } + + /* compute and process all complete datum */ + nb = fifo_count / bytes_per_datum; + inv_mpu6050_update_period(st, pf->timestamp, nb); + for (i = 0; i < nb; ++i) { + result = regmap_noinc_read(st->map, st->reg->fifo_r_w, + st->data, bytes_per_datum); + if (result) + goto flush_fifo; + /* skip first samples if needed */ + if (st->skip_samples) { + st->skip_samples--; + continue; + } + timestamp = inv_mpu6050_get_timestamp(st); + iio_push_to_buffers_with_timestamp(indio_dev, st->data, timestamp); + } + +end_session: + mutex_unlock(&st->lock); + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; + +flush_fifo: + /* Flush HW and SW FIFOs. */ + inv_reset_fifo(indio_dev); + mutex_unlock(&st->lock); + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; +} diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c new file mode 100644 index 000000000..e6107b0cc --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c @@ -0,0 +1,165 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* +* Copyright (C) 2015 Intel Corporation Inc. +*/ +#include +#include +#include +#include +#include +#include +#include "inv_mpu_iio.h" + +static const struct regmap_config inv_mpu_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int inv_mpu_i2c_disable(struct iio_dev *indio_dev) +{ + struct inv_mpu6050_state *st = iio_priv(indio_dev); + int ret = 0; + + if (st->reg->i2c_if) { + ret = regmap_write(st->map, st->reg->i2c_if, + INV_ICM20602_BIT_I2C_IF_DIS); + } else { + st->chip_config.user_ctrl |= INV_MPU6050_BIT_I2C_IF_DIS; + ret = regmap_write(st->map, st->reg->user_ctrl, + st->chip_config.user_ctrl); + } + + return ret; +} + +static int inv_mpu_probe(struct spi_device *spi) +{ + const void *match; + struct regmap *regmap; + const struct spi_device_id *spi_id; + const char *name = NULL; + enum inv_devices chip_type; + + if ((spi_id = spi_get_device_id(spi))) { + chip_type = (enum inv_devices)spi_id->driver_data; + name = spi_id->name; + } else if ((match = device_get_match_data(&spi->dev))) { + chip_type = (uintptr_t)match; + name = dev_name(&spi->dev); + } else { + return -ENODEV; + } + + regmap = devm_regmap_init_spi(spi, &inv_mpu_regmap_config); + if (IS_ERR(regmap)) { + dev_err(&spi->dev, "Failed to register spi regmap: %pe\n", + regmap); + return PTR_ERR(regmap); + } + + return inv_mpu_core_probe(regmap, spi->irq, name, + inv_mpu_i2c_disable, chip_type); +} + +/* + * device id table is used to identify what device can be + * supported by this driver + */ +static const struct spi_device_id inv_mpu_id[] = { + {"mpu6000", INV_MPU6000}, + {"mpu6500", INV_MPU6500}, + {"mpu6515", INV_MPU6515}, + {"mpu6880", INV_MPU6880}, + {"mpu9250", INV_MPU9250}, + {"mpu9255", INV_MPU9255}, + {"icm20608", INV_ICM20608}, + {"icm20608d", INV_ICM20608D}, + {"icm20609", INV_ICM20609}, + {"icm20689", INV_ICM20689}, + {"icm20602", INV_ICM20602}, + {"icm20690", INV_ICM20690}, + {"iam20680", INV_IAM20680}, + {} +}; + +MODULE_DEVICE_TABLE(spi, inv_mpu_id); + +static const struct of_device_id inv_of_match[] = { + { + .compatible = "invensense,mpu6000", + .data = (void *)INV_MPU6000 + }, + { + .compatible = "invensense,mpu6500", + .data = (void *)INV_MPU6500 + }, + { + .compatible = "invensense,mpu6515", + .data = (void *)INV_MPU6515 + }, + { + .compatible = "invensense,mpu6880", + .data = (void *)INV_MPU6880 + }, + { + .compatible = "invensense,mpu9250", + .data = (void *)INV_MPU9250 + }, + { + .compatible = "invensense,mpu9255", + .data = (void *)INV_MPU9255 + }, + { + .compatible = "invensense,icm20608", + .data = (void *)INV_ICM20608 + }, + { + .compatible = "invensense,icm20608d", + .data = (void *)INV_ICM20608D + }, + { + .compatible = "invensense,icm20609", + .data = (void *)INV_ICM20609 + }, + { + .compatible = "invensense,icm20689", + .data = (void *)INV_ICM20689 + }, + { + .compatible = "invensense,icm20602", + .data = (void *)INV_ICM20602 + }, + { + .compatible = "invensense,icm20690", + .data = (void *)INV_ICM20690 + }, + { + .compatible = "invensense,iam20680", + .data = (void *)INV_IAM20680 + }, + { } +}; +MODULE_DEVICE_TABLE(of, inv_of_match); + +static const struct acpi_device_id inv_acpi_match[] = { + {"INVN6000", INV_MPU6000}, + { }, +}; +MODULE_DEVICE_TABLE(acpi, inv_acpi_match); + +static struct spi_driver inv_mpu_driver = { + .probe = inv_mpu_probe, + .id_table = inv_mpu_id, + .driver = { + .of_match_table = inv_of_match, + .acpi_match_table = inv_acpi_match, + .name = "inv-mpu6000-spi", + .pm = &inv_mpu_pmops, + }, +}; + +module_spi_driver(inv_mpu_driver); + +MODULE_AUTHOR("Adriana Reus "); +MODULE_DESCRIPTION("Invensense device MPU6000 driver"); +MODULE_LICENSE("GPL"); diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c new file mode 100644 index 000000000..882546897 --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c @@ -0,0 +1,251 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* +* Copyright (C) 2012 Invensense, Inc. +*/ + +#include +#include "inv_mpu_iio.h" + +static unsigned int inv_scan_query_mpu6050(struct iio_dev *indio_dev) +{ + struct inv_mpu6050_state *st = iio_priv(indio_dev); + unsigned int mask; + + /* + * If the MPU6050 is just used as a trigger, then the scan mask + * is not allocated so we simply enable the temperature channel + * as a dummy and bail out. + */ + if (!indio_dev->active_scan_mask) { + st->chip_config.temp_fifo_enable = true; + return INV_MPU6050_SENSOR_TEMP; + } + + st->chip_config.gyro_fifo_enable = + test_bit(INV_MPU6050_SCAN_GYRO_X, + indio_dev->active_scan_mask) || + test_bit(INV_MPU6050_SCAN_GYRO_Y, + indio_dev->active_scan_mask) || + test_bit(INV_MPU6050_SCAN_GYRO_Z, + indio_dev->active_scan_mask); + + st->chip_config.accl_fifo_enable = + test_bit(INV_MPU6050_SCAN_ACCL_X, + indio_dev->active_scan_mask) || + test_bit(INV_MPU6050_SCAN_ACCL_Y, + indio_dev->active_scan_mask) || + test_bit(INV_MPU6050_SCAN_ACCL_Z, + indio_dev->active_scan_mask); + + st->chip_config.temp_fifo_enable = + test_bit(INV_MPU6050_SCAN_TEMP, indio_dev->active_scan_mask); + + mask = 0; + if (st->chip_config.gyro_fifo_enable) + mask |= INV_MPU6050_SENSOR_GYRO; + if (st->chip_config.accl_fifo_enable) + mask |= INV_MPU6050_SENSOR_ACCL; + if (st->chip_config.temp_fifo_enable) + mask |= INV_MPU6050_SENSOR_TEMP; + + return mask; +} + +static unsigned int inv_scan_query_mpu9x50(struct iio_dev *indio_dev) +{ + struct inv_mpu6050_state *st = iio_priv(indio_dev); + unsigned int mask; + + mask = inv_scan_query_mpu6050(indio_dev); + + /* no magnetometer if i2c auxiliary bus is used */ + if (st->magn_disabled) + return mask; + + st->chip_config.magn_fifo_enable = + test_bit(INV_MPU9X50_SCAN_MAGN_X, + indio_dev->active_scan_mask) || + test_bit(INV_MPU9X50_SCAN_MAGN_Y, + indio_dev->active_scan_mask) || + test_bit(INV_MPU9X50_SCAN_MAGN_Z, + indio_dev->active_scan_mask); + if (st->chip_config.magn_fifo_enable) + mask |= INV_MPU6050_SENSOR_MAGN; + + return mask; +} + +static unsigned int inv_scan_query(struct iio_dev *indio_dev) +{ + struct inv_mpu6050_state *st = iio_priv(indio_dev); + + switch (st->chip_type) { + case INV_MPU9150: + case INV_MPU9250: + case INV_MPU9255: + return inv_scan_query_mpu9x50(indio_dev); + default: + return inv_scan_query_mpu6050(indio_dev); + } +} + +static unsigned int inv_compute_skip_samples(const struct inv_mpu6050_state *st) +{ + unsigned int skip_samples = 0; + + /* mag first sample is always not ready, skip it */ + if (st->chip_config.magn_fifo_enable) + skip_samples = 1; + + return skip_samples; +} + +int inv_mpu6050_prepare_fifo(struct inv_mpu6050_state *st, bool enable) +{ + uint8_t d; + int ret; + + if (enable) { + st->it_timestamp = 0; + /* reset FIFO */ + d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST; + ret = regmap_write(st->map, st->reg->user_ctrl, d); + if (ret) + return ret; + /* enable sensor output to FIFO */ + d = 0; + if (st->chip_config.gyro_fifo_enable) + d |= INV_MPU6050_BITS_GYRO_OUT; + if (st->chip_config.accl_fifo_enable) + d |= INV_MPU6050_BIT_ACCEL_OUT; + if (st->chip_config.temp_fifo_enable) + d |= INV_MPU6050_BIT_TEMP_OUT; + if (st->chip_config.magn_fifo_enable) + d |= INV_MPU6050_BIT_SLAVE_0; + ret = regmap_write(st->map, st->reg->fifo_en, d); + if (ret) + return ret; + /* enable FIFO reading */ + d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_EN; + ret = regmap_write(st->map, st->reg->user_ctrl, d); + if (ret) + return ret; + /* enable interrupt */ + ret = regmap_write(st->map, st->reg->int_enable, + INV_MPU6050_BIT_DATA_RDY_EN); + } else { + ret = regmap_write(st->map, st->reg->int_enable, 0); + if (ret) + return ret; + ret = regmap_write(st->map, st->reg->fifo_en, 0); + if (ret) + return ret; + /* restore user_ctrl for disabling FIFO reading */ + ret = regmap_write(st->map, st->reg->user_ctrl, + st->chip_config.user_ctrl); + } + + return ret; +} + +/** + * inv_mpu6050_set_enable() - enable chip functions. + * @indio_dev: Device driver instance. + * @enable: enable/disable + */ +static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable) +{ + struct inv_mpu6050_state *st = iio_priv(indio_dev); + struct device *pdev = regmap_get_device(st->map); + unsigned int scan; + int result; + + if (enable) { + scan = inv_scan_query(indio_dev); + result = pm_runtime_resume_and_get(pdev); + if (result) + return result; + /* + * In case autosuspend didn't trigger, turn off first not + * required sensors. + */ + result = inv_mpu6050_switch_engine(st, false, ~scan); + if (result) + goto error_power_off; + result = inv_mpu6050_switch_engine(st, true, scan); + if (result) + goto error_power_off; + st->skip_samples = inv_compute_skip_samples(st); + result = inv_mpu6050_prepare_fifo(st, true); + if (result) + goto error_power_off; + } else { + result = inv_mpu6050_prepare_fifo(st, false); + if (result) + goto error_power_off; + pm_runtime_mark_last_busy(pdev); + pm_runtime_put_autosuspend(pdev); + } + + return 0; + +error_power_off: + pm_runtime_put_autosuspend(pdev); + return result; +} + +/** + * inv_mpu_data_rdy_trigger_set_state() - set data ready interrupt state + * @trig: Trigger instance + * @state: Desired trigger state + */ +static int inv_mpu_data_rdy_trigger_set_state(struct iio_trigger *trig, + bool state) +{ + struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig); + struct inv_mpu6050_state *st = iio_priv(indio_dev); + int result; + + mutex_lock(&st->lock); + result = inv_mpu6050_set_enable(indio_dev, state); + mutex_unlock(&st->lock); + + return result; +} + +static const struct iio_trigger_ops inv_mpu_trigger_ops = { + .set_trigger_state = &inv_mpu_data_rdy_trigger_set_state, +}; + +int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type) +{ + int ret; + struct inv_mpu6050_state *st = iio_priv(indio_dev); + + st->trig = devm_iio_trigger_alloc(&indio_dev->dev, + "%s-dev%d", + indio_dev->name, + iio_device_id(indio_dev)); + if (!st->trig) + return -ENOMEM; + + ret = devm_request_irq(&indio_dev->dev, st->irq, + &iio_trigger_generic_data_rdy_poll, + irq_type, + "inv_mpu", + st->trig); + if (ret) + return ret; + + st->trig->dev.parent = regmap_get_device(st->map); + st->trig->ops = &inv_mpu_trigger_ops; + iio_trigger_set_drvdata(st->trig, indio_dev); + + ret = devm_iio_trigger_register(&indio_dev->dev, st->trig); + if (ret) + return ret; + + indio_dev->trig = iio_trigger_get(st->trig); + + return 0; +} diff --git a/drivers/iio/imu/kmx61.c b/drivers/iio/imu/kmx61.c new file mode 100644 index 000000000..b10c0dcac --- /dev/null +++ b/drivers/iio/imu/kmx61.c @@ -0,0 +1,1529 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * KMX61 - Kionix 6-axis Accelerometer/Magnetometer + * + * Copyright (c) 2014, Intel Corporation. + * + * IIO driver for KMX61 (7-bit I2C slave address 0x0E or 0x0F). + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define KMX61_DRV_NAME "kmx61" +#define KMX61_IRQ_NAME "kmx61_event" + +#define KMX61_REG_WHO_AM_I 0x00 +#define KMX61_REG_INS1 0x01 +#define KMX61_REG_INS2 0x02 + +/* + * three 16-bit accelerometer output registers for X/Y/Z axis + * we use only XOUT_L as a base register, all other addresses + * can be obtained by applying an offset and are provided here + * only for clarity. + */ +#define KMX61_ACC_XOUT_L 0x0A +#define KMX61_ACC_XOUT_H 0x0B +#define KMX61_ACC_YOUT_L 0x0C +#define KMX61_ACC_YOUT_H 0x0D +#define KMX61_ACC_ZOUT_L 0x0E +#define KMX61_ACC_ZOUT_H 0x0F + +/* + * one 16-bit temperature output register + */ +#define KMX61_TEMP_L 0x10 +#define KMX61_TEMP_H 0x11 + +/* + * three 16-bit magnetometer output registers for X/Y/Z axis + */ +#define KMX61_MAG_XOUT_L 0x12 +#define KMX61_MAG_XOUT_H 0x13 +#define KMX61_MAG_YOUT_L 0x14 +#define KMX61_MAG_YOUT_H 0x15 +#define KMX61_MAG_ZOUT_L 0x16 +#define KMX61_MAG_ZOUT_H 0x17 + +#define KMX61_REG_INL 0x28 +#define KMX61_REG_STBY 0x29 +#define KMX61_REG_CTRL1 0x2A +#define KMX61_REG_CTRL2 0x2B +#define KMX61_REG_ODCNTL 0x2C +#define KMX61_REG_INC1 0x2D + +#define KMX61_REG_WUF_THRESH 0x3D +#define KMX61_REG_WUF_TIMER 0x3E + +#define KMX61_ACC_STBY_BIT BIT(0) +#define KMX61_MAG_STBY_BIT BIT(1) +#define KMX61_ACT_STBY_BIT BIT(7) + +#define KMX61_ALL_STBY (KMX61_ACC_STBY_BIT | KMX61_MAG_STBY_BIT) + +#define KMX61_REG_INS1_BIT_WUFS BIT(1) + +#define KMX61_REG_INS2_BIT_ZP BIT(0) +#define KMX61_REG_INS2_BIT_ZN BIT(1) +#define KMX61_REG_INS2_BIT_YP BIT(2) +#define KMX61_REG_INS2_BIT_YN BIT(3) +#define KMX61_REG_INS2_BIT_XP BIT(4) +#define KMX61_REG_INS2_BIT_XN BIT(5) + +#define KMX61_REG_CTRL1_GSEL_MASK 0x03 + +#define KMX61_REG_CTRL1_BIT_RES BIT(4) +#define KMX61_REG_CTRL1_BIT_DRDYE BIT(5) +#define KMX61_REG_CTRL1_BIT_WUFE BIT(6) +#define KMX61_REG_CTRL1_BIT_BTSE BIT(7) + +#define KMX61_REG_INC1_BIT_WUFS BIT(0) +#define KMX61_REG_INC1_BIT_DRDYM BIT(1) +#define KMX61_REG_INC1_BIT_DRDYA BIT(2) +#define KMX61_REG_INC1_BIT_IEN BIT(5) + +#define KMX61_ACC_ODR_SHIFT 0 +#define KMX61_MAG_ODR_SHIFT 4 +#define KMX61_ACC_ODR_MASK 0x0F +#define KMX61_MAG_ODR_MASK 0xF0 + +#define KMX61_OWUF_MASK 0x7 + +#define KMX61_DEFAULT_WAKE_THRESH 1 +#define KMX61_DEFAULT_WAKE_DURATION 1 + +#define KMX61_SLEEP_DELAY_MS 2000 + +#define KMX61_CHIP_ID 0x12 + +/* KMX61 devices */ +#define KMX61_ACC 0x01 +#define KMX61_MAG 0x02 + +struct kmx61_data { + struct i2c_client *client; + + /* serialize access to non-atomic ops, e.g set_mode */ + struct mutex lock; + + /* standby state */ + bool acc_stby; + bool mag_stby; + + /* power state */ + bool acc_ps; + bool mag_ps; + + /* config bits */ + u8 range; + u8 odr_bits; + u8 wake_thresh; + u8 wake_duration; + + /* accelerometer specific data */ + struct iio_dev *acc_indio_dev; + struct iio_trigger *acc_dready_trig; + struct iio_trigger *motion_trig; + bool acc_dready_trig_on; + bool motion_trig_on; + bool ev_enable_state; + + /* magnetometer specific data */ + struct iio_dev *mag_indio_dev; + struct iio_trigger *mag_dready_trig; + bool mag_dready_trig_on; +}; + +enum kmx61_range { + KMX61_RANGE_2G, + KMX61_RANGE_4G, + KMX61_RANGE_8G, +}; + +enum kmx61_axis { + KMX61_AXIS_X, + KMX61_AXIS_Y, + KMX61_AXIS_Z, +}; + +static const u16 kmx61_uscale_table[] = {9582, 19163, 38326}; + +static const struct { + int val; + int val2; +} kmx61_samp_freq_table[] = { {12, 500000}, + {25, 0}, + {50, 0}, + {100, 0}, + {200, 0}, + {400, 0}, + {800, 0}, + {1600, 0}, + {0, 781000}, + {1, 563000}, + {3, 125000}, + {6, 250000} }; + +static const struct { + int val; + int val2; + int odr_bits; +} kmx61_wake_up_odr_table[] = { {0, 781000, 0x00}, + {1, 563000, 0x01}, + {3, 125000, 0x02}, + {6, 250000, 0x03}, + {12, 500000, 0x04}, + {25, 0, 0x05}, + {50, 0, 0x06}, + {100, 0, 0x06}, + {200, 0, 0x06}, + {400, 0, 0x06}, + {800, 0, 0x06}, + {1600, 0, 0x06} }; + +static IIO_CONST_ATTR(accel_scale_available, "0.009582 0.019163 0.038326"); +static IIO_CONST_ATTR(magn_scale_available, "0.001465"); +static IIO_CONST_ATTR_SAMP_FREQ_AVAIL( + "0.781000 1.563000 3.125000 6.250000 12.500000 25 50 100 200 400 800"); + +static struct attribute *kmx61_acc_attributes[] = { + &iio_const_attr_accel_scale_available.dev_attr.attr, + &iio_const_attr_sampling_frequency_available.dev_attr.attr, + NULL, +}; + +static struct attribute *kmx61_mag_attributes[] = { + &iio_const_attr_magn_scale_available.dev_attr.attr, + &iio_const_attr_sampling_frequency_available.dev_attr.attr, + NULL, +}; + +static const struct attribute_group kmx61_acc_attribute_group = { + .attrs = kmx61_acc_attributes, +}; + +static const struct attribute_group kmx61_mag_attribute_group = { + .attrs = kmx61_mag_attributes, +}; + +static const struct iio_event_spec kmx61_event = { + .type = IIO_EV_TYPE_THRESH, + .dir = IIO_EV_DIR_EITHER, + .mask_separate = BIT(IIO_EV_INFO_VALUE) | + BIT(IIO_EV_INFO_ENABLE) | + BIT(IIO_EV_INFO_PERIOD), +}; + +#define KMX61_ACC_CHAN(_axis) { \ + .type = IIO_ACCEL, \ + .modified = 1, \ + .channel2 = IIO_MOD_ ## _axis, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \ + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) | \ + BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .address = KMX61_ACC, \ + .scan_index = KMX61_AXIS_ ## _axis, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 12, \ + .storagebits = 16, \ + .shift = 4, \ + .endianness = IIO_LE, \ + }, \ + .event_spec = &kmx61_event, \ + .num_event_specs = 1 \ +} + +#define KMX61_MAG_CHAN(_axis) { \ + .type = IIO_MAGN, \ + .modified = 1, \ + .channel2 = IIO_MOD_ ## _axis, \ + .address = KMX61_MAG, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \ + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) | \ + BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .scan_index = KMX61_AXIS_ ## _axis, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 14, \ + .storagebits = 16, \ + .shift = 2, \ + .endianness = IIO_LE, \ + }, \ +} + +static const struct iio_chan_spec kmx61_acc_channels[] = { + KMX61_ACC_CHAN(X), + KMX61_ACC_CHAN(Y), + KMX61_ACC_CHAN(Z), +}; + +static const struct iio_chan_spec kmx61_mag_channels[] = { + KMX61_MAG_CHAN(X), + KMX61_MAG_CHAN(Y), + KMX61_MAG_CHAN(Z), +}; + +static void kmx61_set_data(struct iio_dev *indio_dev, struct kmx61_data *data) +{ + struct kmx61_data **priv = iio_priv(indio_dev); + + *priv = data; +} + +static struct kmx61_data *kmx61_get_data(struct iio_dev *indio_dev) +{ + return *(struct kmx61_data **)iio_priv(indio_dev); +} + +static int kmx61_convert_freq_to_bit(int val, int val2) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(kmx61_samp_freq_table); i++) + if (val == kmx61_samp_freq_table[i].val && + val2 == kmx61_samp_freq_table[i].val2) + return i; + return -EINVAL; +} + +static int kmx61_convert_wake_up_odr_to_bit(int val, int val2) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(kmx61_wake_up_odr_table); ++i) + if (kmx61_wake_up_odr_table[i].val == val && + kmx61_wake_up_odr_table[i].val2 == val2) + return kmx61_wake_up_odr_table[i].odr_bits; + return -EINVAL; +} + +/** + * kmx61_set_mode() - set KMX61 device operating mode + * @data: kmx61 device private data pointer + * @mode: bitmask, indicating operating mode for @device + * @device: bitmask, indicating device for which @mode needs to be set + * @update: update stby bits stored in device's private @data + * + * For each sensor (accelerometer/magnetometer) there are two operating modes + * STANDBY and OPERATION. Neither accel nor magn can be disabled independently + * if they are both enabled. Internal sensors state is saved in acc_stby and + * mag_stby members of driver's private @data. + */ +static int kmx61_set_mode(struct kmx61_data *data, u8 mode, u8 device, + bool update) +{ + int ret; + int acc_stby = -1, mag_stby = -1; + + ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_STBY); + if (ret < 0) { + dev_err(&data->client->dev, "Error reading reg_stby\n"); + return ret; + } + if (device & KMX61_ACC) { + if (mode & KMX61_ACC_STBY_BIT) { + ret |= KMX61_ACC_STBY_BIT; + acc_stby = 1; + } else { + ret &= ~KMX61_ACC_STBY_BIT; + acc_stby = 0; + } + } + + if (device & KMX61_MAG) { + if (mode & KMX61_MAG_STBY_BIT) { + ret |= KMX61_MAG_STBY_BIT; + mag_stby = 1; + } else { + ret &= ~KMX61_MAG_STBY_BIT; + mag_stby = 0; + } + } + + if (mode & KMX61_ACT_STBY_BIT) + ret |= KMX61_ACT_STBY_BIT; + + ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_STBY, ret); + if (ret < 0) { + dev_err(&data->client->dev, "Error writing reg_stby\n"); + return ret; + } + + if (acc_stby != -1 && update) + data->acc_stby = acc_stby; + if (mag_stby != -1 && update) + data->mag_stby = mag_stby; + + return 0; +} + +static int kmx61_get_mode(struct kmx61_data *data, u8 *mode, u8 device) +{ + int ret; + + ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_STBY); + if (ret < 0) { + dev_err(&data->client->dev, "Error reading reg_stby\n"); + return ret; + } + *mode = 0; + + if (device & KMX61_ACC) { + if (ret & KMX61_ACC_STBY_BIT) + *mode |= KMX61_ACC_STBY_BIT; + else + *mode &= ~KMX61_ACC_STBY_BIT; + } + + if (device & KMX61_MAG) { + if (ret & KMX61_MAG_STBY_BIT) + *mode |= KMX61_MAG_STBY_BIT; + else + *mode &= ~KMX61_MAG_STBY_BIT; + } + + return 0; +} + +static int kmx61_set_wake_up_odr(struct kmx61_data *data, int val, int val2) +{ + int ret, odr_bits; + + odr_bits = kmx61_convert_wake_up_odr_to_bit(val, val2); + if (odr_bits < 0) + return odr_bits; + + ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_CTRL2, + odr_bits); + if (ret < 0) + dev_err(&data->client->dev, "Error writing reg_ctrl2\n"); + return ret; +} + +static int kmx61_set_odr(struct kmx61_data *data, int val, int val2, u8 device) +{ + int ret; + u8 mode; + int lodr_bits, odr_bits; + + ret = kmx61_get_mode(data, &mode, KMX61_ACC | KMX61_MAG); + if (ret < 0) + return ret; + + lodr_bits = kmx61_convert_freq_to_bit(val, val2); + if (lodr_bits < 0) + return lodr_bits; + + /* To change ODR, accel and magn must be in STDBY */ + ret = kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, + true); + if (ret < 0) + return ret; + + odr_bits = 0; + if (device & KMX61_ACC) + odr_bits |= lodr_bits << KMX61_ACC_ODR_SHIFT; + if (device & KMX61_MAG) + odr_bits |= lodr_bits << KMX61_MAG_ODR_SHIFT; + + ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_ODCNTL, + odr_bits); + if (ret < 0) + return ret; + + data->odr_bits = odr_bits; + + if (device & KMX61_ACC) { + ret = kmx61_set_wake_up_odr(data, val, val2); + if (ret) + return ret; + } + + return kmx61_set_mode(data, mode, KMX61_ACC | KMX61_MAG, true); +} + +static int kmx61_get_odr(struct kmx61_data *data, int *val, int *val2, + u8 device) +{ + u8 lodr_bits; + + if (device & KMX61_ACC) + lodr_bits = (data->odr_bits >> KMX61_ACC_ODR_SHIFT) & + KMX61_ACC_ODR_MASK; + else if (device & KMX61_MAG) + lodr_bits = (data->odr_bits >> KMX61_MAG_ODR_SHIFT) & + KMX61_MAG_ODR_MASK; + else + return -EINVAL; + + if (lodr_bits >= ARRAY_SIZE(kmx61_samp_freq_table)) + return -EINVAL; + + *val = kmx61_samp_freq_table[lodr_bits].val; + *val2 = kmx61_samp_freq_table[lodr_bits].val2; + + return 0; +} + +static int kmx61_set_range(struct kmx61_data *data, u8 range) +{ + int ret; + + ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_CTRL1); + if (ret < 0) { + dev_err(&data->client->dev, "Error reading reg_ctrl1\n"); + return ret; + } + + ret &= ~KMX61_REG_CTRL1_GSEL_MASK; + ret |= range & KMX61_REG_CTRL1_GSEL_MASK; + + ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_CTRL1, ret); + if (ret < 0) { + dev_err(&data->client->dev, "Error writing reg_ctrl1\n"); + return ret; + } + + data->range = range; + + return 0; +} + +static int kmx61_set_scale(struct kmx61_data *data, u16 uscale) +{ + int ret, i; + u8 mode; + + for (i = 0; i < ARRAY_SIZE(kmx61_uscale_table); i++) { + if (kmx61_uscale_table[i] == uscale) { + ret = kmx61_get_mode(data, &mode, + KMX61_ACC | KMX61_MAG); + if (ret < 0) + return ret; + + ret = kmx61_set_mode(data, KMX61_ALL_STBY, + KMX61_ACC | KMX61_MAG, true); + if (ret < 0) + return ret; + + ret = kmx61_set_range(data, i); + if (ret < 0) + return ret; + + return kmx61_set_mode(data, mode, + KMX61_ACC | KMX61_MAG, true); + } + } + return -EINVAL; +} + +static int kmx61_chip_init(struct kmx61_data *data) +{ + int ret, val, val2; + + ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_WHO_AM_I); + if (ret < 0) { + dev_err(&data->client->dev, "Error reading who_am_i\n"); + return ret; + } + + if (ret != KMX61_CHIP_ID) { + dev_err(&data->client->dev, + "Wrong chip id, got %x expected %x\n", + ret, KMX61_CHIP_ID); + return -EINVAL; + } + + /* set accel 12bit, 4g range */ + ret = kmx61_set_range(data, KMX61_RANGE_4G); + if (ret < 0) + return ret; + + ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_ODCNTL); + if (ret < 0) { + dev_err(&data->client->dev, "Error reading reg_odcntl\n"); + return ret; + } + data->odr_bits = ret; + + /* + * set output data rate for wake up (motion detection) function + * to match data rate for accelerometer sampling + */ + ret = kmx61_get_odr(data, &val, &val2, KMX61_ACC); + if (ret < 0) + return ret; + + ret = kmx61_set_wake_up_odr(data, val, val2); + if (ret < 0) + return ret; + + /* set acc/magn to OPERATION mode */ + ret = kmx61_set_mode(data, 0, KMX61_ACC | KMX61_MAG, true); + if (ret < 0) + return ret; + + data->wake_thresh = KMX61_DEFAULT_WAKE_THRESH; + data->wake_duration = KMX61_DEFAULT_WAKE_DURATION; + + return 0; +} + +static int kmx61_setup_new_data_interrupt(struct kmx61_data *data, + bool status, u8 device) +{ + u8 mode; + int ret; + + ret = kmx61_get_mode(data, &mode, KMX61_ACC | KMX61_MAG); + if (ret < 0) + return ret; + + ret = kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, true); + if (ret < 0) + return ret; + + ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_INC1); + if (ret < 0) { + dev_err(&data->client->dev, "Error reading reg_ctrl1\n"); + return ret; + } + + if (status) { + ret |= KMX61_REG_INC1_BIT_IEN; + if (device & KMX61_ACC) + ret |= KMX61_REG_INC1_BIT_DRDYA; + if (device & KMX61_MAG) + ret |= KMX61_REG_INC1_BIT_DRDYM; + } else { + ret &= ~KMX61_REG_INC1_BIT_IEN; + if (device & KMX61_ACC) + ret &= ~KMX61_REG_INC1_BIT_DRDYA; + if (device & KMX61_MAG) + ret &= ~KMX61_REG_INC1_BIT_DRDYM; + } + ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_INC1, ret); + if (ret < 0) { + dev_err(&data->client->dev, "Error writing reg_int_ctrl1\n"); + return ret; + } + + ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_CTRL1); + if (ret < 0) { + dev_err(&data->client->dev, "Error reading reg_ctrl1\n"); + return ret; + } + + if (status) + ret |= KMX61_REG_CTRL1_BIT_DRDYE; + else + ret &= ~KMX61_REG_CTRL1_BIT_DRDYE; + + ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_CTRL1, ret); + if (ret < 0) { + dev_err(&data->client->dev, "Error writing reg_ctrl1\n"); + return ret; + } + + return kmx61_set_mode(data, mode, KMX61_ACC | KMX61_MAG, true); +} + +static int kmx61_chip_update_thresholds(struct kmx61_data *data) +{ + int ret; + + ret = i2c_smbus_write_byte_data(data->client, + KMX61_REG_WUF_TIMER, + data->wake_duration); + if (ret < 0) { + dev_err(&data->client->dev, "Errow writing reg_wuf_timer\n"); + return ret; + } + + ret = i2c_smbus_write_byte_data(data->client, + KMX61_REG_WUF_THRESH, + data->wake_thresh); + if (ret < 0) + dev_err(&data->client->dev, "Error writing reg_wuf_thresh\n"); + + return ret; +} + +static int kmx61_setup_any_motion_interrupt(struct kmx61_data *data, + bool status) +{ + u8 mode; + int ret; + + ret = kmx61_get_mode(data, &mode, KMX61_ACC | KMX61_MAG); + if (ret < 0) + return ret; + + ret = kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, true); + if (ret < 0) + return ret; + + ret = kmx61_chip_update_thresholds(data); + if (ret < 0) + return ret; + + ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_INC1); + if (ret < 0) { + dev_err(&data->client->dev, "Error reading reg_inc1\n"); + return ret; + } + if (status) + ret |= (KMX61_REG_INC1_BIT_IEN | KMX61_REG_INC1_BIT_WUFS); + else + ret &= ~(KMX61_REG_INC1_BIT_IEN | KMX61_REG_INC1_BIT_WUFS); + + ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_INC1, ret); + if (ret < 0) { + dev_err(&data->client->dev, "Error writing reg_inc1\n"); + return ret; + } + + ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_CTRL1); + if (ret < 0) { + dev_err(&data->client->dev, "Error reading reg_ctrl1\n"); + return ret; + } + + if (status) + ret |= KMX61_REG_CTRL1_BIT_WUFE | KMX61_REG_CTRL1_BIT_BTSE; + else + ret &= ~(KMX61_REG_CTRL1_BIT_WUFE | KMX61_REG_CTRL1_BIT_BTSE); + + ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_CTRL1, ret); + if (ret < 0) { + dev_err(&data->client->dev, "Error writing reg_ctrl1\n"); + return ret; + } + mode |= KMX61_ACT_STBY_BIT; + return kmx61_set_mode(data, mode, KMX61_ACC | KMX61_MAG, true); +} + +/** + * kmx61_set_power_state() - set power state for kmx61 @device + * @data: kmx61 device private pointer + * @on: power state to be set for @device + * @device: bitmask indicating device for which @on state needs to be set + * + * Notice that when ACC power state needs to be set to ON and MAG is in + * OPERATION then we know that kmx61_runtime_resume was already called + * so we must set ACC OPERATION mode here. The same happens when MAG power + * state needs to be set to ON and ACC is in OPERATION. + */ +static int kmx61_set_power_state(struct kmx61_data *data, bool on, u8 device) +{ +#ifdef CONFIG_PM + int ret; + + if (device & KMX61_ACC) { + if (on && !data->acc_ps && !data->mag_stby) { + ret = kmx61_set_mode(data, 0, KMX61_ACC, true); + if (ret < 0) + return ret; + } + data->acc_ps = on; + } + if (device & KMX61_MAG) { + if (on && !data->mag_ps && !data->acc_stby) { + ret = kmx61_set_mode(data, 0, KMX61_MAG, true); + if (ret < 0) + return ret; + } + data->mag_ps = on; + } + + if (on) { + ret = pm_runtime_resume_and_get(&data->client->dev); + } else { + pm_runtime_mark_last_busy(&data->client->dev); + ret = pm_runtime_put_autosuspend(&data->client->dev); + } + if (ret < 0) { + dev_err(&data->client->dev, + "Failed: kmx61_set_power_state for %d, ret %d\n", + on, ret); + + return ret; + } +#endif + return 0; +} + +static int kmx61_read_measurement(struct kmx61_data *data, u8 base, u8 offset) +{ + int ret; + u8 reg = base + offset * 2; + + ret = i2c_smbus_read_word_data(data->client, reg); + if (ret < 0) + dev_err(&data->client->dev, "failed to read reg at %x\n", reg); + + return ret; +} + +static int kmx61_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, int *val, + int *val2, long mask) +{ + int ret; + u8 base_reg; + struct kmx61_data *data = kmx61_get_data(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_RAW: + switch (chan->type) { + case IIO_ACCEL: + base_reg = KMX61_ACC_XOUT_L; + break; + case IIO_MAGN: + base_reg = KMX61_MAG_XOUT_L; + break; + default: + return -EINVAL; + } + mutex_lock(&data->lock); + + ret = kmx61_set_power_state(data, true, chan->address); + if (ret) { + mutex_unlock(&data->lock); + return ret; + } + + ret = kmx61_read_measurement(data, base_reg, chan->scan_index); + if (ret < 0) { + kmx61_set_power_state(data, false, chan->address); + mutex_unlock(&data->lock); + return ret; + } + *val = sign_extend32(ret >> chan->scan_type.shift, + chan->scan_type.realbits - 1); + ret = kmx61_set_power_state(data, false, chan->address); + + mutex_unlock(&data->lock); + if (ret) + return ret; + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + switch (chan->type) { + case IIO_ACCEL: + *val = 0; + *val2 = kmx61_uscale_table[data->range]; + return IIO_VAL_INT_PLUS_MICRO; + case IIO_MAGN: + /* 14 bits res, 1465 microGauss per magn count */ + *val = 0; + *val2 = 1465; + return IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } + case IIO_CHAN_INFO_SAMP_FREQ: + if (chan->type != IIO_ACCEL && chan->type != IIO_MAGN) + return -EINVAL; + + mutex_lock(&data->lock); + ret = kmx61_get_odr(data, val, val2, chan->address); + mutex_unlock(&data->lock); + if (ret) + return -EINVAL; + return IIO_VAL_INT_PLUS_MICRO; + } + return -EINVAL; +} + +static int kmx61_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, int val, + int val2, long mask) +{ + int ret; + struct kmx61_data *data = kmx61_get_data(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: + if (chan->type != IIO_ACCEL && chan->type != IIO_MAGN) + return -EINVAL; + + mutex_lock(&data->lock); + ret = kmx61_set_odr(data, val, val2, chan->address); + mutex_unlock(&data->lock); + return ret; + case IIO_CHAN_INFO_SCALE: + switch (chan->type) { + case IIO_ACCEL: + if (val != 0) + return -EINVAL; + mutex_lock(&data->lock); + ret = kmx61_set_scale(data, val2); + mutex_unlock(&data->lock); + return ret; + default: + return -EINVAL; + } + default: + return -EINVAL; + } +} + +static int kmx61_read_event(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, + enum iio_event_type type, + enum iio_event_direction dir, + enum iio_event_info info, + int *val, int *val2) +{ + struct kmx61_data *data = kmx61_get_data(indio_dev); + + *val2 = 0; + switch (info) { + case IIO_EV_INFO_VALUE: + *val = data->wake_thresh; + return IIO_VAL_INT; + case IIO_EV_INFO_PERIOD: + *val = data->wake_duration; + return IIO_VAL_INT; + default: + return -EINVAL; + } +} + +static int kmx61_write_event(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, + enum iio_event_type type, + enum iio_event_direction dir, + enum iio_event_info info, + int val, int val2) +{ + struct kmx61_data *data = kmx61_get_data(indio_dev); + + if (data->ev_enable_state) + return -EBUSY; + + switch (info) { + case IIO_EV_INFO_VALUE: + data->wake_thresh = val; + return IIO_VAL_INT; + case IIO_EV_INFO_PERIOD: + data->wake_duration = val; + return IIO_VAL_INT; + default: + return -EINVAL; + } +} + +static int kmx61_read_event_config(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, + enum iio_event_type type, + enum iio_event_direction dir) +{ + struct kmx61_data *data = kmx61_get_data(indio_dev); + + return data->ev_enable_state; +} + +static int kmx61_write_event_config(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, + enum iio_event_type type, + enum iio_event_direction dir, + int state) +{ + struct kmx61_data *data = kmx61_get_data(indio_dev); + int ret = 0; + + if (state && data->ev_enable_state) + return 0; + + mutex_lock(&data->lock); + + if (!state && data->motion_trig_on) { + data->ev_enable_state = false; + goto err_unlock; + } + + ret = kmx61_set_power_state(data, state, KMX61_ACC); + if (ret < 0) + goto err_unlock; + + ret = kmx61_setup_any_motion_interrupt(data, state); + if (ret < 0) { + kmx61_set_power_state(data, false, KMX61_ACC); + goto err_unlock; + } + + data->ev_enable_state = state; + +err_unlock: + mutex_unlock(&data->lock); + + return ret; +} + +static int kmx61_acc_validate_trigger(struct iio_dev *indio_dev, + struct iio_trigger *trig) +{ + struct kmx61_data *data = kmx61_get_data(indio_dev); + + if (data->acc_dready_trig != trig && data->motion_trig != trig) + return -EINVAL; + + return 0; +} + +static int kmx61_mag_validate_trigger(struct iio_dev *indio_dev, + struct iio_trigger *trig) +{ + struct kmx61_data *data = kmx61_get_data(indio_dev); + + if (data->mag_dready_trig != trig) + return -EINVAL; + + return 0; +} + +static const struct iio_info kmx61_acc_info = { + .read_raw = kmx61_read_raw, + .write_raw = kmx61_write_raw, + .attrs = &kmx61_acc_attribute_group, + .read_event_value = kmx61_read_event, + .write_event_value = kmx61_write_event, + .read_event_config = kmx61_read_event_config, + .write_event_config = kmx61_write_event_config, + .validate_trigger = kmx61_acc_validate_trigger, +}; + +static const struct iio_info kmx61_mag_info = { + .read_raw = kmx61_read_raw, + .write_raw = kmx61_write_raw, + .attrs = &kmx61_mag_attribute_group, + .validate_trigger = kmx61_mag_validate_trigger, +}; + + +static int kmx61_data_rdy_trigger_set_state(struct iio_trigger *trig, + bool state) +{ + int ret = 0; + u8 device; + + struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig); + struct kmx61_data *data = kmx61_get_data(indio_dev); + + mutex_lock(&data->lock); + + if (!state && data->ev_enable_state && data->motion_trig_on) { + data->motion_trig_on = false; + goto err_unlock; + } + + if (data->acc_dready_trig == trig || data->motion_trig == trig) + device = KMX61_ACC; + else + device = KMX61_MAG; + + ret = kmx61_set_power_state(data, state, device); + if (ret < 0) + goto err_unlock; + + if (data->acc_dready_trig == trig || data->mag_dready_trig == trig) + ret = kmx61_setup_new_data_interrupt(data, state, device); + else + ret = kmx61_setup_any_motion_interrupt(data, state); + if (ret < 0) { + kmx61_set_power_state(data, false, device); + goto err_unlock; + } + + if (data->acc_dready_trig == trig) + data->acc_dready_trig_on = state; + else if (data->mag_dready_trig == trig) + data->mag_dready_trig_on = state; + else + data->motion_trig_on = state; +err_unlock: + mutex_unlock(&data->lock); + + return ret; +} + +static void kmx61_trig_reenable(struct iio_trigger *trig) +{ + struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig); + struct kmx61_data *data = kmx61_get_data(indio_dev); + int ret; + + ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_INL); + if (ret < 0) + dev_err(&data->client->dev, "Error reading reg_inl\n"); +} + +static const struct iio_trigger_ops kmx61_trigger_ops = { + .set_trigger_state = kmx61_data_rdy_trigger_set_state, + .reenable = kmx61_trig_reenable, +}; + +static irqreturn_t kmx61_event_handler(int irq, void *private) +{ + struct kmx61_data *data = private; + struct iio_dev *indio_dev = data->acc_indio_dev; + int ret; + + ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_INS1); + if (ret < 0) { + dev_err(&data->client->dev, "Error reading reg_ins1\n"); + goto ack_intr; + } + + if (ret & KMX61_REG_INS1_BIT_WUFS) { + ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_INS2); + if (ret < 0) { + dev_err(&data->client->dev, "Error reading reg_ins2\n"); + goto ack_intr; + } + + if (ret & KMX61_REG_INS2_BIT_XN) + iio_push_event(indio_dev, + IIO_MOD_EVENT_CODE(IIO_ACCEL, + 0, + IIO_MOD_X, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_FALLING), + 0); + + if (ret & KMX61_REG_INS2_BIT_XP) + iio_push_event(indio_dev, + IIO_MOD_EVENT_CODE(IIO_ACCEL, + 0, + IIO_MOD_X, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING), + 0); + + if (ret & KMX61_REG_INS2_BIT_YN) + iio_push_event(indio_dev, + IIO_MOD_EVENT_CODE(IIO_ACCEL, + 0, + IIO_MOD_Y, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_FALLING), + 0); + + if (ret & KMX61_REG_INS2_BIT_YP) + iio_push_event(indio_dev, + IIO_MOD_EVENT_CODE(IIO_ACCEL, + 0, + IIO_MOD_Y, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING), + 0); + + if (ret & KMX61_REG_INS2_BIT_ZN) + iio_push_event(indio_dev, + IIO_MOD_EVENT_CODE(IIO_ACCEL, + 0, + IIO_MOD_Z, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_FALLING), + 0); + + if (ret & KMX61_REG_INS2_BIT_ZP) + iio_push_event(indio_dev, + IIO_MOD_EVENT_CODE(IIO_ACCEL, + 0, + IIO_MOD_Z, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_RISING), + 0); + } + +ack_intr: + ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_CTRL1); + if (ret < 0) + dev_err(&data->client->dev, "Error reading reg_ctrl1\n"); + + ret |= KMX61_REG_CTRL1_BIT_RES; + ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_CTRL1, ret); + if (ret < 0) + dev_err(&data->client->dev, "Error writing reg_ctrl1\n"); + + ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_INL); + if (ret < 0) + dev_err(&data->client->dev, "Error reading reg_inl\n"); + + return IRQ_HANDLED; +} + +static irqreturn_t kmx61_data_rdy_trig_poll(int irq, void *private) +{ + struct kmx61_data *data = private; + + if (data->acc_dready_trig_on) + iio_trigger_poll(data->acc_dready_trig); + if (data->mag_dready_trig_on) + iio_trigger_poll(data->mag_dready_trig); + + if (data->motion_trig_on) + iio_trigger_poll(data->motion_trig); + + if (data->ev_enable_state) + return IRQ_WAKE_THREAD; + return IRQ_HANDLED; +} + +static irqreturn_t kmx61_trigger_handler(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + struct kmx61_data *data = kmx61_get_data(indio_dev); + int bit, ret, i = 0; + u8 base; + s16 buffer[8]; + + if (indio_dev == data->acc_indio_dev) + base = KMX61_ACC_XOUT_L; + else + base = KMX61_MAG_XOUT_L; + + mutex_lock(&data->lock); + for_each_set_bit(bit, indio_dev->active_scan_mask, + indio_dev->masklength) { + ret = kmx61_read_measurement(data, base, bit); + if (ret < 0) { + mutex_unlock(&data->lock); + goto err; + } + buffer[i++] = ret; + } + mutex_unlock(&data->lock); + + iio_push_to_buffers(indio_dev, buffer); +err: + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; +} + +static const char *kmx61_match_acpi_device(struct device *dev) +{ + const struct acpi_device_id *id; + + id = acpi_match_device(dev->driver->acpi_match_table, dev); + if (!id) + return NULL; + return dev_name(dev); +} + +static struct iio_dev *kmx61_indiodev_setup(struct kmx61_data *data, + const struct iio_info *info, + const struct iio_chan_spec *chan, + int num_channels, + const char *name) +{ + struct iio_dev *indio_dev; + + indio_dev = devm_iio_device_alloc(&data->client->dev, sizeof(data)); + if (!indio_dev) + return ERR_PTR(-ENOMEM); + + kmx61_set_data(indio_dev, data); + + indio_dev->channels = chan; + indio_dev->num_channels = num_channels; + indio_dev->name = name; + indio_dev->modes = INDIO_DIRECT_MODE; + indio_dev->info = info; + + return indio_dev; +} + +static struct iio_trigger *kmx61_trigger_setup(struct kmx61_data *data, + struct iio_dev *indio_dev, + const char *tag) +{ + struct iio_trigger *trig; + int ret; + + trig = devm_iio_trigger_alloc(&data->client->dev, + "%s-%s-dev%d", + indio_dev->name, + tag, + iio_device_id(indio_dev)); + if (!trig) + return ERR_PTR(-ENOMEM); + + trig->ops = &kmx61_trigger_ops; + iio_trigger_set_drvdata(trig, indio_dev); + + ret = iio_trigger_register(trig); + if (ret) + return ERR_PTR(ret); + + return trig; +} + +static int kmx61_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + int ret; + struct kmx61_data *data; + const char *name = NULL; + + data = devm_kzalloc(&client->dev, sizeof(*data), GFP_KERNEL); + if (!data) + return -ENOMEM; + + i2c_set_clientdata(client, data); + data->client = client; + + mutex_init(&data->lock); + + if (id) + name = id->name; + else if (ACPI_HANDLE(&client->dev)) + name = kmx61_match_acpi_device(&client->dev); + else + return -ENODEV; + + data->acc_indio_dev = + kmx61_indiodev_setup(data, &kmx61_acc_info, + kmx61_acc_channels, + ARRAY_SIZE(kmx61_acc_channels), + name); + if (IS_ERR(data->acc_indio_dev)) + return PTR_ERR(data->acc_indio_dev); + + data->mag_indio_dev = + kmx61_indiodev_setup(data, &kmx61_mag_info, + kmx61_mag_channels, + ARRAY_SIZE(kmx61_mag_channels), + name); + if (IS_ERR(data->mag_indio_dev)) + return PTR_ERR(data->mag_indio_dev); + + ret = kmx61_chip_init(data); + if (ret < 0) + return ret; + + if (client->irq > 0) { + ret = devm_request_threaded_irq(&client->dev, client->irq, + kmx61_data_rdy_trig_poll, + kmx61_event_handler, + IRQF_TRIGGER_RISING, + KMX61_IRQ_NAME, + data); + if (ret) + goto err_chip_uninit; + + data->acc_dready_trig = + kmx61_trigger_setup(data, data->acc_indio_dev, + "dready"); + if (IS_ERR(data->acc_dready_trig)) { + ret = PTR_ERR(data->acc_dready_trig); + goto err_chip_uninit; + } + + data->mag_dready_trig = + kmx61_trigger_setup(data, data->mag_indio_dev, + "dready"); + if (IS_ERR(data->mag_dready_trig)) { + ret = PTR_ERR(data->mag_dready_trig); + goto err_trigger_unregister_acc_dready; + } + + data->motion_trig = + kmx61_trigger_setup(data, data->acc_indio_dev, + "any-motion"); + if (IS_ERR(data->motion_trig)) { + ret = PTR_ERR(data->motion_trig); + goto err_trigger_unregister_mag_dready; + } + + ret = iio_triggered_buffer_setup(data->acc_indio_dev, + &iio_pollfunc_store_time, + kmx61_trigger_handler, + NULL); + if (ret < 0) { + dev_err(&data->client->dev, + "Failed to setup acc triggered buffer\n"); + goto err_trigger_unregister_motion; + } + + ret = iio_triggered_buffer_setup(data->mag_indio_dev, + &iio_pollfunc_store_time, + kmx61_trigger_handler, + NULL); + if (ret < 0) { + dev_err(&data->client->dev, + "Failed to setup mag triggered buffer\n"); + goto err_buffer_cleanup_acc; + } + } + + ret = pm_runtime_set_active(&client->dev); + if (ret < 0) + goto err_buffer_cleanup_mag; + + pm_runtime_enable(&client->dev); + pm_runtime_set_autosuspend_delay(&client->dev, KMX61_SLEEP_DELAY_MS); + pm_runtime_use_autosuspend(&client->dev); + + ret = iio_device_register(data->acc_indio_dev); + if (ret < 0) { + dev_err(&client->dev, "Failed to register acc iio device\n"); + goto err_pm_cleanup; + } + + ret = iio_device_register(data->mag_indio_dev); + if (ret < 0) { + dev_err(&client->dev, "Failed to register mag iio device\n"); + goto err_iio_unregister_acc; + } + + return 0; + +err_iio_unregister_acc: + iio_device_unregister(data->acc_indio_dev); +err_pm_cleanup: + pm_runtime_dont_use_autosuspend(&client->dev); + pm_runtime_disable(&client->dev); +err_buffer_cleanup_mag: + if (client->irq > 0) + iio_triggered_buffer_cleanup(data->mag_indio_dev); +err_buffer_cleanup_acc: + if (client->irq > 0) + iio_triggered_buffer_cleanup(data->acc_indio_dev); +err_trigger_unregister_motion: + iio_trigger_unregister(data->motion_trig); +err_trigger_unregister_mag_dready: + iio_trigger_unregister(data->mag_dready_trig); +err_trigger_unregister_acc_dready: + iio_trigger_unregister(data->acc_dready_trig); +err_chip_uninit: + kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, true); + return ret; +} + +static void kmx61_remove(struct i2c_client *client) +{ + struct kmx61_data *data = i2c_get_clientdata(client); + + iio_device_unregister(data->acc_indio_dev); + iio_device_unregister(data->mag_indio_dev); + + pm_runtime_disable(&client->dev); + pm_runtime_set_suspended(&client->dev); + + if (client->irq > 0) { + iio_triggered_buffer_cleanup(data->acc_indio_dev); + iio_triggered_buffer_cleanup(data->mag_indio_dev); + iio_trigger_unregister(data->acc_dready_trig); + iio_trigger_unregister(data->mag_dready_trig); + iio_trigger_unregister(data->motion_trig); + } + + mutex_lock(&data->lock); + kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, true); + mutex_unlock(&data->lock); +} + +static int kmx61_suspend(struct device *dev) +{ + int ret; + struct kmx61_data *data = i2c_get_clientdata(to_i2c_client(dev)); + + mutex_lock(&data->lock); + ret = kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, + false); + mutex_unlock(&data->lock); + + return ret; +} + +static int kmx61_resume(struct device *dev) +{ + u8 stby = 0; + struct kmx61_data *data = i2c_get_clientdata(to_i2c_client(dev)); + + if (data->acc_stby) + stby |= KMX61_ACC_STBY_BIT; + if (data->mag_stby) + stby |= KMX61_MAG_STBY_BIT; + + return kmx61_set_mode(data, stby, KMX61_ACC | KMX61_MAG, true); +} + +static int kmx61_runtime_suspend(struct device *dev) +{ + struct kmx61_data *data = i2c_get_clientdata(to_i2c_client(dev)); + int ret; + + mutex_lock(&data->lock); + ret = kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, true); + mutex_unlock(&data->lock); + + return ret; +} + +static int kmx61_runtime_resume(struct device *dev) +{ + struct kmx61_data *data = i2c_get_clientdata(to_i2c_client(dev)); + u8 stby = 0; + + if (!data->acc_ps) + stby |= KMX61_ACC_STBY_BIT; + if (!data->mag_ps) + stby |= KMX61_MAG_STBY_BIT; + + return kmx61_set_mode(data, stby, KMX61_ACC | KMX61_MAG, true); +} + +static const struct dev_pm_ops kmx61_pm_ops = { + SYSTEM_SLEEP_PM_OPS(kmx61_suspend, kmx61_resume) + RUNTIME_PM_OPS(kmx61_runtime_suspend, kmx61_runtime_resume, NULL) +}; + +static const struct acpi_device_id kmx61_acpi_match[] = { + {"KMX61021", 0}, + {} +}; + +MODULE_DEVICE_TABLE(acpi, kmx61_acpi_match); + +static const struct i2c_device_id kmx61_id[] = { + {"kmx611021", 0}, + {} +}; + +MODULE_DEVICE_TABLE(i2c, kmx61_id); + +static struct i2c_driver kmx61_driver = { + .driver = { + .name = KMX61_DRV_NAME, + .acpi_match_table = ACPI_PTR(kmx61_acpi_match), + .pm = pm_ptr(&kmx61_pm_ops), + }, + .probe = kmx61_probe, + .remove = kmx61_remove, + .id_table = kmx61_id, +}; + +module_i2c_driver(kmx61_driver); + +MODULE_AUTHOR("Daniel Baluta "); +MODULE_DESCRIPTION("KMX61 accelerometer/magnetometer driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/imu/st_lsm6dsx/Kconfig b/drivers/iio/imu/st_lsm6dsx/Kconfig new file mode 100644 index 000000000..2ed2b3f40 --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/Kconfig @@ -0,0 +1,34 @@ +# SPDX-License-Identifier: GPL-2.0-only + +config IIO_ST_LSM6DSX + tristate "ST_LSM6DSx driver for STM 6-axis IMU MEMS sensors" + depends on (I2C || SPI || I3C) + select IIO_BUFFER + select IIO_KFIFO_BUF + select IIO_ST_LSM6DSX_I2C if (I2C) + select IIO_ST_LSM6DSX_SPI if (SPI_MASTER) + select IIO_ST_LSM6DSX_I3C if (I3C) + help + Say yes here to build support for STMicroelectronics LSM6DSx imu + sensor. Supported devices: lsm6ds3, lsm6ds3h, lsm6dsl, lsm6dsm, + ism330dlc, lsm6dso, lsm6dsox, asm330lhh, asm330lhhx, lsm6dsr, + lsm6ds3tr-c, ism330dhcx, lsm6dsrx, lsm6ds0, lsm6dsop, lsm6dstx, + the accelerometer/gyroscope of lsm9ds1 and lsm6dst. + + To compile this driver as a module, choose M here: the module + will be called st_lsm6dsx. + +config IIO_ST_LSM6DSX_I2C + tristate + depends on IIO_ST_LSM6DSX + select REGMAP_I2C + +config IIO_ST_LSM6DSX_SPI + tristate + depends on IIO_ST_LSM6DSX + select REGMAP_SPI + +config IIO_ST_LSM6DSX_I3C + tristate + depends on IIO_ST_LSM6DSX + select REGMAP_I3C diff --git a/drivers/iio/imu/st_lsm6dsx/Makefile b/drivers/iio/imu/st_lsm6dsx/Makefile new file mode 100644 index 000000000..57cbcd67d --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/Makefile @@ -0,0 +1,8 @@ +# SPDX-License-Identifier: GPL-2.0-only +st_lsm6dsx-y := st_lsm6dsx_core.o st_lsm6dsx_buffer.o \ + st_lsm6dsx_shub.o + +obj-$(CONFIG_IIO_ST_LSM6DSX) += st_lsm6dsx.o +obj-$(CONFIG_IIO_ST_LSM6DSX_I2C) += st_lsm6dsx_i2c.o +obj-$(CONFIG_IIO_ST_LSM6DSX_SPI) += st_lsm6dsx_spi.o +obj-$(CONFIG_IIO_ST_LSM6DSX_I3C) += st_lsm6dsx_i3c.o diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h new file mode 100644 index 000000000..6b57d47be --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h @@ -0,0 +1,518 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics st_lsm6dsx sensor driver + * + * Copyright 2016 STMicroelectronics Inc. + * + * Lorenzo Bianconi + * Denis Ciocca + */ + +#ifndef ST_LSM6DSX_H +#define ST_LSM6DSX_H + +#include +#include +#include + +#define ST_LSM6DS3_DEV_NAME "lsm6ds3" +#define ST_LSM6DS3H_DEV_NAME "lsm6ds3h" +#define ST_LSM6DSL_DEV_NAME "lsm6dsl" +#define ST_LSM6DSM_DEV_NAME "lsm6dsm" +#define ST_ISM330DLC_DEV_NAME "ism330dlc" +#define ST_LSM6DSO_DEV_NAME "lsm6dso" +#define ST_ASM330LHH_DEV_NAME "asm330lhh" +#define ST_LSM6DSOX_DEV_NAME "lsm6dsox" +#define ST_LSM6DSR_DEV_NAME "lsm6dsr" +#define ST_LSM6DS3TRC_DEV_NAME "lsm6ds3tr-c" +#define ST_ISM330DHCX_DEV_NAME "ism330dhcx" +#define ST_LSM9DS1_DEV_NAME "lsm9ds1-imu" +#define ST_LSM6DS0_DEV_NAME "lsm6ds0" +#define ST_LSM6DSRX_DEV_NAME "lsm6dsrx" +#define ST_LSM6DST_DEV_NAME "lsm6dst" +#define ST_LSM6DSOP_DEV_NAME "lsm6dsop" +#define ST_ASM330LHHX_DEV_NAME "asm330lhhx" +#define ST_LSM6DSTX_DEV_NAME "lsm6dstx" + +enum st_lsm6dsx_hw_id { + ST_LSM6DS3_ID, + ST_LSM6DS3H_ID, + ST_LSM6DSL_ID, + ST_LSM6DSM_ID, + ST_ISM330DLC_ID, + ST_LSM6DSO_ID, + ST_ASM330LHH_ID, + ST_LSM6DSOX_ID, + ST_LSM6DSR_ID, + ST_LSM6DS3TRC_ID, + ST_ISM330DHCX_ID, + ST_LSM9DS1_ID, + ST_LSM6DS0_ID, + ST_LSM6DSRX_ID, + ST_LSM6DST_ID, + ST_LSM6DSOP_ID, + ST_ASM330LHHX_ID, + ST_LSM6DSTX_ID, + ST_LSM6DSX_MAX_ID, +}; + +#define ST_LSM6DSX_BUFF_SIZE 512 +#define ST_LSM6DSX_CHAN_SIZE 2 +#define ST_LSM6DSX_SAMPLE_SIZE 6 +#define ST_LSM6DSX_TAG_SIZE 1 +#define ST_LSM6DSX_TAGGED_SAMPLE_SIZE (ST_LSM6DSX_SAMPLE_SIZE + \ + ST_LSM6DSX_TAG_SIZE) +#define ST_LSM6DSX_MAX_WORD_LEN ((32 / ST_LSM6DSX_SAMPLE_SIZE) * \ + ST_LSM6DSX_SAMPLE_SIZE) +#define ST_LSM6DSX_MAX_TAGGED_WORD_LEN ((32 / ST_LSM6DSX_TAGGED_SAMPLE_SIZE) \ + * ST_LSM6DSX_TAGGED_SAMPLE_SIZE) +#define ST_LSM6DSX_SHIFT_VAL(val, mask) (((val) << __ffs(mask)) & (mask)) + +#define ST_LSM6DSX_CHANNEL_ACC(chan_type, addr, mod, scan_idx) \ +{ \ + .type = chan_type, \ + .address = addr, \ + .modified = 1, \ + .channel2 = mod, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \ + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .scan_index = scan_idx, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_LE, \ + }, \ + .event_spec = &st_lsm6dsx_event, \ + .ext_info = st_lsm6dsx_accel_ext_info, \ + .num_event_specs = 1, \ +} + +#define ST_LSM6DSX_CHANNEL(chan_type, addr, mod, scan_idx) \ +{ \ + .type = chan_type, \ + .address = addr, \ + .modified = 1, \ + .channel2 = mod, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \ + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .scan_index = scan_idx, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_LE, \ + }, \ +} + +struct st_lsm6dsx_reg { + u8 addr; + u8 mask; +}; + +struct st_lsm6dsx_sensor; +struct st_lsm6dsx_hw; + +struct st_lsm6dsx_odr { + u32 milli_hz; + u8 val; +}; + +#define ST_LSM6DSX_ODR_LIST_SIZE 8 +struct st_lsm6dsx_odr_table_entry { + struct st_lsm6dsx_reg reg; + + struct st_lsm6dsx_odr odr_avl[ST_LSM6DSX_ODR_LIST_SIZE]; + int odr_len; +}; + +struct st_lsm6dsx_fs { + u32 gain; + u8 val; +}; + +#define ST_LSM6DSX_FS_LIST_SIZE 4 +struct st_lsm6dsx_fs_table_entry { + struct st_lsm6dsx_reg reg; + + struct st_lsm6dsx_fs fs_avl[ST_LSM6DSX_FS_LIST_SIZE]; + int fs_len; +}; + +/** + * struct st_lsm6dsx_fifo_ops - ST IMU FIFO settings + * @update_fifo: Update FIFO configuration callback. + * @read_fifo: Read FIFO callback. + * @fifo_th: FIFO threshold register info (addr + mask). + * @fifo_diff: FIFO diff status register info (addr + mask). + * @max_size: Sensor max fifo length in FIFO words. + * @th_wl: FIFO threshold word length. + */ +struct st_lsm6dsx_fifo_ops { + int (*update_fifo)(struct st_lsm6dsx_sensor *sensor, bool enable); + int (*read_fifo)(struct st_lsm6dsx_hw *hw); + struct { + u8 addr; + u16 mask; + } fifo_th; + struct { + u8 addr; + u16 mask; + } fifo_diff; + u16 max_size; + u8 th_wl; +}; + +/** + * struct st_lsm6dsx_hw_ts_settings - ST IMU hw timer settings + * @timer_en: Hw timer enable register info (addr + mask). + * @hr_timer: Hw timer resolution register info (addr + mask). + * @fifo_en: Hw timer FIFO enable register info (addr + mask). + * @decimator: Hw timer FIFO decimator register info (addr + mask). + * @freq_fine: Difference in % of ODR with respect to the typical. + */ +struct st_lsm6dsx_hw_ts_settings { + struct st_lsm6dsx_reg timer_en; + struct st_lsm6dsx_reg hr_timer; + struct st_lsm6dsx_reg fifo_en; + struct st_lsm6dsx_reg decimator; + u8 freq_fine; +}; + +/** + * struct st_lsm6dsx_shub_settings - ST IMU hw i2c controller settings + * @page_mux: register page mux info (addr + mask). + * @master_en: master config register info (addr + mask). + * @pullup_en: i2c controller pull-up register info (addr + mask). + * @aux_sens: aux sensor register info (addr + mask). + * @wr_once: write_once register info (addr + mask). + * @emb_func: embedded function register info (addr + mask). + * @num_ext_dev: max number of slave devices. + * @shub_out: sensor hub first output register info. + * @slv0_addr: slave0 address in secondary page. + * @dw_slv0_addr: slave0 write register address in secondary page. + * @batch_en: Enable/disable FIFO batching. + * @pause: controller pause value. + */ +struct st_lsm6dsx_shub_settings { + struct st_lsm6dsx_reg page_mux; + struct { + bool sec_page; + u8 addr; + u8 mask; + } master_en; + struct { + bool sec_page; + u8 addr; + u8 mask; + } pullup_en; + struct st_lsm6dsx_reg aux_sens; + struct st_lsm6dsx_reg wr_once; + struct st_lsm6dsx_reg emb_func; + u8 num_ext_dev; + struct { + bool sec_page; + u8 addr; + } shub_out; + u8 slv0_addr; + u8 dw_slv0_addr; + u8 batch_en; + u8 pause; +}; + +struct st_lsm6dsx_event_settings { + struct st_lsm6dsx_reg enable_reg; + struct st_lsm6dsx_reg wakeup_reg; + u8 wakeup_src_reg; + u8 wakeup_src_status_mask; + u8 wakeup_src_z_mask; + u8 wakeup_src_y_mask; + u8 wakeup_src_x_mask; +}; + +enum st_lsm6dsx_ext_sensor_id { + ST_LSM6DSX_ID_MAGN, +}; + +/** + * struct st_lsm6dsx_ext_dev_settings - i2c controller slave settings + * @i2c_addr: I2c slave address list. + * @wai: Wai address info. + * @id: external sensor id. + * @odr_table: Output data rate of the sensor [Hz]. + * @fs_table: Configured sensor sensitivity table depending on full scale. + * @temp_comp: Temperature compensation register info (addr + mask). + * @pwr_table: Power on register info (addr + mask). + * @off_canc: Offset cancellation register info (addr + mask). + * @bdu: Block data update register info (addr + mask). + * @out: Output register info. + */ +struct st_lsm6dsx_ext_dev_settings { + u8 i2c_addr[2]; + struct { + u8 addr; + u8 val; + } wai; + enum st_lsm6dsx_ext_sensor_id id; + struct st_lsm6dsx_odr_table_entry odr_table; + struct st_lsm6dsx_fs_table_entry fs_table; + struct st_lsm6dsx_reg temp_comp; + struct { + struct st_lsm6dsx_reg reg; + u8 off_val; + u8 on_val; + } pwr_table; + struct st_lsm6dsx_reg off_canc; + struct st_lsm6dsx_reg bdu; + struct { + u8 addr; + u8 len; + } out; +}; + +/** + * struct st_lsm6dsx_settings - ST IMU sensor settings + * @reset: register address for reset. + * @boot: register address for boot. + * @bdu: register address for Block Data Update. + * @id: List of hw id/device name supported by the driver configuration. + * @channels: IIO channels supported by the device. + * @irq_config: interrupts related registers. + * @drdy_mask: register info for data-ready mask (addr + mask). + * @odr_table: Hw sensors odr table (Hz + val). + * @fs_table: Hw sensors gain table (gain + val). + * @decimator: List of decimator register info (addr + mask). + * @batch: List of FIFO batching register info (addr + mask). + * @fifo_ops: Sensor hw FIFO parameters. + * @ts_settings: Hw timer related settings. + * @shub_settings: i2c controller related settings. + */ +struct st_lsm6dsx_settings { + struct st_lsm6dsx_reg reset; + struct st_lsm6dsx_reg boot; + struct st_lsm6dsx_reg bdu; + struct { + enum st_lsm6dsx_hw_id hw_id; + const char *name; + u8 wai; + } id[ST_LSM6DSX_MAX_ID]; + struct { + const struct iio_chan_spec *chan; + int len; + } channels[2]; + struct { + struct st_lsm6dsx_reg irq1; + struct st_lsm6dsx_reg irq2; + struct st_lsm6dsx_reg irq1_func; + struct st_lsm6dsx_reg irq2_func; + struct st_lsm6dsx_reg lir; + struct st_lsm6dsx_reg clear_on_read; + struct st_lsm6dsx_reg hla; + struct st_lsm6dsx_reg od; + } irq_config; + struct st_lsm6dsx_reg drdy_mask; + struct st_lsm6dsx_odr_table_entry odr_table[2]; + struct st_lsm6dsx_fs_table_entry fs_table[2]; + struct st_lsm6dsx_reg decimator[ST_LSM6DSX_MAX_ID]; + struct st_lsm6dsx_reg batch[ST_LSM6DSX_MAX_ID]; + struct st_lsm6dsx_fifo_ops fifo_ops; + struct st_lsm6dsx_hw_ts_settings ts_settings; + struct st_lsm6dsx_shub_settings shub_settings; + struct st_lsm6dsx_event_settings event_settings; +}; + +enum st_lsm6dsx_sensor_id { + ST_LSM6DSX_ID_GYRO, + ST_LSM6DSX_ID_ACC, + ST_LSM6DSX_ID_EXT0, + ST_LSM6DSX_ID_EXT1, + ST_LSM6DSX_ID_EXT2, + ST_LSM6DSX_ID_MAX, +}; + +enum st_lsm6dsx_fifo_mode { + ST_LSM6DSX_FIFO_BYPASS = 0x0, + ST_LSM6DSX_FIFO_CONT = 0x6, +}; + +/** + * struct st_lsm6dsx_sensor - ST IMU sensor instance + * @name: Sensor name. + * @id: Sensor identifier. + * @hw: Pointer to instance of struct st_lsm6dsx_hw. + * @gain: Configured sensor sensitivity. + * @odr: Output data rate of the sensor [Hz]. + * @watermark: Sensor watermark level. + * @decimator: Sensor decimation factor. + * @sip: Number of samples in a given pattern. + * @ts_ref: Sensor timestamp reference for hw one. + * @ext_info: Sensor settings if it is connected to i2c controller + */ +struct st_lsm6dsx_sensor { + char name[32]; + enum st_lsm6dsx_sensor_id id; + struct st_lsm6dsx_hw *hw; + + u32 gain; + u32 odr; + + u16 watermark; + u8 decimator; + u8 sip; + s64 ts_ref; + + struct { + const struct st_lsm6dsx_ext_dev_settings *settings; + u32 slv_odr; + u8 addr; + } ext_info; +}; + +/** + * struct st_lsm6dsx_hw - ST IMU MEMS hw instance + * @dev: Pointer to instance of struct device (I2C or SPI). + * @regmap: Register map of the device. + * @regulators: VDD/VDDIO voltage regulators. + * @irq: Device interrupt line (I2C or SPI). + * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO. + * @conf_lock: Mutex to prevent concurrent FIFO configuration update. + * @page_lock: Mutex to prevent concurrent memory page configuration. + * @suspend_mask: Suspended sensor bitmask. + * @enable_mask: Enabled sensor bitmask. + * @fifo_mask: Enabled hw FIFO bitmask. + * @ts_gain: Hw timestamp rate after internal calibration. + * @ts_sip: Total number of timestamp samples in a given pattern. + * @sip: Total number of samples (acc/gyro/ts) in a given pattern. + * @buff: Device read buffer. + * @irq_routing: pointer to interrupt routing configuration. + * @event_threshold: wakeup event threshold. + * @enable_event: enabled event bitmask. + * @iio_devs: Pointers to acc/gyro iio_dev instances. + * @settings: Pointer to the specific sensor settings in use. + * @orientation: sensor chip orientation relative to main hardware. + * @scan: Temporary buffers used to align data before iio_push_to_buffers() + */ +struct st_lsm6dsx_hw { + struct device *dev; + struct regmap *regmap; + struct regulator_bulk_data regulators[2]; + int irq; + + struct mutex fifo_lock; + struct mutex conf_lock; + struct mutex page_lock; + + u8 suspend_mask; + u8 enable_mask; + u8 fifo_mask; + s64 ts_gain; + u8 ts_sip; + u8 sip; + + const struct st_lsm6dsx_reg *irq_routing; + u8 event_threshold; + u8 enable_event; + + u8 *buff; + + struct iio_dev *iio_devs[ST_LSM6DSX_ID_MAX]; + + const struct st_lsm6dsx_settings *settings; + + struct iio_mount_matrix orientation; + /* Ensure natural alignment of buffer elements */ + struct { + __le16 channels[3]; + s64 ts __aligned(8); + } scan[3]; +}; + +static __maybe_unused const struct iio_event_spec st_lsm6dsx_event = { + .type = IIO_EV_TYPE_THRESH, + .dir = IIO_EV_DIR_EITHER, + .mask_separate = BIT(IIO_EV_INFO_VALUE) | + BIT(IIO_EV_INFO_ENABLE) +}; + +static __maybe_unused const unsigned long st_lsm6dsx_available_scan_masks[] = { + 0x7, 0x0, +}; + +extern const struct dev_pm_ops st_lsm6dsx_pm_ops; + +int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, + struct regmap *regmap); +int st_lsm6dsx_sensor_set_enable(struct st_lsm6dsx_sensor *sensor, + bool enable); +int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw); +int st_lsm6dsx_set_watermark(struct iio_dev *iio_dev, unsigned int val); +int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, + u16 watermark); +int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable); +int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw); +int st_lsm6dsx_resume_fifo(struct st_lsm6dsx_hw *hw); +int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw); +int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw); +int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, u32 odr, u8 *val); +int st_lsm6dsx_shub_probe(struct st_lsm6dsx_hw *hw, const char *name); +int st_lsm6dsx_shub_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable); +int st_lsm6dsx_set_page(struct st_lsm6dsx_hw *hw, bool enable); + +static inline int +st_lsm6dsx_update_bits_locked(struct st_lsm6dsx_hw *hw, unsigned int addr, + unsigned int mask, unsigned int val) +{ + int err; + + mutex_lock(&hw->page_lock); + err = regmap_update_bits(hw->regmap, addr, mask, val); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lsm6dsx_read_locked(struct st_lsm6dsx_hw *hw, unsigned int addr, + void *val, unsigned int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = regmap_bulk_read(hw->regmap, addr, val, len); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline int +st_lsm6dsx_write_locked(struct st_lsm6dsx_hw *hw, unsigned int addr, + unsigned int val) +{ + int err; + + mutex_lock(&hw->page_lock); + err = regmap_write(hw->regmap, addr, val); + mutex_unlock(&hw->page_lock); + + return err; +} + +static inline const struct iio_mount_matrix * +st_lsm6dsx_get_mount_matrix(const struct iio_dev *iio_dev, + const struct iio_chan_spec *chan) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsx_hw *hw = sensor->hw; + + return &hw->orientation; +} + +static const +struct iio_chan_spec_ext_info __maybe_unused st_lsm6dsx_accel_ext_info[] = { + IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, st_lsm6dsx_get_mount_matrix), + { } +}; + +#endif /* ST_LSM6DSX_H */ diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c new file mode 100644 index 000000000..e49f2d120 --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c @@ -0,0 +1,756 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsx FIFO buffer library driver + * + * LSM6DS3/LSM6DS3H/LSM6DSL/LSM6DSM/ISM330DLC/LSM6DS3TR-C: + * The FIFO buffer can be configured to store data from gyroscope and + * accelerometer. Samples are queued without any tag according to a + * specific pattern based on 'FIFO data sets' (6 bytes each): + * - 1st data set is reserved for gyroscope data + * - 2nd data set is reserved for accelerometer data + * The FIFO pattern changes depending on the ODRs and decimation factors + * assigned to the FIFO data sets. The first sequence of data stored in FIFO + * buffer contains the data of all the enabled FIFO data sets + * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the + * value of the decimation factor and ODR set for each FIFO data set. + * + * LSM6DSO/LSM6DSOX/ASM330LHH/ASM330LHHX/LSM6DSR/LSM6DSRX/ISM330DHCX/ + * LSM6DST/LSM6DSOP/LSM6DSTX: + * The FIFO buffer can be configured to store data from gyroscope and + * accelerometer. Each sample is queued with a tag (1B) indicating data + * source (gyroscope, accelerometer, hw timer). + * + * FIFO supported modes: + * - BYPASS: FIFO disabled + * - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index + * restarts from the beginning and the oldest sample is overwritten + * + * Copyright 2016 STMicroelectronics Inc. + * + * Lorenzo Bianconi + * Denis Ciocca + */ +#include +#include +#include +#include +#include +#include + +#include + +#include "st_lsm6dsx.h" + +#define ST_LSM6DSX_REG_FIFO_MODE_ADDR 0x0a +#define ST_LSM6DSX_FIFO_MODE_MASK GENMASK(2, 0) +#define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3) +#define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12) +#define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e +#define ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR 0x78 +#define ST_LSM6DSX_REG_TS_RESET_ADDR 0x42 + +#define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08 + +#define ST_LSM6DSX_TS_RESET_VAL 0xaa + +struct st_lsm6dsx_decimator_entry { + u8 decimator; + u8 val; +}; + +enum st_lsm6dsx_fifo_tag { + ST_LSM6DSX_GYRO_TAG = 0x01, + ST_LSM6DSX_ACC_TAG = 0x02, + ST_LSM6DSX_TS_TAG = 0x04, + ST_LSM6DSX_EXT0_TAG = 0x0f, + ST_LSM6DSX_EXT1_TAG = 0x10, + ST_LSM6DSX_EXT2_TAG = 0x11, +}; + +static const +struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = { + { 0, 0x0 }, + { 1, 0x1 }, + { 2, 0x2 }, + { 3, 0x3 }, + { 4, 0x4 }, + { 8, 0x5 }, + { 16, 0x6 }, + { 32, 0x7 }, +}; + +static int +st_lsm6dsx_get_decimator_val(struct st_lsm6dsx_sensor *sensor, u32 max_odr) +{ + const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table); + u32 decimator = max_odr / sensor->odr; + int i; + + if (decimator > 1) + decimator = round_down(decimator, 2); + + for (i = 0; i < max_size; i++) { + if (st_lsm6dsx_decimator_table[i].decimator == decimator) + break; + } + + sensor->decimator = decimator; + return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val; +} + +static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw, + u32 *max_odr, u32 *min_odr) +{ + struct st_lsm6dsx_sensor *sensor; + int i; + + *max_odr = 0, *min_odr = ~0; + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + if (!hw->iio_devs[i]) + continue; + + sensor = iio_priv(hw->iio_devs[i]); + + if (!(hw->enable_mask & BIT(sensor->id))) + continue; + + *max_odr = max_t(u32, *max_odr, sensor->odr); + *min_odr = min_t(u32, *min_odr, sensor->odr); + } +} + +static u8 st_lsm6dsx_get_sip(struct st_lsm6dsx_sensor *sensor, u32 min_odr) +{ + u8 sip = sensor->odr / min_odr; + + return sip > 1 ? round_down(sip, 2) : sip; +} + +static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw) +{ + const struct st_lsm6dsx_reg *ts_dec_reg; + struct st_lsm6dsx_sensor *sensor; + u16 sip = 0, ts_sip = 0; + u32 max_odr, min_odr; + int err = 0, i; + u8 data; + + st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr); + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + const struct st_lsm6dsx_reg *dec_reg; + + if (!hw->iio_devs[i]) + continue; + + sensor = iio_priv(hw->iio_devs[i]); + /* update fifo decimators and sample in pattern */ + if (hw->enable_mask & BIT(sensor->id)) { + sensor->sip = st_lsm6dsx_get_sip(sensor, min_odr); + data = st_lsm6dsx_get_decimator_val(sensor, max_odr); + } else { + sensor->sip = 0; + data = 0; + } + ts_sip = max_t(u16, ts_sip, sensor->sip); + + dec_reg = &hw->settings->decimator[sensor->id]; + if (dec_reg->addr) { + int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask); + + err = st_lsm6dsx_update_bits_locked(hw, dec_reg->addr, + dec_reg->mask, + val); + if (err < 0) + return err; + } + sip += sensor->sip; + } + hw->sip = sip + ts_sip; + hw->ts_sip = ts_sip; + + /* + * update hw ts decimator if necessary. Decimator for hw timestamp + * is always 1 or 0 in order to have a ts sample for each data + * sample in FIFO + */ + ts_dec_reg = &hw->settings->ts_settings.decimator; + if (ts_dec_reg->addr) { + int val, ts_dec = !!hw->ts_sip; + + val = ST_LSM6DSX_SHIFT_VAL(ts_dec, ts_dec_reg->mask); + err = st_lsm6dsx_update_bits_locked(hw, ts_dec_reg->addr, + ts_dec_reg->mask, val); + } + return err; +} + +static int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw, + enum st_lsm6dsx_fifo_mode fifo_mode) +{ + unsigned int data; + + data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode); + return st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR, + ST_LSM6DSX_FIFO_MODE_MASK, data); +} + +static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor, + bool enable) +{ + struct st_lsm6dsx_hw *hw = sensor->hw; + const struct st_lsm6dsx_reg *batch_reg; + u8 data; + + batch_reg = &hw->settings->batch[sensor->id]; + if (batch_reg->addr) { + int val; + + if (enable) { + int err; + + err = st_lsm6dsx_check_odr(sensor, sensor->odr, + &data); + if (err < 0) + return err; + } else { + data = 0; + } + val = ST_LSM6DSX_SHIFT_VAL(data, batch_reg->mask); + return st_lsm6dsx_update_bits_locked(hw, batch_reg->addr, + batch_reg->mask, val); + } else { + data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0; + return st_lsm6dsx_update_bits_locked(hw, + ST_LSM6DSX_REG_FIFO_MODE_ADDR, + ST_LSM6DSX_FIFO_ODR_MASK, + FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK, + data)); + } +} + +int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark) +{ + u16 fifo_watermark = ~0, cur_watermark, fifo_th_mask; + struct st_lsm6dsx_hw *hw = sensor->hw; + struct st_lsm6dsx_sensor *cur_sensor; + int i, err, data; + __le16 wdata; + + if (!hw->sip) + return 0; + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + if (!hw->iio_devs[i]) + continue; + + cur_sensor = iio_priv(hw->iio_devs[i]); + + if (!(hw->enable_mask & BIT(cur_sensor->id))) + continue; + + cur_watermark = (cur_sensor == sensor) ? watermark + : cur_sensor->watermark; + + fifo_watermark = min_t(u16, fifo_watermark, cur_watermark); + } + + fifo_watermark = max_t(u16, fifo_watermark, hw->sip); + fifo_watermark = (fifo_watermark / hw->sip) * hw->sip; + fifo_watermark = fifo_watermark * hw->settings->fifo_ops.th_wl; + + mutex_lock(&hw->page_lock); + err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1, + &data); + if (err < 0) + goto out; + + fifo_th_mask = hw->settings->fifo_ops.fifo_th.mask; + fifo_watermark = ((data << 8) & ~fifo_th_mask) | + (fifo_watermark & fifo_th_mask); + + wdata = cpu_to_le16(fifo_watermark); + err = regmap_bulk_write(hw->regmap, + hw->settings->fifo_ops.fifo_th.addr, + &wdata, sizeof(wdata)); +out: + mutex_unlock(&hw->page_lock); + return err; +} + +static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw) +{ + struct st_lsm6dsx_sensor *sensor; + int i, err; + + /* reset hw ts counter */ + err = st_lsm6dsx_write_locked(hw, ST_LSM6DSX_REG_TS_RESET_ADDR, + ST_LSM6DSX_TS_RESET_VAL); + if (err < 0) + return err; + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + if (!hw->iio_devs[i]) + continue; + + sensor = iio_priv(hw->iio_devs[i]); + /* + * store enable buffer timestamp as reference for + * hw timestamp + */ + sensor->ts_ref = iio_get_time_ns(hw->iio_devs[i]); + } + return 0; +} + +int st_lsm6dsx_resume_fifo(struct st_lsm6dsx_hw *hw) +{ + int err; + + /* reset hw ts counter */ + err = st_lsm6dsx_reset_hw_ts(hw); + if (err < 0) + return err; + + return st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT); +} + +/* + * Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN/ST_LSM6DSX_MAX_TAGGED_WORD_LEN + * in order to avoid a kmalloc for each bus access + */ +static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr, + u8 *data, unsigned int data_len, + unsigned int max_word_len) +{ + unsigned int word_len, read_len = 0; + int err; + + while (read_len < data_len) { + word_len = min_t(unsigned int, data_len - read_len, + max_word_len); + err = st_lsm6dsx_read_locked(hw, addr, data + read_len, + word_len); + if (err < 0) + return err; + read_len += word_len; + } + return 0; +} + +#define ST_LSM6DSX_IIO_BUFF_SIZE (ALIGN(ST_LSM6DSX_SAMPLE_SIZE, \ + sizeof(s64)) + sizeof(s64)) +/** + * st_lsm6dsx_read_fifo() - hw FIFO read routine + * @hw: Pointer to instance of struct st_lsm6dsx_hw. + * + * Read samples from the hw FIFO and push them to IIO buffers. + * + * Return: Number of bytes read from the FIFO + */ +int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw) +{ + struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor, *ext_sensor = NULL; + int err, sip, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset; + u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE; + u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask; + bool reset_ts = false; + __le16 fifo_status; + s64 ts = 0; + + err = st_lsm6dsx_read_locked(hw, + hw->settings->fifo_ops.fifo_diff.addr, + &fifo_status, sizeof(fifo_status)); + if (err < 0) { + dev_err(hw->dev, "failed to read fifo status (err=%d)\n", + err); + return err; + } + + if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK)) + return 0; + + fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) * + ST_LSM6DSX_CHAN_SIZE; + fifo_len = (fifo_len / pattern_len) * pattern_len; + + acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]); + gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]); + if (hw->iio_devs[ST_LSM6DSX_ID_EXT0]) + ext_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_EXT0]); + + for (read_len = 0; read_len < fifo_len; read_len += pattern_len) { + err = st_lsm6dsx_read_block(hw, ST_LSM6DSX_REG_FIFO_OUTL_ADDR, + hw->buff, pattern_len, + ST_LSM6DSX_MAX_WORD_LEN); + if (err < 0) { + dev_err(hw->dev, + "failed to read pattern from fifo (err=%d)\n", + err); + return err; + } + + /* + * Data are written to the FIFO with a specific pattern + * depending on the configured ODRs. The first sequence of data + * stored in FIFO contains the data of all enabled sensors + * (e.g. Gx, Gy, Gz, Ax, Ay, Az, Ts), then data are repeated + * depending on the value of the decimation factor set for each + * sensor. + * + * Supposing the FIFO is storing data from gyroscope and + * accelerometer at different ODRs: + * - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz + * Since the gyroscope ODR is twice the accelerometer one, the + * following pattern is repeated every 9 samples: + * - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, .. + */ + ext_sip = ext_sensor ? ext_sensor->sip : 0; + gyro_sip = gyro_sensor->sip; + acc_sip = acc_sensor->sip; + ts_sip = hw->ts_sip; + offset = 0; + sip = 0; + + while (acc_sip > 0 || gyro_sip > 0 || ext_sip > 0) { + if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) { + memcpy(hw->scan[ST_LSM6DSX_ID_GYRO].channels, + &hw->buff[offset], + sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels)); + offset += sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels); + } + if (acc_sip > 0 && !(sip % acc_sensor->decimator)) { + memcpy(hw->scan[ST_LSM6DSX_ID_ACC].channels, + &hw->buff[offset], + sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels)); + offset += sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels); + } + if (ext_sip > 0 && !(sip % ext_sensor->decimator)) { + memcpy(hw->scan[ST_LSM6DSX_ID_EXT0].channels, + &hw->buff[offset], + sizeof(hw->scan[ST_LSM6DSX_ID_EXT0].channels)); + offset += sizeof(hw->scan[ST_LSM6DSX_ID_EXT0].channels); + } + + if (ts_sip-- > 0) { + u8 data[ST_LSM6DSX_SAMPLE_SIZE]; + + memcpy(data, &hw->buff[offset], sizeof(data)); + /* + * hw timestamp is 3B long and it is stored + * in FIFO using 6B as 4th FIFO data set + * according to this schema: + * B0 = ts[15:8], B1 = ts[23:16], B3 = ts[7:0] + */ + ts = data[1] << 16 | data[0] << 8 | data[3]; + /* + * check if hw timestamp engine is going to + * reset (the sensor generates an interrupt + * to signal the hw timestamp will reset in + * 1.638s) + */ + if (!reset_ts && ts >= 0xff0000) + reset_ts = true; + ts *= hw->ts_gain; + + offset += ST_LSM6DSX_SAMPLE_SIZE; + } + + if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) { + iio_push_to_buffers_with_timestamp( + hw->iio_devs[ST_LSM6DSX_ID_GYRO], + &hw->scan[ST_LSM6DSX_ID_GYRO], + gyro_sensor->ts_ref + ts); + gyro_sip--; + } + if (acc_sip > 0 && !(sip % acc_sensor->decimator)) { + iio_push_to_buffers_with_timestamp( + hw->iio_devs[ST_LSM6DSX_ID_ACC], + &hw->scan[ST_LSM6DSX_ID_ACC], + acc_sensor->ts_ref + ts); + acc_sip--; + } + if (ext_sip > 0 && !(sip % ext_sensor->decimator)) { + iio_push_to_buffers_with_timestamp( + hw->iio_devs[ST_LSM6DSX_ID_EXT0], + &hw->scan[ST_LSM6DSX_ID_EXT0], + ext_sensor->ts_ref + ts); + ext_sip--; + } + sip++; + } + } + + if (unlikely(reset_ts)) { + err = st_lsm6dsx_reset_hw_ts(hw); + if (err < 0) { + dev_err(hw->dev, "failed to reset hw ts (err=%d)\n", + err); + return err; + } + } + return read_len; +} + +#define ST_LSM6DSX_INVALID_SAMPLE 0x7ffd +static int +st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag, + u8 *data, s64 ts) +{ + s16 val = le16_to_cpu(*(__le16 *)data); + struct st_lsm6dsx_sensor *sensor; + struct iio_dev *iio_dev; + + /* invalid sample during bootstrap phase */ + if (val >= ST_LSM6DSX_INVALID_SAMPLE) + return -EINVAL; + + /* + * EXT_TAG are managed in FIFO fashion so ST_LSM6DSX_EXT0_TAG + * corresponds to the first enabled channel, ST_LSM6DSX_EXT1_TAG + * to the second one and ST_LSM6DSX_EXT2_TAG to the last enabled + * channel + */ + switch (tag) { + case ST_LSM6DSX_GYRO_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSX_ID_GYRO]; + break; + case ST_LSM6DSX_ACC_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSX_ID_ACC]; + break; + case ST_LSM6DSX_EXT0_TAG: + if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) + iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT0]; + else if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1)) + iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1]; + else + iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2]; + break; + case ST_LSM6DSX_EXT1_TAG: + if ((hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) && + (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1))) + iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1]; + else + iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2]; + break; + case ST_LSM6DSX_EXT2_TAG: + iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2]; + break; + default: + return -EINVAL; + } + + sensor = iio_priv(iio_dev); + iio_push_to_buffers_with_timestamp(iio_dev, data, + ts + sensor->ts_ref); + + return 0; +} + +/** + * st_lsm6dsx_read_tagged_fifo() - tagged hw FIFO read routine + * @hw: Pointer to instance of struct st_lsm6dsx_hw. + * + * Read samples from the hw FIFO and push them to IIO buffers. + * + * Return: Number of bytes read from the FIFO + */ +int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw) +{ + u16 pattern_len = hw->sip * ST_LSM6DSX_TAGGED_SAMPLE_SIZE; + u16 fifo_len, fifo_diff_mask; + /* + * Alignment needed as this can ultimately be passed to a + * call to iio_push_to_buffers_with_timestamp() which + * must be passed a buffer that is aligned to 8 bytes so + * as to allow insertion of a naturally aligned timestamp. + */ + u8 iio_buff[ST_LSM6DSX_IIO_BUFF_SIZE] __aligned(8); + u8 tag; + bool reset_ts = false; + int i, err, read_len; + __le16 fifo_status; + s64 ts = 0; + + err = st_lsm6dsx_read_locked(hw, + hw->settings->fifo_ops.fifo_diff.addr, + &fifo_status, sizeof(fifo_status)); + if (err < 0) { + dev_err(hw->dev, "failed to read fifo status (err=%d)\n", + err); + return err; + } + + fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask; + fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) * + ST_LSM6DSX_TAGGED_SAMPLE_SIZE; + if (!fifo_len) + return 0; + + for (read_len = 0; read_len < fifo_len; read_len += pattern_len) { + err = st_lsm6dsx_read_block(hw, + ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR, + hw->buff, pattern_len, + ST_LSM6DSX_MAX_TAGGED_WORD_LEN); + if (err < 0) { + dev_err(hw->dev, + "failed to read pattern from fifo (err=%d)\n", + err); + return err; + } + + for (i = 0; i < pattern_len; + i += ST_LSM6DSX_TAGGED_SAMPLE_SIZE) { + memcpy(iio_buff, &hw->buff[i + ST_LSM6DSX_TAG_SIZE], + ST_LSM6DSX_SAMPLE_SIZE); + + tag = hw->buff[i] >> 3; + if (tag == ST_LSM6DSX_TS_TAG) { + /* + * hw timestamp is 4B long and it is stored + * in FIFO according to this schema: + * B0 = ts[7:0], B1 = ts[15:8], B2 = ts[23:16], + * B3 = ts[31:24] + */ + ts = le32_to_cpu(*((__le32 *)iio_buff)); + /* + * check if hw timestamp engine is going to + * reset (the sensor generates an interrupt + * to signal the hw timestamp will reset in + * 1.638s) + */ + if (!reset_ts && ts >= 0xffff0000) + reset_ts = true; + ts *= hw->ts_gain; + } else { + st_lsm6dsx_push_tagged_data(hw, tag, iio_buff, + ts); + } + } + } + + if (unlikely(reset_ts)) { + err = st_lsm6dsx_reset_hw_ts(hw); + if (err < 0) + return err; + } + return read_len; +} + +int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw) +{ + int err; + + if (!hw->settings->fifo_ops.read_fifo) + return -ENOTSUPP; + + mutex_lock(&hw->fifo_lock); + + hw->settings->fifo_ops.read_fifo(hw); + err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS); + + mutex_unlock(&hw->fifo_lock); + + return err; +} + +int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable) +{ + struct st_lsm6dsx_hw *hw = sensor->hw; + u8 fifo_mask; + int err; + + mutex_lock(&hw->conf_lock); + + if (enable) + fifo_mask = hw->fifo_mask | BIT(sensor->id); + else + fifo_mask = hw->fifo_mask & ~BIT(sensor->id); + + if (hw->fifo_mask) { + err = st_lsm6dsx_flush_fifo(hw); + if (err < 0) + goto out; + } + + if (sensor->id == ST_LSM6DSX_ID_EXT0 || + sensor->id == ST_LSM6DSX_ID_EXT1 || + sensor->id == ST_LSM6DSX_ID_EXT2) { + err = st_lsm6dsx_shub_set_enable(sensor, enable); + if (err < 0) + goto out; + } else { + err = st_lsm6dsx_sensor_set_enable(sensor, enable); + if (err < 0) + goto out; + } + + err = st_lsm6dsx_set_fifo_odr(sensor, enable); + if (err < 0) + goto out; + + err = st_lsm6dsx_update_decimators(hw); + if (err < 0) + goto out; + + err = st_lsm6dsx_update_watermark(sensor, sensor->watermark); + if (err < 0) + goto out; + + if (fifo_mask) { + err = st_lsm6dsx_resume_fifo(hw); + if (err < 0) + goto out; + } + + hw->fifo_mask = fifo_mask; + +out: + mutex_unlock(&hw->conf_lock); + + return err; +} + +static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsx_hw *hw = sensor->hw; + + if (!hw->settings->fifo_ops.update_fifo) + return -ENOTSUPP; + + return hw->settings->fifo_ops.update_fifo(sensor, true); +} + +static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsx_hw *hw = sensor->hw; + + if (!hw->settings->fifo_ops.update_fifo) + return -ENOTSUPP; + + return hw->settings->fifo_ops.update_fifo(sensor, false); +} + +static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = { + .preenable = st_lsm6dsx_buffer_preenable, + .postdisable = st_lsm6dsx_buffer_postdisable, +}; + +int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw) +{ + int i, ret; + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + if (!hw->iio_devs[i]) + continue; + + ret = devm_iio_kfifo_buffer_setup(hw->dev, hw->iio_devs[i], + &st_lsm6dsx_buffer_ops); + if (ret) + return ret; + } + + return 0; +} diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c new file mode 100644 index 000000000..f8bbb0057 --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c @@ -0,0 +1,2380 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsx sensor driver + * + * The ST LSM6DSx IMU MEMS series consists of 3D digital accelerometer + * and 3D digital gyroscope system-in-package with a digital I2C/SPI serial + * interface standard output. + * LSM6DSx IMU MEMS series has a dynamic user-selectable full-scale + * acceleration range of +-2/+-4/+-8/+-16 g and an angular rate range of + * +-125/+-245/+-500/+-1000/+-2000 dps + * LSM6DSx series has an integrated First-In-First-Out (FIFO) buffer + * allowing dynamic batching of sensor data. + * LSM9DSx series is similar but includes an additional magnetometer, handled + * by a different driver. + * + * Supported sensors: + * - LSM6DS3: + * - Accelerometer/Gyroscope supported ODR [Hz]: 12.5, 26, 52, 104, 208, 416 + * - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16 + * - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000 + * - FIFO size: 8KB + * + * - LSM6DS3H/LSM6DSL/LSM6DSM/ISM330DLC/LSM6DS3TR-C: + * - Accelerometer/Gyroscope supported ODR [Hz]: 12.5, 26, 52, 104, 208, 416 + * - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16 + * - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000 + * - FIFO size: 4KB + * + * - LSM6DSO/LSM6DSOX/ASM330LHH/ASM330LHHX/LSM6DSR/ISM330DHCX/LSM6DST/LSM6DSOP/ + * LSM6DSTX: + * - Accelerometer/Gyroscope supported ODR [Hz]: 12.5, 26, 52, 104, 208, 416, + * 833 + * - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16 + * - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000 + * - FIFO size: 3KB + * + * - LSM9DS1/LSM6DS0: + * - Accelerometer supported ODR [Hz]: 10, 50, 119, 238, 476, 952 + * - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16 + * - Gyroscope supported ODR [Hz]: 15, 60, 119, 238, 476, 952 + * - Gyroscope supported full-scale [dps]: +-245/+-500/+-2000 + * - FIFO size: 32 + * + * Copyright 2016 STMicroelectronics Inc. + * + * Lorenzo Bianconi + * Denis Ciocca + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "st_lsm6dsx.h" + +#define ST_LSM6DSX_REG_WHOAMI_ADDR 0x0f + +#define ST_LSM6DSX_TS_SENSITIVITY 25000UL /* 25us */ + +static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = { + ST_LSM6DSX_CHANNEL_ACC(IIO_ACCEL, 0x28, IIO_MOD_X, 0), + ST_LSM6DSX_CHANNEL_ACC(IIO_ACCEL, 0x2a, IIO_MOD_Y, 1), + ST_LSM6DSX_CHANNEL_ACC(IIO_ACCEL, 0x2c, IIO_MOD_Z, 2), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static const struct iio_chan_spec st_lsm6dsx_gyro_channels[] = { + ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, 0x22, IIO_MOD_X, 0), + ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, 0x24, IIO_MOD_Y, 1), + ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, 0x26, IIO_MOD_Z, 2), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static const struct iio_chan_spec st_lsm6ds0_gyro_channels[] = { + ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, 0x18, IIO_MOD_X, 0), + ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, 0x1a, IIO_MOD_Y, 1), + ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, 0x1c, IIO_MOD_Z, 2), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = { + { + .reset = { + .addr = 0x22, + .mask = BIT(0), + }, + .boot = { + .addr = 0x22, + .mask = BIT(7), + }, + .bdu = { + .addr = 0x22, + .mask = BIT(6), + }, + .id = { + { + .hw_id = ST_LSM9DS1_ID, + .name = ST_LSM9DS1_DEV_NAME, + .wai = 0x68, + }, { + .hw_id = ST_LSM6DS0_ID, + .name = ST_LSM6DS0_DEV_NAME, + .wai = 0x68, + }, + }, + .channels = { + [ST_LSM6DSX_ID_ACC] = { + .chan = st_lsm6dsx_acc_channels, + .len = ARRAY_SIZE(st_lsm6dsx_acc_channels), + }, + [ST_LSM6DSX_ID_GYRO] = { + .chan = st_lsm6ds0_gyro_channels, + .len = ARRAY_SIZE(st_lsm6ds0_gyro_channels), + }, + }, + .odr_table = { + [ST_LSM6DSX_ID_ACC] = { + .reg = { + .addr = 0x20, + .mask = GENMASK(7, 5), + }, + .odr_avl[0] = { 10000, 0x01 }, + .odr_avl[1] = { 50000, 0x02 }, + .odr_avl[2] = { 119000, 0x03 }, + .odr_avl[3] = { 238000, 0x04 }, + .odr_avl[4] = { 476000, 0x05 }, + .odr_avl[5] = { 952000, 0x06 }, + .odr_len = 6, + }, + [ST_LSM6DSX_ID_GYRO] = { + .reg = { + .addr = 0x10, + .mask = GENMASK(7, 5), + }, + .odr_avl[0] = { 14900, 0x01 }, + .odr_avl[1] = { 59500, 0x02 }, + .odr_avl[2] = { 119000, 0x03 }, + .odr_avl[3] = { 238000, 0x04 }, + .odr_avl[4] = { 476000, 0x05 }, + .odr_avl[5] = { 952000, 0x06 }, + .odr_len = 6, + }, + }, + .fs_table = { + [ST_LSM6DSX_ID_ACC] = { + .reg = { + .addr = 0x20, + .mask = GENMASK(4, 3), + }, + .fs_avl[0] = { IIO_G_TO_M_S_2(61000), 0x0 }, + .fs_avl[1] = { IIO_G_TO_M_S_2(122000), 0x2 }, + .fs_avl[2] = { IIO_G_TO_M_S_2(244000), 0x3 }, + .fs_avl[3] = { IIO_G_TO_M_S_2(732000), 0x1 }, + .fs_len = 4, + }, + [ST_LSM6DSX_ID_GYRO] = { + .reg = { + .addr = 0x10, + .mask = GENMASK(4, 3), + }, + + .fs_avl[0] = { IIO_DEGREE_TO_RAD(8750000), 0x0 }, + .fs_avl[1] = { IIO_DEGREE_TO_RAD(17500000), 0x1 }, + .fs_avl[2] = { IIO_DEGREE_TO_RAD(70000000), 0x3 }, + .fs_len = 3, + }, + }, + .irq_config = { + .irq1 = { + .addr = 0x0c, + .mask = BIT(3), + }, + .irq2 = { + .addr = 0x0d, + .mask = BIT(3), + }, + .hla = { + .addr = 0x22, + .mask = BIT(5), + }, + .od = { + .addr = 0x22, + .mask = BIT(4), + }, + }, + .fifo_ops = { + .max_size = 32, + }, + }, + { + .reset = { + .addr = 0x12, + .mask = BIT(0), + }, + .boot = { + .addr = 0x12, + .mask = BIT(7), + }, + .bdu = { + .addr = 0x12, + .mask = BIT(6), + }, + .id = { + { + .hw_id = ST_LSM6DS3_ID, + .name = ST_LSM6DS3_DEV_NAME, + .wai = 0x69, + }, + }, + .channels = { + [ST_LSM6DSX_ID_ACC] = { + .chan = st_lsm6dsx_acc_channels, + .len = ARRAY_SIZE(st_lsm6dsx_acc_channels), + }, + [ST_LSM6DSX_ID_GYRO] = { + .chan = st_lsm6dsx_gyro_channels, + .len = ARRAY_SIZE(st_lsm6dsx_gyro_channels), + }, + }, + .odr_table = { + [ST_LSM6DSX_ID_ACC] = { + .reg = { + .addr = 0x10, + .mask = GENMASK(7, 4), + }, + .odr_avl[0] = { 12500, 0x01 }, + .odr_avl[1] = { 26000, 0x02 }, + .odr_avl[2] = { 52000, 0x03 }, + .odr_avl[3] = { 104000, 0x04 }, + .odr_avl[4] = { 208000, 0x05 }, + .odr_avl[5] = { 416000, 0x06 }, + .odr_len = 6, + }, + [ST_LSM6DSX_ID_GYRO] = { + .reg = { + .addr = 0x11, + .mask = GENMASK(7, 4), + }, + .odr_avl[0] = { 12500, 0x01 }, + .odr_avl[1] = { 26000, 0x02 }, + .odr_avl[2] = { 52000, 0x03 }, + .odr_avl[3] = { 104000, 0x04 }, + .odr_avl[4] = { 208000, 0x05 }, + .odr_avl[5] = { 416000, 0x06 }, + .odr_len = 6, + }, + }, + .fs_table = { + [ST_LSM6DSX_ID_ACC] = { + .reg = { + .addr = 0x10, + .mask = GENMASK(3, 2), + }, + .fs_avl[0] = { IIO_G_TO_M_S_2(61000), 0x0 }, + .fs_avl[1] = { IIO_G_TO_M_S_2(122000), 0x2 }, + .fs_avl[2] = { IIO_G_TO_M_S_2(244000), 0x3 }, + .fs_avl[3] = { IIO_G_TO_M_S_2(488000), 0x1 }, + .fs_len = 4, + }, + [ST_LSM6DSX_ID_GYRO] = { + .reg = { + .addr = 0x11, + .mask = GENMASK(3, 2), + }, + .fs_avl[0] = { IIO_DEGREE_TO_RAD(8750000), 0x0 }, + .fs_avl[1] = { IIO_DEGREE_TO_RAD(17500000), 0x1 }, + .fs_avl[2] = { IIO_DEGREE_TO_RAD(35000000), 0x2 }, + .fs_avl[3] = { IIO_DEGREE_TO_RAD(70000000), 0x3 }, + .fs_len = 4, + }, + }, + .irq_config = { + .irq1 = { + .addr = 0x0d, + .mask = BIT(3), + }, + .irq2 = { + .addr = 0x0e, + .mask = BIT(3), + }, + .lir = { + .addr = 0x58, + .mask = BIT(0), + }, + .irq1_func = { + .addr = 0x5e, + .mask = BIT(5), + }, + .irq2_func = { + .addr = 0x5f, + .mask = BIT(5), + }, + .hla = { + .addr = 0x12, + .mask = BIT(5), + }, + .od = { + .addr = 0x12, + .mask = BIT(4), + }, + }, + .decimator = { + [ST_LSM6DSX_ID_ACC] = { + .addr = 0x08, + .mask = GENMASK(2, 0), + }, + [ST_LSM6DSX_ID_GYRO] = { + .addr = 0x08, + .mask = GENMASK(5, 3), + }, + }, + .fifo_ops = { + .update_fifo = st_lsm6dsx_update_fifo, + .read_fifo = st_lsm6dsx_read_fifo, + .fifo_th = { + .addr = 0x06, + .mask = GENMASK(11, 0), + }, + .fifo_diff = { + .addr = 0x3a, + .mask = GENMASK(11, 0), + }, + .max_size = 1365, + .th_wl = 3, /* 1LSB = 2B */ + }, + .ts_settings = { + .timer_en = { + .addr = 0x58, + .mask = BIT(7), + }, + .hr_timer = { + .addr = 0x5c, + .mask = BIT(4), + }, + .fifo_en = { + .addr = 0x07, + .mask = BIT(7), + }, + .decimator = { + .addr = 0x09, + .mask = GENMASK(5, 3), + }, + }, + .event_settings = { + .wakeup_reg = { + .addr = 0x5B, + .mask = GENMASK(5, 0), + }, + .wakeup_src_reg = 0x1b, + .wakeup_src_status_mask = BIT(3), + .wakeup_src_z_mask = BIT(0), + .wakeup_src_y_mask = BIT(1), + .wakeup_src_x_mask = BIT(2), + }, + }, + { + .reset = { + .addr = 0x12, + .mask = BIT(0), + }, + .boot = { + .addr = 0x12, + .mask = BIT(7), + }, + .bdu = { + .addr = 0x12, + .mask = BIT(6), + }, + .id = { + { + .hw_id = ST_LSM6DS3H_ID, + .name = ST_LSM6DS3H_DEV_NAME, + .wai = 0x69, + }, + }, + .channels = { + [ST_LSM6DSX_ID_ACC] = { + .chan = st_lsm6dsx_acc_channels, + .len = ARRAY_SIZE(st_lsm6dsx_acc_channels), + }, + [ST_LSM6DSX_ID_GYRO] = { + .chan = st_lsm6dsx_gyro_channels, + .len = ARRAY_SIZE(st_lsm6dsx_gyro_channels), + }, + }, + .odr_table = { + [ST_LSM6DSX_ID_ACC] = { + .reg = { + .addr = 0x10, + .mask = GENMASK(7, 4), + }, + .odr_avl[0] = { 12500, 0x01 }, + .odr_avl[1] = { 26000, 0x02 }, + .odr_avl[2] = { 52000, 0x03 }, + .odr_avl[3] = { 104000, 0x04 }, + .odr_avl[4] = { 208000, 0x05 }, + .odr_avl[5] = { 416000, 0x06 }, + .odr_len = 6, + }, + [ST_LSM6DSX_ID_GYRO] = { + .reg = { + .addr = 0x11, + .mask = GENMASK(7, 4), + }, + .odr_avl[0] = { 12500, 0x01 }, + .odr_avl[1] = { 26000, 0x02 }, + .odr_avl[2] = { 52000, 0x03 }, + .odr_avl[3] = { 104000, 0x04 }, + .odr_avl[4] = { 208000, 0x05 }, + .odr_avl[5] = { 416000, 0x06 }, + .odr_len = 6, + }, + }, + .fs_table = { + [ST_LSM6DSX_ID_ACC] = { + .reg = { + .addr = 0x10, + .mask = GENMASK(3, 2), + }, + .fs_avl[0] = { IIO_G_TO_M_S_2(61000), 0x0 }, + .fs_avl[1] = { IIO_G_TO_M_S_2(122000), 0x2 }, + .fs_avl[2] = { IIO_G_TO_M_S_2(244000), 0x3 }, + .fs_avl[3] = { IIO_G_TO_M_S_2(488000), 0x1 }, + .fs_len = 4, + }, + [ST_LSM6DSX_ID_GYRO] = { + .reg = { + .addr = 0x11, + .mask = GENMASK(3, 2), + }, + .fs_avl[0] = { IIO_DEGREE_TO_RAD(8750000), 0x0 }, + .fs_avl[1] = { IIO_DEGREE_TO_RAD(17500000), 0x1 }, + .fs_avl[2] = { IIO_DEGREE_TO_RAD(35000000), 0x2 }, + .fs_avl[3] = { IIO_DEGREE_TO_RAD(70000000), 0x3 }, + .fs_len = 4, + }, + }, + .irq_config = { + .irq1 = { + .addr = 0x0d, + .mask = BIT(3), + }, + .irq2 = { + .addr = 0x0e, + .mask = BIT(3), + }, + .lir = { + .addr = 0x58, + .mask = BIT(0), + }, + .irq1_func = { + .addr = 0x5e, + .mask = BIT(5), + }, + .irq2_func = { + .addr = 0x5f, + .mask = BIT(5), + }, + .hla = { + .addr = 0x12, + .mask = BIT(5), + }, + .od = { + .addr = 0x12, + .mask = BIT(4), + }, + }, + .decimator = { + [ST_LSM6DSX_ID_ACC] = { + .addr = 0x08, + .mask = GENMASK(2, 0), + }, + [ST_LSM6DSX_ID_GYRO] = { + .addr = 0x08, + .mask = GENMASK(5, 3), + }, + }, + .fifo_ops = { + .update_fifo = st_lsm6dsx_update_fifo, + .read_fifo = st_lsm6dsx_read_fifo, + .fifo_th = { + .addr = 0x06, + .mask = GENMASK(11, 0), + }, + .fifo_diff = { + .addr = 0x3a, + .mask = GENMASK(11, 0), + }, + .max_size = 682, + .th_wl = 3, /* 1LSB = 2B */ + }, + .ts_settings = { + .timer_en = { + .addr = 0x58, + .mask = BIT(7), + }, + .hr_timer = { + .addr = 0x5c, + .mask = BIT(4), + }, + .fifo_en = { + .addr = 0x07, + .mask = BIT(7), + }, + .decimator = { + .addr = 0x09, + .mask = GENMASK(5, 3), + }, + }, + .event_settings = { + .wakeup_reg = { + .addr = 0x5B, + .mask = GENMASK(5, 0), + }, + .wakeup_src_reg = 0x1b, + .wakeup_src_status_mask = BIT(3), + .wakeup_src_z_mask = BIT(0), + .wakeup_src_y_mask = BIT(1), + .wakeup_src_x_mask = BIT(2), + }, + }, + { + .reset = { + .addr = 0x12, + .mask = BIT(0), + }, + .boot = { + .addr = 0x12, + .mask = BIT(7), + }, + .bdu = { + .addr = 0x12, + .mask = BIT(6), + }, + .id = { + { + .hw_id = ST_LSM6DSL_ID, + .name = ST_LSM6DSL_DEV_NAME, + .wai = 0x6a, + }, { + .hw_id = ST_LSM6DSM_ID, + .name = ST_LSM6DSM_DEV_NAME, + .wai = 0x6a, + }, { + .hw_id = ST_ISM330DLC_ID, + .name = ST_ISM330DLC_DEV_NAME, + .wai = 0x6a, + }, { + .hw_id = ST_LSM6DS3TRC_ID, + .name = ST_LSM6DS3TRC_DEV_NAME, + .wai = 0x6a, + }, + }, + .channels = { + [ST_LSM6DSX_ID_ACC] = { + .chan = st_lsm6dsx_acc_channels, + .len = ARRAY_SIZE(st_lsm6dsx_acc_channels), + }, + [ST_LSM6DSX_ID_GYRO] = { + .chan = st_lsm6dsx_gyro_channels, + .len = ARRAY_SIZE(st_lsm6dsx_gyro_channels), + }, + }, + .odr_table = { + [ST_LSM6DSX_ID_ACC] = { + .reg = { + .addr = 0x10, + .mask = GENMASK(7, 4), + }, + .odr_avl[0] = { 12500, 0x01 }, + .odr_avl[1] = { 26000, 0x02 }, + .odr_avl[2] = { 52000, 0x03 }, + .odr_avl[3] = { 104000, 0x04 }, + .odr_avl[4] = { 208000, 0x05 }, + .odr_avl[5] = { 416000, 0x06 }, + .odr_len = 6, + }, + [ST_LSM6DSX_ID_GYRO] = { + .reg = { + .addr = 0x11, + .mask = GENMASK(7, 4), + }, + .odr_avl[0] = { 12500, 0x01 }, + .odr_avl[1] = { 26000, 0x02 }, + .odr_avl[2] = { 52000, 0x03 }, + .odr_avl[3] = { 104000, 0x04 }, + .odr_avl[4] = { 208000, 0x05 }, + .odr_avl[5] = { 416000, 0x06 }, + .odr_len = 6, + }, + }, + .fs_table = { + [ST_LSM6DSX_ID_ACC] = { + .reg = { + .addr = 0x10, + .mask = GENMASK(3, 2), + }, + .fs_avl[0] = { IIO_G_TO_M_S_2(61000), 0x0 }, + .fs_avl[1] = { IIO_G_TO_M_S_2(122000), 0x2 }, + .fs_avl[2] = { IIO_G_TO_M_S_2(244000), 0x3 }, + .fs_avl[3] = { IIO_G_TO_M_S_2(488000), 0x1 }, + .fs_len = 4, + }, + [ST_LSM6DSX_ID_GYRO] = { + .reg = { + .addr = 0x11, + .mask = GENMASK(3, 2), + }, + .fs_avl[0] = { IIO_DEGREE_TO_RAD(8750000), 0x0 }, + .fs_avl[1] = { IIO_DEGREE_TO_RAD(17500000), 0x1 }, + .fs_avl[2] = { IIO_DEGREE_TO_RAD(35000000), 0x2 }, + .fs_avl[3] = { IIO_DEGREE_TO_RAD(70000000), 0x3 }, + .fs_len = 4, + }, + }, + .irq_config = { + .irq1 = { + .addr = 0x0d, + .mask = BIT(3), + }, + .irq2 = { + .addr = 0x0e, + .mask = BIT(3), + }, + .lir = { + .addr = 0x58, + .mask = BIT(0), + }, + .irq1_func = { + .addr = 0x5e, + .mask = BIT(5), + }, + .irq2_func = { + .addr = 0x5f, + .mask = BIT(5), + }, + .hla = { + .addr = 0x12, + .mask = BIT(5), + }, + .od = { + .addr = 0x12, + .mask = BIT(4), + }, + }, + .decimator = { + [ST_LSM6DSX_ID_ACC] = { + .addr = 0x08, + .mask = GENMASK(2, 0), + }, + [ST_LSM6DSX_ID_GYRO] = { + .addr = 0x08, + .mask = GENMASK(5, 3), + }, + [ST_LSM6DSX_ID_EXT0] = { + .addr = 0x09, + .mask = GENMASK(2, 0), + }, + }, + .fifo_ops = { + .update_fifo = st_lsm6dsx_update_fifo, + .read_fifo = st_lsm6dsx_read_fifo, + .fifo_th = { + .addr = 0x06, + .mask = GENMASK(10, 0), + }, + .fifo_diff = { + .addr = 0x3a, + .mask = GENMASK(10, 0), + }, + .max_size = 682, + .th_wl = 3, /* 1LSB = 2B */ + }, + .ts_settings = { + .timer_en = { + .addr = 0x19, + .mask = BIT(5), + }, + .hr_timer = { + .addr = 0x5c, + .mask = BIT(4), + }, + .fifo_en = { + .addr = 0x07, + .mask = BIT(7), + }, + .decimator = { + .addr = 0x09, + .mask = GENMASK(5, 3), + }, + }, + .shub_settings = { + .page_mux = { + .addr = 0x01, + .mask = BIT(7), + }, + .master_en = { + .addr = 0x1a, + .mask = BIT(0), + }, + .pullup_en = { + .addr = 0x1a, + .mask = BIT(3), + }, + .aux_sens = { + .addr = 0x04, + .mask = GENMASK(5, 4), + }, + .wr_once = { + .addr = 0x07, + .mask = BIT(5), + }, + .emb_func = { + .addr = 0x19, + .mask = BIT(2), + }, + .num_ext_dev = 1, + .shub_out = { + .addr = 0x2e, + }, + .slv0_addr = 0x02, + .dw_slv0_addr = 0x0e, + .pause = 0x7, + }, + .event_settings = { + .enable_reg = { + .addr = 0x58, + .mask = BIT(7), + }, + .wakeup_reg = { + .addr = 0x5B, + .mask = GENMASK(5, 0), + }, + .wakeup_src_reg = 0x1b, + .wakeup_src_status_mask = BIT(3), + .wakeup_src_z_mask = BIT(0), + .wakeup_src_y_mask = BIT(1), + .wakeup_src_x_mask = BIT(2), + }, + }, + { + .reset = { + .addr = 0x12, + .mask = BIT(0), + }, + .boot = { + .addr = 0x12, + .mask = BIT(7), + }, + .bdu = { + .addr = 0x12, + .mask = BIT(6), + }, + .id = { + { + .hw_id = ST_LSM6DSR_ID, + .name = ST_LSM6DSR_DEV_NAME, + .wai = 0x6b, + }, { + .hw_id = ST_ISM330DHCX_ID, + .name = ST_ISM330DHCX_DEV_NAME, + .wai = 0x6b, + }, { + .hw_id = ST_LSM6DSRX_ID, + .name = ST_LSM6DSRX_DEV_NAME, + .wai = 0x6b, + }, { + .hw_id = ST_LSM6DSO_ID, + .name = ST_LSM6DSO_DEV_NAME, + .wai = 0x6c, + }, { + .hw_id = ST_LSM6DSOX_ID, + .name = ST_LSM6DSOX_DEV_NAME, + .wai = 0x6c, + }, { + .hw_id = ST_LSM6DST_ID, + .name = ST_LSM6DST_DEV_NAME, + .wai = 0x6d, + }, { + .hw_id = ST_ASM330LHHX_ID, + .name = ST_ASM330LHHX_DEV_NAME, + .wai = 0x6b, + }, { + .hw_id = ST_LSM6DSTX_ID, + .name = ST_LSM6DSTX_DEV_NAME, + .wai = 0x6d, + }, + }, + .channels = { + [ST_LSM6DSX_ID_ACC] = { + .chan = st_lsm6dsx_acc_channels, + .len = ARRAY_SIZE(st_lsm6dsx_acc_channels), + }, + [ST_LSM6DSX_ID_GYRO] = { + .chan = st_lsm6dsx_gyro_channels, + .len = ARRAY_SIZE(st_lsm6dsx_gyro_channels), + }, + }, + .drdy_mask = { + .addr = 0x13, + .mask = BIT(3), + }, + .odr_table = { + [ST_LSM6DSX_ID_ACC] = { + .reg = { + .addr = 0x10, + .mask = GENMASK(7, 4), + }, + .odr_avl[0] = { 12500, 0x01 }, + .odr_avl[1] = { 26000, 0x02 }, + .odr_avl[2] = { 52000, 0x03 }, + .odr_avl[3] = { 104000, 0x04 }, + .odr_avl[4] = { 208000, 0x05 }, + .odr_avl[5] = { 416000, 0x06 }, + .odr_avl[6] = { 833000, 0x07 }, + .odr_len = 7, + }, + [ST_LSM6DSX_ID_GYRO] = { + .reg = { + .addr = 0x11, + .mask = GENMASK(7, 4), + }, + .odr_avl[0] = { 12500, 0x01 }, + .odr_avl[1] = { 26000, 0x02 }, + .odr_avl[2] = { 52000, 0x03 }, + .odr_avl[3] = { 104000, 0x04 }, + .odr_avl[4] = { 208000, 0x05 }, + .odr_avl[5] = { 416000, 0x06 }, + .odr_avl[6] = { 833000, 0x07 }, + .odr_len = 7, + }, + }, + .fs_table = { + [ST_LSM6DSX_ID_ACC] = { + .reg = { + .addr = 0x10, + .mask = GENMASK(3, 2), + }, + .fs_avl[0] = { IIO_G_TO_M_S_2(61000), 0x0 }, + .fs_avl[1] = { IIO_G_TO_M_S_2(122000), 0x2 }, + .fs_avl[2] = { IIO_G_TO_M_S_2(244000), 0x3 }, + .fs_avl[3] = { IIO_G_TO_M_S_2(488000), 0x1 }, + .fs_len = 4, + }, + [ST_LSM6DSX_ID_GYRO] = { + .reg = { + .addr = 0x11, + .mask = GENMASK(3, 2), + }, + .fs_avl[0] = { IIO_DEGREE_TO_RAD(8750000), 0x0 }, + .fs_avl[1] = { IIO_DEGREE_TO_RAD(17500000), 0x1 }, + .fs_avl[2] = { IIO_DEGREE_TO_RAD(35000000), 0x2 }, + .fs_avl[3] = { IIO_DEGREE_TO_RAD(70000000), 0x3 }, + .fs_len = 4, + }, + }, + .irq_config = { + .irq1 = { + .addr = 0x0d, + .mask = BIT(3), + }, + .irq2 = { + .addr = 0x0e, + .mask = BIT(3), + }, + .lir = { + .addr = 0x56, + .mask = BIT(0), + }, + .clear_on_read = { + .addr = 0x56, + .mask = BIT(6), + }, + .irq1_func = { + .addr = 0x5e, + .mask = BIT(5), + }, + .irq2_func = { + .addr = 0x5f, + .mask = BIT(5), + }, + .hla = { + .addr = 0x12, + .mask = BIT(5), + }, + .od = { + .addr = 0x12, + .mask = BIT(4), + }, + }, + .batch = { + [ST_LSM6DSX_ID_ACC] = { + .addr = 0x09, + .mask = GENMASK(3, 0), + }, + [ST_LSM6DSX_ID_GYRO] = { + .addr = 0x09, + .mask = GENMASK(7, 4), + }, + }, + .fifo_ops = { + .update_fifo = st_lsm6dsx_update_fifo, + .read_fifo = st_lsm6dsx_read_tagged_fifo, + .fifo_th = { + .addr = 0x07, + .mask = GENMASK(8, 0), + }, + .fifo_diff = { + .addr = 0x3a, + .mask = GENMASK(9, 0), + }, + .max_size = 512, + .th_wl = 1, + }, + .ts_settings = { + .timer_en = { + .addr = 0x19, + .mask = BIT(5), + }, + .decimator = { + .addr = 0x0a, + .mask = GENMASK(7, 6), + }, + .freq_fine = 0x63, + }, + .shub_settings = { + .page_mux = { + .addr = 0x01, + .mask = BIT(6), + }, + .master_en = { + .sec_page = true, + .addr = 0x14, + .mask = BIT(2), + }, + .pullup_en = { + .sec_page = true, + .addr = 0x14, + .mask = BIT(3), + }, + .aux_sens = { + .addr = 0x14, + .mask = GENMASK(1, 0), + }, + .wr_once = { + .addr = 0x14, + .mask = BIT(6), + }, + .num_ext_dev = 3, + .shub_out = { + .sec_page = true, + .addr = 0x02, + }, + .slv0_addr = 0x15, + .dw_slv0_addr = 0x21, + .batch_en = BIT(3), + }, + .event_settings = { + .enable_reg = { + .addr = 0x58, + .mask = BIT(7), + }, + .wakeup_reg = { + .addr = 0x5b, + .mask = GENMASK(5, 0), + }, + .wakeup_src_reg = 0x1b, + .wakeup_src_status_mask = BIT(3), + .wakeup_src_z_mask = BIT(0), + .wakeup_src_y_mask = BIT(1), + .wakeup_src_x_mask = BIT(2), + }, + }, + { + .reset = { + .addr = 0x12, + .mask = BIT(0), + }, + .boot = { + .addr = 0x12, + .mask = BIT(7), + }, + .bdu = { + .addr = 0x12, + .mask = BIT(6), + }, + .id = { + { + .hw_id = ST_ASM330LHH_ID, + .name = ST_ASM330LHH_DEV_NAME, + .wai = 0x6b, + }, { + .hw_id = ST_LSM6DSOP_ID, + .name = ST_LSM6DSOP_DEV_NAME, + .wai = 0x6c, + }, + }, + .channels = { + [ST_LSM6DSX_ID_ACC] = { + .chan = st_lsm6dsx_acc_channels, + .len = ARRAY_SIZE(st_lsm6dsx_acc_channels), + }, + [ST_LSM6DSX_ID_GYRO] = { + .chan = st_lsm6dsx_gyro_channels, + .len = ARRAY_SIZE(st_lsm6dsx_gyro_channels), + }, + }, + .drdy_mask = { + .addr = 0x13, + .mask = BIT(3), + }, + .odr_table = { + [ST_LSM6DSX_ID_ACC] = { + .reg = { + .addr = 0x10, + .mask = GENMASK(7, 4), + }, + .odr_avl[0] = { 12500, 0x01 }, + .odr_avl[1] = { 26000, 0x02 }, + .odr_avl[2] = { 52000, 0x03 }, + .odr_avl[3] = { 104000, 0x04 }, + .odr_avl[4] = { 208000, 0x05 }, + .odr_avl[5] = { 416000, 0x06 }, + .odr_avl[6] = { 833000, 0x07 }, + .odr_len = 7, + }, + [ST_LSM6DSX_ID_GYRO] = { + .reg = { + .addr = 0x11, + .mask = GENMASK(7, 4), + }, + .odr_avl[0] = { 12500, 0x01 }, + .odr_avl[1] = { 26000, 0x02 }, + .odr_avl[2] = { 52000, 0x03 }, + .odr_avl[3] = { 104000, 0x04 }, + .odr_avl[4] = { 208000, 0x05 }, + .odr_avl[5] = { 416000, 0x06 }, + .odr_avl[6] = { 833000, 0x07 }, + .odr_len = 7, + }, + }, + .fs_table = { + [ST_LSM6DSX_ID_ACC] = { + .reg = { + .addr = 0x10, + .mask = GENMASK(3, 2), + }, + .fs_avl[0] = { IIO_G_TO_M_S_2(61000), 0x0 }, + .fs_avl[1] = { IIO_G_TO_M_S_2(122000), 0x2 }, + .fs_avl[2] = { IIO_G_TO_M_S_2(244000), 0x3 }, + .fs_avl[3] = { IIO_G_TO_M_S_2(488000), 0x1 }, + .fs_len = 4, + }, + [ST_LSM6DSX_ID_GYRO] = { + .reg = { + .addr = 0x11, + .mask = GENMASK(3, 2), + }, + .fs_avl[0] = { IIO_DEGREE_TO_RAD(8750000), 0x0 }, + .fs_avl[1] = { IIO_DEGREE_TO_RAD(17500000), 0x1 }, + .fs_avl[2] = { IIO_DEGREE_TO_RAD(35000000), 0x2 }, + .fs_avl[3] = { IIO_DEGREE_TO_RAD(70000000), 0x3 }, + .fs_len = 4, + }, + }, + .irq_config = { + .irq1 = { + .addr = 0x0d, + .mask = BIT(3), + }, + .irq2 = { + .addr = 0x0e, + .mask = BIT(3), + }, + .lir = { + .addr = 0x56, + .mask = BIT(0), + }, + .clear_on_read = { + .addr = 0x56, + .mask = BIT(6), + }, + .irq1_func = { + .addr = 0x5e, + .mask = BIT(5), + }, + .irq2_func = { + .addr = 0x5f, + .mask = BIT(5), + }, + .hla = { + .addr = 0x12, + .mask = BIT(5), + }, + .od = { + .addr = 0x12, + .mask = BIT(4), + }, + }, + .batch = { + [ST_LSM6DSX_ID_ACC] = { + .addr = 0x09, + .mask = GENMASK(3, 0), + }, + [ST_LSM6DSX_ID_GYRO] = { + .addr = 0x09, + .mask = GENMASK(7, 4), + }, + }, + .fifo_ops = { + .update_fifo = st_lsm6dsx_update_fifo, + .read_fifo = st_lsm6dsx_read_tagged_fifo, + .fifo_th = { + .addr = 0x07, + .mask = GENMASK(8, 0), + }, + .fifo_diff = { + .addr = 0x3a, + .mask = GENMASK(9, 0), + }, + .max_size = 512, + .th_wl = 1, + }, + .ts_settings = { + .timer_en = { + .addr = 0x19, + .mask = BIT(5), + }, + .decimator = { + .addr = 0x0a, + .mask = GENMASK(7, 6), + }, + .freq_fine = 0x63, + }, + .event_settings = { + .enable_reg = { + .addr = 0x58, + .mask = BIT(7), + }, + .wakeup_reg = { + .addr = 0x5B, + .mask = GENMASK(5, 0), + }, + .wakeup_src_reg = 0x1b, + .wakeup_src_status_mask = BIT(3), + .wakeup_src_z_mask = BIT(0), + .wakeup_src_y_mask = BIT(1), + .wakeup_src_x_mask = BIT(2), + }, + }, +}; + +int st_lsm6dsx_set_page(struct st_lsm6dsx_hw *hw, bool enable) +{ + const struct st_lsm6dsx_shub_settings *hub_settings; + unsigned int data; + int err; + + hub_settings = &hw->settings->shub_settings; + data = ST_LSM6DSX_SHIFT_VAL(enable, hub_settings->page_mux.mask); + err = regmap_update_bits(hw->regmap, hub_settings->page_mux.addr, + hub_settings->page_mux.mask, data); + usleep_range(100, 150); + + return err; +} + +static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw, int id, + const char **name) +{ + int err, i, j, data; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) { + for (j = 0; j < ST_LSM6DSX_MAX_ID; j++) { + if (st_lsm6dsx_sensor_settings[i].id[j].name && + id == st_lsm6dsx_sensor_settings[i].id[j].hw_id) + break; + } + if (j < ST_LSM6DSX_MAX_ID) + break; + } + + if (i == ARRAY_SIZE(st_lsm6dsx_sensor_settings)) { + dev_err(hw->dev, "unsupported hw id [%02x]\n", id); + return -ENODEV; + } + + err = regmap_read(hw->regmap, ST_LSM6DSX_REG_WHOAMI_ADDR, &data); + if (err < 0) { + dev_err(hw->dev, "failed to read whoami register\n"); + return err; + } + + if (data != st_lsm6dsx_sensor_settings[i].id[j].wai) { + dev_err(hw->dev, "unsupported whoami [%02x]\n", data); + return -ENODEV; + } + + *name = st_lsm6dsx_sensor_settings[i].id[j].name; + hw->settings = &st_lsm6dsx_sensor_settings[i]; + + return 0; +} + +static int st_lsm6dsx_set_full_scale(struct st_lsm6dsx_sensor *sensor, + u32 gain) +{ + const struct st_lsm6dsx_fs_table_entry *fs_table; + unsigned int data; + int i, err; + + fs_table = &sensor->hw->settings->fs_table[sensor->id]; + for (i = 0; i < fs_table->fs_len; i++) { + if (fs_table->fs_avl[i].gain == gain) + break; + } + + if (i == fs_table->fs_len) + return -EINVAL; + + data = ST_LSM6DSX_SHIFT_VAL(fs_table->fs_avl[i].val, + fs_table->reg.mask); + err = st_lsm6dsx_update_bits_locked(sensor->hw, fs_table->reg.addr, + fs_table->reg.mask, data); + if (err < 0) + return err; + + sensor->gain = gain; + + return 0; +} + +int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, u32 odr, u8 *val) +{ + const struct st_lsm6dsx_odr_table_entry *odr_table; + int i; + + odr_table = &sensor->hw->settings->odr_table[sensor->id]; + for (i = 0; i < odr_table->odr_len; i++) { + /* + * ext devices can run at different odr respect to + * accel sensor + */ + if (odr_table->odr_avl[i].milli_hz >= odr) + break; + } + + if (i == odr_table->odr_len) + return -EINVAL; + + *val = odr_table->odr_avl[i].val; + return odr_table->odr_avl[i].milli_hz; +} + +static int +st_lsm6dsx_check_odr_dependency(struct st_lsm6dsx_hw *hw, u32 odr, + enum st_lsm6dsx_sensor_id id) +{ + struct st_lsm6dsx_sensor *ref = iio_priv(hw->iio_devs[id]); + + if (odr > 0) { + if (hw->enable_mask & BIT(id)) + return max_t(u32, ref->odr, odr); + else + return odr; + } else { + return (hw->enable_mask & BIT(id)) ? ref->odr : 0; + } +} + +static int +st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u32 req_odr) +{ + struct st_lsm6dsx_sensor *ref_sensor = sensor; + struct st_lsm6dsx_hw *hw = sensor->hw; + const struct st_lsm6dsx_reg *reg; + unsigned int data; + u8 val = 0; + int err; + + switch (sensor->id) { + case ST_LSM6DSX_ID_GYRO: + break; + case ST_LSM6DSX_ID_EXT0: + case ST_LSM6DSX_ID_EXT1: + case ST_LSM6DSX_ID_EXT2: + case ST_LSM6DSX_ID_ACC: { + u32 odr; + int i; + + /* + * i2c embedded controller relies on the accelerometer sensor as + * bus read/write trigger so we need to enable accel device + * at odr = max(accel_odr, ext_odr) in order to properly + * communicate with i2c slave devices + */ + ref_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]); + for (i = ST_LSM6DSX_ID_ACC; i < ST_LSM6DSX_ID_MAX; i++) { + if (!hw->iio_devs[i] || i == sensor->id) + continue; + + odr = st_lsm6dsx_check_odr_dependency(hw, req_odr, i); + if (odr != req_odr) + /* device already configured */ + return 0; + } + break; + } + default: /* should never occur */ + return -EINVAL; + } + + if (req_odr > 0) { + err = st_lsm6dsx_check_odr(ref_sensor, req_odr, &val); + if (err < 0) + return err; + } + + reg = &hw->settings->odr_table[ref_sensor->id].reg; + data = ST_LSM6DSX_SHIFT_VAL(val, reg->mask); + return st_lsm6dsx_update_bits_locked(hw, reg->addr, reg->mask, data); +} + +static int +__st_lsm6dsx_sensor_set_enable(struct st_lsm6dsx_sensor *sensor, + bool enable) +{ + struct st_lsm6dsx_hw *hw = sensor->hw; + u32 odr = enable ? sensor->odr : 0; + int err; + + err = st_lsm6dsx_set_odr(sensor, odr); + if (err < 0) + return err; + + if (enable) + hw->enable_mask |= BIT(sensor->id); + else + hw->enable_mask &= ~BIT(sensor->id); + + return 0; +} + +static int +st_lsm6dsx_check_events(struct st_lsm6dsx_sensor *sensor, bool enable) +{ + struct st_lsm6dsx_hw *hw = sensor->hw; + + if (sensor->id == ST_LSM6DSX_ID_GYRO || enable) + return 0; + + return hw->enable_event; +} + +int st_lsm6dsx_sensor_set_enable(struct st_lsm6dsx_sensor *sensor, + bool enable) +{ + if (st_lsm6dsx_check_events(sensor, enable)) + return 0; + + return __st_lsm6dsx_sensor_set_enable(sensor, enable); +} + +static int st_lsm6dsx_read_oneshot(struct st_lsm6dsx_sensor *sensor, + u8 addr, int *val) +{ + struct st_lsm6dsx_hw *hw = sensor->hw; + int err, delay; + __le16 data; + + err = st_lsm6dsx_sensor_set_enable(sensor, true); + if (err < 0) + return err; + + /* + * we need to wait for sensor settling time before + * reading data in order to avoid corrupted samples + */ + delay = 1000000000 / sensor->odr; + usleep_range(3 * delay, 4 * delay); + + err = st_lsm6dsx_read_locked(hw, addr, &data, sizeof(data)); + if (err < 0) + return err; + + if (!hw->enable_event) { + err = st_lsm6dsx_sensor_set_enable(sensor, false); + if (err < 0) + return err; + } + + *val = (s16)le16_to_cpu(data); + + return IIO_VAL_INT; +} + +static int st_lsm6dsx_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + ret = iio_device_claim_direct_mode(iio_dev); + if (ret) + break; + + ret = st_lsm6dsx_read_oneshot(sensor, ch->address, val); + iio_device_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = sensor->odr / 1000; + *val2 = (sensor->odr % 1000) * 1000; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sensor->gain; + ret = IIO_VAL_INT_PLUS_NANO; + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +static int st_lsm6dsx_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + int err; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + err = st_lsm6dsx_set_full_scale(sensor, val2); + break; + case IIO_CHAN_INFO_SAMP_FREQ: { + u8 data; + + val = val * 1000 + val2 / 1000; + val = st_lsm6dsx_check_odr(sensor, val, &data); + if (val < 0) + err = val; + else + sensor->odr = val; + break; + } + default: + err = -EINVAL; + break; + } + + iio_device_release_direct_mode(iio_dev); + + return err; +} + +static int st_lsm6dsx_event_setup(struct st_lsm6dsx_hw *hw, int state) +{ + const struct st_lsm6dsx_reg *reg; + unsigned int data; + int err; + + if (!hw->settings->irq_config.irq1_func.addr) + return -ENOTSUPP; + + reg = &hw->settings->event_settings.enable_reg; + if (reg->addr) { + data = ST_LSM6DSX_SHIFT_VAL(state, reg->mask); + err = st_lsm6dsx_update_bits_locked(hw, reg->addr, + reg->mask, data); + if (err < 0) + return err; + } + + /* Enable wakeup interrupt */ + data = ST_LSM6DSX_SHIFT_VAL(state, hw->irq_routing->mask); + return st_lsm6dsx_update_bits_locked(hw, hw->irq_routing->addr, + hw->irq_routing->mask, data); +} + +static int st_lsm6dsx_read_event(struct iio_dev *iio_dev, + const struct iio_chan_spec *chan, + enum iio_event_type type, + enum iio_event_direction dir, + enum iio_event_info info, + int *val, int *val2) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsx_hw *hw = sensor->hw; + + if (type != IIO_EV_TYPE_THRESH) + return -EINVAL; + + *val2 = 0; + *val = hw->event_threshold; + + return IIO_VAL_INT; +} + +static int +st_lsm6dsx_write_event(struct iio_dev *iio_dev, + const struct iio_chan_spec *chan, + enum iio_event_type type, + enum iio_event_direction dir, + enum iio_event_info info, + int val, int val2) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsx_hw *hw = sensor->hw; + const struct st_lsm6dsx_reg *reg; + unsigned int data; + int err; + + if (type != IIO_EV_TYPE_THRESH) + return -EINVAL; + + if (val < 0 || val > 31) + return -EINVAL; + + reg = &hw->settings->event_settings.wakeup_reg; + data = ST_LSM6DSX_SHIFT_VAL(val, reg->mask); + err = st_lsm6dsx_update_bits_locked(hw, reg->addr, + reg->mask, data); + if (err < 0) + return -EINVAL; + + hw->event_threshold = val; + + return 0; +} + +static int +st_lsm6dsx_read_event_config(struct iio_dev *iio_dev, + const struct iio_chan_spec *chan, + enum iio_event_type type, + enum iio_event_direction dir) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsx_hw *hw = sensor->hw; + + if (type != IIO_EV_TYPE_THRESH) + return -EINVAL; + + return !!(hw->enable_event & BIT(chan->channel2)); +} + +static int +st_lsm6dsx_write_event_config(struct iio_dev *iio_dev, + const struct iio_chan_spec *chan, + enum iio_event_type type, + enum iio_event_direction dir, int state) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsx_hw *hw = sensor->hw; + u8 enable_event; + int err; + + if (type != IIO_EV_TYPE_THRESH) + return -EINVAL; + + if (state) { + enable_event = hw->enable_event | BIT(chan->channel2); + + /* do not enable events if they are already enabled */ + if (hw->enable_event) + goto out; + } else { + enable_event = hw->enable_event & ~BIT(chan->channel2); + + /* only turn off sensor if no events is enabled */ + if (enable_event) + goto out; + } + + /* stop here if no changes have been made */ + if (hw->enable_event == enable_event) + return 0; + + err = st_lsm6dsx_event_setup(hw, state); + if (err < 0) + return err; + + mutex_lock(&hw->conf_lock); + if (enable_event || !(hw->fifo_mask & BIT(sensor->id))) + err = __st_lsm6dsx_sensor_set_enable(sensor, state); + mutex_unlock(&hw->conf_lock); + if (err < 0) + return err; + +out: + hw->enable_event = enable_event; + + return 0; +} + +int st_lsm6dsx_set_watermark(struct iio_dev *iio_dev, unsigned int val) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsx_hw *hw = sensor->hw; + int err; + + val = clamp_val(val, 1, hw->settings->fifo_ops.max_size); + + mutex_lock(&hw->conf_lock); + + err = st_lsm6dsx_update_watermark(sensor, val); + + mutex_unlock(&hw->conf_lock); + + if (err < 0) + return err; + + sensor->watermark = val; + + return 0; +} + +static ssize_t +st_lsm6dsx_sysfs_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(dev_to_iio_dev(dev)); + const struct st_lsm6dsx_odr_table_entry *odr_table; + int i, len = 0; + + odr_table = &sensor->hw->settings->odr_table[sensor->id]; + for (i = 0; i < odr_table->odr_len; i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%03d ", + odr_table->odr_avl[i].milli_hz / 1000, + odr_table->odr_avl[i].milli_hz % 1000); + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_lsm6dsx_sysfs_scale_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(dev_to_iio_dev(dev)); + const struct st_lsm6dsx_fs_table_entry *fs_table; + struct st_lsm6dsx_hw *hw = sensor->hw; + int i, len = 0; + + fs_table = &hw->settings->fs_table[sensor->id]; + for (i = 0; i < fs_table->fs_len; i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%09u ", + fs_table->fs_avl[i].gain); + buf[len - 1] = '\n'; + + return len; +} + +static int st_lsm6dsx_write_raw_get_fmt(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + long mask) +{ + switch (mask) { + case IIO_CHAN_INFO_SCALE: + switch (chan->type) { + case IIO_ANGL_VEL: + case IIO_ACCEL: + return IIO_VAL_INT_PLUS_NANO; + default: + return IIO_VAL_INT_PLUS_MICRO; + } + default: + return IIO_VAL_INT_PLUS_MICRO; + } +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_sysfs_sampling_frequency_avail); +static IIO_DEVICE_ATTR(in_accel_scale_available, 0444, + st_lsm6dsx_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(in_anglvel_scale_available, 0444, + st_lsm6dsx_sysfs_scale_avail, NULL, 0); + +static struct attribute *st_lsm6dsx_acc_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_accel_scale_available.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsx_acc_attribute_group = { + .attrs = st_lsm6dsx_acc_attributes, +}; + +static const struct iio_info st_lsm6dsx_acc_info = { + .attrs = &st_lsm6dsx_acc_attribute_group, + .read_raw = st_lsm6dsx_read_raw, + .write_raw = st_lsm6dsx_write_raw, + .read_event_value = st_lsm6dsx_read_event, + .write_event_value = st_lsm6dsx_write_event, + .read_event_config = st_lsm6dsx_read_event_config, + .write_event_config = st_lsm6dsx_write_event_config, + .hwfifo_set_watermark = st_lsm6dsx_set_watermark, + .write_raw_get_fmt = st_lsm6dsx_write_raw_get_fmt, +}; + +static struct attribute *st_lsm6dsx_gyro_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_anglvel_scale_available.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsx_gyro_attribute_group = { + .attrs = st_lsm6dsx_gyro_attributes, +}; + +static const struct iio_info st_lsm6dsx_gyro_info = { + .attrs = &st_lsm6dsx_gyro_attribute_group, + .read_raw = st_lsm6dsx_read_raw, + .write_raw = st_lsm6dsx_write_raw, + .hwfifo_set_watermark = st_lsm6dsx_set_watermark, + .write_raw_get_fmt = st_lsm6dsx_write_raw_get_fmt, +}; + +static int st_lsm6dsx_get_drdy_pin(struct st_lsm6dsx_hw *hw, int *drdy_pin) +{ + struct device *dev = hw->dev; + + if (!dev_fwnode(dev)) + return -EINVAL; + + return device_property_read_u32(dev, "st,drdy-int-pin", drdy_pin); +} + +static int +st_lsm6dsx_get_drdy_reg(struct st_lsm6dsx_hw *hw, + const struct st_lsm6dsx_reg **drdy_reg) +{ + int err = 0, drdy_pin; + + if (st_lsm6dsx_get_drdy_pin(hw, &drdy_pin) < 0) { + struct st_sensors_platform_data *pdata; + struct device *dev = hw->dev; + + pdata = (struct st_sensors_platform_data *)dev->platform_data; + drdy_pin = pdata ? pdata->drdy_int_pin : 1; + } + + switch (drdy_pin) { + case 1: + hw->irq_routing = &hw->settings->irq_config.irq1_func; + *drdy_reg = &hw->settings->irq_config.irq1; + break; + case 2: + hw->irq_routing = &hw->settings->irq_config.irq2_func; + *drdy_reg = &hw->settings->irq_config.irq2; + break; + default: + dev_err(hw->dev, "unsupported data ready pin\n"); + err = -EINVAL; + break; + } + + return err; +} + +static int st_lsm6dsx_init_shub(struct st_lsm6dsx_hw *hw) +{ + const struct st_lsm6dsx_shub_settings *hub_settings; + struct st_sensors_platform_data *pdata; + struct device *dev = hw->dev; + unsigned int data; + int err = 0; + + hub_settings = &hw->settings->shub_settings; + + pdata = (struct st_sensors_platform_data *)dev->platform_data; + if ((dev_fwnode(dev) && device_property_read_bool(dev, "st,pullups")) || + (pdata && pdata->pullups)) { + if (hub_settings->pullup_en.sec_page) { + err = st_lsm6dsx_set_page(hw, true); + if (err < 0) + return err; + } + + data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->pullup_en.mask); + err = regmap_update_bits(hw->regmap, + hub_settings->pullup_en.addr, + hub_settings->pullup_en.mask, data); + + if (hub_settings->pullup_en.sec_page) + st_lsm6dsx_set_page(hw, false); + + if (err < 0) + return err; + } + + if (hub_settings->aux_sens.addr) { + /* configure aux sensors */ + err = st_lsm6dsx_set_page(hw, true); + if (err < 0) + return err; + + data = ST_LSM6DSX_SHIFT_VAL(3, hub_settings->aux_sens.mask); + err = regmap_update_bits(hw->regmap, + hub_settings->aux_sens.addr, + hub_settings->aux_sens.mask, data); + + st_lsm6dsx_set_page(hw, false); + + if (err < 0) + return err; + } + + if (hub_settings->emb_func.addr) { + data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->emb_func.mask); + err = regmap_update_bits(hw->regmap, + hub_settings->emb_func.addr, + hub_settings->emb_func.mask, data); + } + + return err; +} + +static int st_lsm6dsx_init_hw_timer(struct st_lsm6dsx_hw *hw) +{ + const struct st_lsm6dsx_hw_ts_settings *ts_settings; + int err, val; + + ts_settings = &hw->settings->ts_settings; + /* enable hw timestamp generation if necessary */ + if (ts_settings->timer_en.addr) { + val = ST_LSM6DSX_SHIFT_VAL(1, ts_settings->timer_en.mask); + err = regmap_update_bits(hw->regmap, + ts_settings->timer_en.addr, + ts_settings->timer_en.mask, val); + if (err < 0) + return err; + } + + /* enable high resolution for hw ts timer if necessary */ + if (ts_settings->hr_timer.addr) { + val = ST_LSM6DSX_SHIFT_VAL(1, ts_settings->hr_timer.mask); + err = regmap_update_bits(hw->regmap, + ts_settings->hr_timer.addr, + ts_settings->hr_timer.mask, val); + if (err < 0) + return err; + } + + /* enable ts queueing in FIFO if necessary */ + if (ts_settings->fifo_en.addr) { + val = ST_LSM6DSX_SHIFT_VAL(1, ts_settings->fifo_en.mask); + err = regmap_update_bits(hw->regmap, + ts_settings->fifo_en.addr, + ts_settings->fifo_en.mask, val); + if (err < 0) + return err; + } + + /* calibrate timestamp sensitivity */ + hw->ts_gain = ST_LSM6DSX_TS_SENSITIVITY; + if (ts_settings->freq_fine) { + err = regmap_read(hw->regmap, ts_settings->freq_fine, &val); + if (err < 0) + return err; + + /* + * linearize the AN5192 formula: + * 1 / (1 + x) ~= 1 - x (Taylor’s Series) + * ttrim[s] = 1 / (40000 * (1 + 0.0015 * val)) + * ttrim[ns] ~= 25000 - 37.5 * val + * ttrim[ns] ~= 25000 - (37500 * val) / 1000 + */ + hw->ts_gain -= ((s8)val * 37500) / 1000; + } + + return 0; +} + +static int st_lsm6dsx_reset_device(struct st_lsm6dsx_hw *hw) +{ + const struct st_lsm6dsx_reg *reg; + int err; + + /* + * flush hw FIFO before device reset in order to avoid + * possible races on interrupt line 1. If the first interrupt + * line is asserted during hw reset the device will work in + * I3C-only mode (if it is supported) + */ + err = st_lsm6dsx_flush_fifo(hw); + if (err < 0 && err != -ENOTSUPP) + return err; + + /* device sw reset */ + reg = &hw->settings->reset; + err = regmap_update_bits(hw->regmap, reg->addr, reg->mask, + ST_LSM6DSX_SHIFT_VAL(1, reg->mask)); + if (err < 0) + return err; + + msleep(50); + + /* reload trimming parameter */ + reg = &hw->settings->boot; + err = regmap_update_bits(hw->regmap, reg->addr, reg->mask, + ST_LSM6DSX_SHIFT_VAL(1, reg->mask)); + if (err < 0) + return err; + + msleep(50); + + return 0; +} + +static int st_lsm6dsx_init_device(struct st_lsm6dsx_hw *hw) +{ + const struct st_lsm6dsx_reg *reg; + int err; + + err = st_lsm6dsx_reset_device(hw); + if (err < 0) + return err; + + /* enable Block Data Update */ + reg = &hw->settings->bdu; + err = regmap_update_bits(hw->regmap, reg->addr, reg->mask, + ST_LSM6DSX_SHIFT_VAL(1, reg->mask)); + if (err < 0) + return err; + + /* enable FIFO watermak interrupt */ + err = st_lsm6dsx_get_drdy_reg(hw, ®); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, reg->addr, reg->mask, + ST_LSM6DSX_SHIFT_VAL(1, reg->mask)); + if (err < 0) + return err; + + /* enable Latched interrupts for device events */ + if (hw->settings->irq_config.lir.addr) { + reg = &hw->settings->irq_config.lir; + err = regmap_update_bits(hw->regmap, reg->addr, reg->mask, + ST_LSM6DSX_SHIFT_VAL(1, reg->mask)); + if (err < 0) + return err; + + /* enable clear on read for latched interrupts */ + if (hw->settings->irq_config.clear_on_read.addr) { + reg = &hw->settings->irq_config.clear_on_read; + err = regmap_update_bits(hw->regmap, + reg->addr, reg->mask, + ST_LSM6DSX_SHIFT_VAL(1, reg->mask)); + if (err < 0) + return err; + } + } + + /* enable drdy-mas if available */ + if (hw->settings->drdy_mask.addr) { + reg = &hw->settings->drdy_mask; + err = regmap_update_bits(hw->regmap, reg->addr, reg->mask, + ST_LSM6DSX_SHIFT_VAL(1, reg->mask)); + if (err < 0) + return err; + } + + err = st_lsm6dsx_init_shub(hw); + if (err < 0) + return err; + + return st_lsm6dsx_init_hw_timer(hw); +} + +static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw, + enum st_lsm6dsx_sensor_id id, + const char *name) +{ + struct st_lsm6dsx_sensor *sensor; + struct iio_dev *iio_dev; + + iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor)); + if (!iio_dev) + return NULL; + + iio_dev->modes = INDIO_DIRECT_MODE; + iio_dev->available_scan_masks = st_lsm6dsx_available_scan_masks; + iio_dev->channels = hw->settings->channels[id].chan; + iio_dev->num_channels = hw->settings->channels[id].len; + + sensor = iio_priv(iio_dev); + sensor->id = id; + sensor->hw = hw; + sensor->odr = hw->settings->odr_table[id].odr_avl[0].milli_hz; + sensor->gain = hw->settings->fs_table[id].fs_avl[0].gain; + sensor->watermark = 1; + + switch (id) { + case ST_LSM6DSX_ID_ACC: + iio_dev->info = &st_lsm6dsx_acc_info; + scnprintf(sensor->name, sizeof(sensor->name), "%s_accel", + name); + break; + case ST_LSM6DSX_ID_GYRO: + iio_dev->info = &st_lsm6dsx_gyro_info; + scnprintf(sensor->name, sizeof(sensor->name), "%s_gyro", + name); + break; + default: + return NULL; + } + iio_dev->name = sensor->name; + + return iio_dev; +} + +static bool +st_lsm6dsx_report_motion_event(struct st_lsm6dsx_hw *hw) +{ + const struct st_lsm6dsx_event_settings *event_settings; + int err, data; + s64 timestamp; + + if (!hw->enable_event) + return false; + + event_settings = &hw->settings->event_settings; + err = st_lsm6dsx_read_locked(hw, event_settings->wakeup_src_reg, + &data, sizeof(data)); + if (err < 0) + return false; + + timestamp = iio_get_time_ns(hw->iio_devs[ST_LSM6DSX_ID_ACC]); + if ((data & hw->settings->event_settings.wakeup_src_z_mask) && + (hw->enable_event & BIT(IIO_MOD_Z))) + iio_push_event(hw->iio_devs[ST_LSM6DSX_ID_ACC], + IIO_MOD_EVENT_CODE(IIO_ACCEL, + 0, + IIO_MOD_Z, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_EITHER), + timestamp); + + if ((data & hw->settings->event_settings.wakeup_src_y_mask) && + (hw->enable_event & BIT(IIO_MOD_Y))) + iio_push_event(hw->iio_devs[ST_LSM6DSX_ID_ACC], + IIO_MOD_EVENT_CODE(IIO_ACCEL, + 0, + IIO_MOD_Y, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_EITHER), + timestamp); + + if ((data & hw->settings->event_settings.wakeup_src_x_mask) && + (hw->enable_event & BIT(IIO_MOD_X))) + iio_push_event(hw->iio_devs[ST_LSM6DSX_ID_ACC], + IIO_MOD_EVENT_CODE(IIO_ACCEL, + 0, + IIO_MOD_X, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_EITHER), + timestamp); + + return data & event_settings->wakeup_src_status_mask; +} + +static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private) +{ + struct st_lsm6dsx_hw *hw = private; + int fifo_len = 0, len; + bool event; + + event = st_lsm6dsx_report_motion_event(hw); + + if (!hw->settings->fifo_ops.read_fifo) + return event ? IRQ_HANDLED : IRQ_NONE; + + /* + * If we are using edge IRQs, new samples can arrive while + * processing current interrupt since there are no hw + * guarantees the irq line stays "low" long enough to properly + * detect the new interrupt. In this case the new sample will + * be missed. + * Polling FIFO status register allow us to read new + * samples even if the interrupt arrives while processing + * previous data and the timeslot where the line is "low" is + * too short to be properly detected. + */ + do { + mutex_lock(&hw->fifo_lock); + len = hw->settings->fifo_ops.read_fifo(hw); + mutex_unlock(&hw->fifo_lock); + + if (len > 0) + fifo_len += len; + } while (len > 0); + + return fifo_len || event ? IRQ_HANDLED : IRQ_NONE; +} + +static int st_lsm6dsx_irq_setup(struct st_lsm6dsx_hw *hw) +{ + struct st_sensors_platform_data *pdata; + const struct st_lsm6dsx_reg *reg; + struct device *dev = hw->dev; + unsigned long irq_type; + bool irq_active_low; + int err; + + irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq)); + + switch (irq_type) { + case IRQF_TRIGGER_HIGH: + case IRQF_TRIGGER_RISING: + irq_active_low = false; + break; + case IRQF_TRIGGER_LOW: + case IRQF_TRIGGER_FALLING: + irq_active_low = true; + break; + default: + dev_info(hw->dev, "mode %lx unsupported\n", irq_type); + return -EINVAL; + } + + reg = &hw->settings->irq_config.hla; + err = regmap_update_bits(hw->regmap, reg->addr, reg->mask, + ST_LSM6DSX_SHIFT_VAL(irq_active_low, + reg->mask)); + if (err < 0) + return err; + + pdata = (struct st_sensors_platform_data *)dev->platform_data; + if ((dev_fwnode(dev) && device_property_read_bool(dev, "drive-open-drain")) || + (pdata && pdata->open_drain)) { + reg = &hw->settings->irq_config.od; + err = regmap_update_bits(hw->regmap, reg->addr, reg->mask, + ST_LSM6DSX_SHIFT_VAL(1, reg->mask)); + if (err < 0) + return err; + + irq_type |= IRQF_SHARED; + } + + err = devm_request_threaded_irq(hw->dev, hw->irq, + NULL, + st_lsm6dsx_handler_thread, + irq_type | IRQF_ONESHOT, + "lsm6dsx", hw); + if (err) { + dev_err(hw->dev, "failed to request trigger irq %d\n", + hw->irq); + return err; + } + + return 0; +} + +static int st_lsm6dsx_init_regulators(struct device *dev) +{ + struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev); + int err; + + /* vdd-vddio power regulators */ + hw->regulators[0].supply = "vdd"; + hw->regulators[1].supply = "vddio"; + err = devm_regulator_bulk_get(dev, ARRAY_SIZE(hw->regulators), + hw->regulators); + if (err) + return dev_err_probe(dev, err, "failed to get regulators\n"); + + err = regulator_bulk_enable(ARRAY_SIZE(hw->regulators), + hw->regulators); + if (err) { + dev_err(dev, "failed to enable regulators: %d\n", err); + return err; + } + + msleep(50); + + return 0; +} + +static void st_lsm6dsx_chip_uninit(void *data) +{ + struct st_lsm6dsx_hw *hw = data; + + regulator_bulk_disable(ARRAY_SIZE(hw->regulators), hw->regulators); +} + +int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, + struct regmap *regmap) +{ + struct st_sensors_platform_data *pdata = dev->platform_data; + const struct st_lsm6dsx_shub_settings *hub_settings; + struct st_lsm6dsx_hw *hw; + const char *name = NULL; + int i, err; + + hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL); + if (!hw) + return -ENOMEM; + + dev_set_drvdata(dev, (void *)hw); + + mutex_init(&hw->fifo_lock); + mutex_init(&hw->conf_lock); + mutex_init(&hw->page_lock); + + err = st_lsm6dsx_init_regulators(dev); + if (err) + return err; + + err = devm_add_action_or_reset(dev, st_lsm6dsx_chip_uninit, hw); + if (err) + return err; + + hw->buff = devm_kzalloc(dev, ST_LSM6DSX_BUFF_SIZE, GFP_KERNEL); + if (!hw->buff) + return -ENOMEM; + + hw->dev = dev; + hw->irq = irq; + hw->regmap = regmap; + + err = st_lsm6dsx_check_whoami(hw, hw_id, &name); + if (err < 0) + return err; + + for (i = 0; i < ST_LSM6DSX_ID_EXT0; i++) { + hw->iio_devs[i] = st_lsm6dsx_alloc_iiodev(hw, i, name); + if (!hw->iio_devs[i]) + return -ENOMEM; + } + + err = st_lsm6dsx_init_device(hw); + if (err < 0) + return err; + + hub_settings = &hw->settings->shub_settings; + if (hub_settings->master_en.addr && + (!dev_fwnode(dev) || + !device_property_read_bool(dev, "st,disable-sensor-hub"))) { + err = st_lsm6dsx_shub_probe(hw, name); + if (err < 0) + return err; + } + + if (hw->irq > 0) { + err = st_lsm6dsx_irq_setup(hw); + if (err < 0) + return err; + + err = st_lsm6dsx_fifo_setup(hw); + if (err < 0) + return err; + } + + err = iio_read_mount_matrix(hw->dev, &hw->orientation); + if (err) + return err; + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + if (!hw->iio_devs[i]) + continue; + + err = devm_iio_device_register(hw->dev, hw->iio_devs[i]); + if (err) + return err; + } + + if ((dev_fwnode(dev) && device_property_read_bool(dev, "wakeup-source")) || + (pdata && pdata->wakeup_source)) + device_init_wakeup(dev, true); + + return 0; +} +EXPORT_SYMBOL_NS(st_lsm6dsx_probe, IIO_LSM6DSX); + +static int st_lsm6dsx_suspend(struct device *dev) +{ + struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev); + struct st_lsm6dsx_sensor *sensor; + int i, err = 0; + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + if (!hw->iio_devs[i]) + continue; + + sensor = iio_priv(hw->iio_devs[i]); + if (!(hw->enable_mask & BIT(sensor->id))) + continue; + + if (device_may_wakeup(dev) && + sensor->id == ST_LSM6DSX_ID_ACC && hw->enable_event) { + /* Enable wake from IRQ */ + enable_irq_wake(hw->irq); + continue; + } + + if (sensor->id == ST_LSM6DSX_ID_EXT0 || + sensor->id == ST_LSM6DSX_ID_EXT1 || + sensor->id == ST_LSM6DSX_ID_EXT2) + err = st_lsm6dsx_shub_set_enable(sensor, false); + else + err = st_lsm6dsx_sensor_set_enable(sensor, false); + if (err < 0) + return err; + + hw->suspend_mask |= BIT(sensor->id); + } + + if (hw->fifo_mask) + err = st_lsm6dsx_flush_fifo(hw); + + return err; +} + +static int st_lsm6dsx_resume(struct device *dev) +{ + struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev); + struct st_lsm6dsx_sensor *sensor; + int i, err = 0; + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + if (!hw->iio_devs[i]) + continue; + + sensor = iio_priv(hw->iio_devs[i]); + if (device_may_wakeup(dev) && + sensor->id == ST_LSM6DSX_ID_ACC && hw->enable_event) + disable_irq_wake(hw->irq); + + if (!(hw->suspend_mask & BIT(sensor->id))) + continue; + + if (sensor->id == ST_LSM6DSX_ID_EXT0 || + sensor->id == ST_LSM6DSX_ID_EXT1 || + sensor->id == ST_LSM6DSX_ID_EXT2) + err = st_lsm6dsx_shub_set_enable(sensor, true); + else + err = st_lsm6dsx_sensor_set_enable(sensor, true); + if (err < 0) + return err; + + hw->suspend_mask &= ~BIT(sensor->id); + } + + if (hw->fifo_mask) + err = st_lsm6dsx_resume_fifo(hw); + + return err; +} + +EXPORT_NS_SIMPLE_DEV_PM_OPS(st_lsm6dsx_pm_ops, st_lsm6dsx_suspend, + st_lsm6dsx_resume, IIO_LSM6DSX); + +MODULE_AUTHOR("Lorenzo Bianconi "); +MODULE_AUTHOR("Denis Ciocca "); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c new file mode 100644 index 000000000..307c8c436 --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c @@ -0,0 +1,154 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsx i2c driver + * + * Copyright 2016 STMicroelectronics Inc. + * + * Lorenzo Bianconi + * Denis Ciocca + */ + +#include +#include +#include +#include +#include + +#include "st_lsm6dsx.h" + +static const struct regmap_config st_lsm6dsx_i2c_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_lsm6dsx_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + int hw_id = id->driver_data; + struct regmap *regmap; + + regmap = devm_regmap_init_i2c(client, &st_lsm6dsx_i2c_regmap_config); + if (IS_ERR(regmap)) { + dev_err(&client->dev, "Failed to register i2c regmap %ld\n", PTR_ERR(regmap)); + return PTR_ERR(regmap); + } + + return st_lsm6dsx_probe(&client->dev, client->irq, hw_id, regmap); +} + +static const struct of_device_id st_lsm6dsx_i2c_of_match[] = { + { + .compatible = "st,lsm6ds3", + .data = (void *)ST_LSM6DS3_ID, + }, + { + .compatible = "st,lsm6ds3h", + .data = (void *)ST_LSM6DS3H_ID, + }, + { + .compatible = "st,lsm6dsl", + .data = (void *)ST_LSM6DSL_ID, + }, + { + .compatible = "st,lsm6dsm", + .data = (void *)ST_LSM6DSM_ID, + }, + { + .compatible = "st,ism330dlc", + .data = (void *)ST_ISM330DLC_ID, + }, + { + .compatible = "st,lsm6dso", + .data = (void *)ST_LSM6DSO_ID, + }, + { + .compatible = "st,asm330lhh", + .data = (void *)ST_ASM330LHH_ID, + }, + { + .compatible = "st,lsm6dsox", + .data = (void *)ST_LSM6DSOX_ID, + }, + { + .compatible = "st,lsm6dsr", + .data = (void *)ST_LSM6DSR_ID, + }, + { + .compatible = "st,lsm6ds3tr-c", + .data = (void *)ST_LSM6DS3TRC_ID, + }, + { + .compatible = "st,ism330dhcx", + .data = (void *)ST_ISM330DHCX_ID, + }, + { + .compatible = "st,lsm9ds1-imu", + .data = (void *)ST_LSM9DS1_ID, + }, + { + .compatible = "st,lsm6ds0", + .data = (void *)ST_LSM6DS0_ID, + }, + { + .compatible = "st,lsm6dsrx", + .data = (void *)ST_LSM6DSRX_ID, + }, + { + .compatible = "st,lsm6dst", + .data = (void *)ST_LSM6DST_ID, + }, + { + .compatible = "st,lsm6dsop", + .data = (void *)ST_LSM6DSOP_ID, + }, + { + .compatible = "st,asm330lhhx", + .data = (void *)ST_ASM330LHHX_ID, + }, + { + .compatible = "st,lsm6dstx", + .data = (void *)ST_LSM6DSTX_ID, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lsm6dsx_i2c_of_match); + +static const struct i2c_device_id st_lsm6dsx_i2c_id_table[] = { + { ST_LSM6DS3_DEV_NAME, ST_LSM6DS3_ID }, + { ST_LSM6DS3H_DEV_NAME, ST_LSM6DS3H_ID }, + { ST_LSM6DSL_DEV_NAME, ST_LSM6DSL_ID }, + { ST_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID }, + { ST_ISM330DLC_DEV_NAME, ST_ISM330DLC_ID }, + { ST_LSM6DSO_DEV_NAME, ST_LSM6DSO_ID }, + { ST_ASM330LHH_DEV_NAME, ST_ASM330LHH_ID }, + { ST_LSM6DSOX_DEV_NAME, ST_LSM6DSOX_ID }, + { ST_LSM6DSR_DEV_NAME, ST_LSM6DSR_ID }, + { ST_LSM6DS3TRC_DEV_NAME, ST_LSM6DS3TRC_ID }, + { ST_ISM330DHCX_DEV_NAME, ST_ISM330DHCX_ID }, + { ST_LSM9DS1_DEV_NAME, ST_LSM9DS1_ID }, + { ST_LSM6DS0_DEV_NAME, ST_LSM6DS0_ID }, + { ST_LSM6DSRX_DEV_NAME, ST_LSM6DSRX_ID }, + { ST_LSM6DST_DEV_NAME, ST_LSM6DST_ID }, + { ST_LSM6DSOP_DEV_NAME, ST_LSM6DSOP_ID }, + { ST_ASM330LHHX_DEV_NAME, ST_ASM330LHHX_ID }, + { ST_LSM6DSTX_DEV_NAME, ST_LSM6DSTX_ID }, + {}, +}; +MODULE_DEVICE_TABLE(i2c, st_lsm6dsx_i2c_id_table); + +static struct i2c_driver st_lsm6dsx_driver = { + .driver = { + .name = "st_lsm6dsx_i2c", + .pm = pm_sleep_ptr(&st_lsm6dsx_pm_ops), + .of_match_table = st_lsm6dsx_i2c_of_match, + }, + .probe = st_lsm6dsx_i2c_probe, + .id_table = st_lsm6dsx_i2c_id_table, +}; +module_i2c_driver(st_lsm6dsx_driver); + +MODULE_AUTHOR("Lorenzo Bianconi "); +MODULE_AUTHOR("Denis Ciocca "); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx i2c driver"); +MODULE_LICENSE("GPL v2"); +MODULE_IMPORT_NS(IIO_LSM6DSX); diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i3c.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i3c.c new file mode 100644 index 000000000..3b0c8b19c --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i3c.c @@ -0,0 +1,57 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Copyright (c) 2018 Synopsys, Inc. and/or its affiliates. + * + * Author: Vitor Soares + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "st_lsm6dsx.h" + +static const struct i3c_device_id st_lsm6dsx_i3c_ids[] = { + I3C_DEVICE(0x0104, 0x006C, (void *)ST_LSM6DSO_ID), + I3C_DEVICE(0x0104, 0x006B, (void *)ST_LSM6DSR_ID), + { /* sentinel */ }, +}; +MODULE_DEVICE_TABLE(i3c, st_lsm6dsx_i3c_ids); + +static int st_lsm6dsx_i3c_probe(struct i3c_device *i3cdev) +{ + struct regmap_config st_lsm6dsx_i3c_regmap_config = { + .reg_bits = 8, + .val_bits = 8, + }; + const struct i3c_device_id *id = i3c_device_match_id(i3cdev, + st_lsm6dsx_i3c_ids); + struct regmap *regmap; + + regmap = devm_regmap_init_i3c(i3cdev, &st_lsm6dsx_i3c_regmap_config); + if (IS_ERR(regmap)) { + dev_err(&i3cdev->dev, "Failed to register i3c regmap %ld\n", PTR_ERR(regmap)); + return PTR_ERR(regmap); + } + + return st_lsm6dsx_probe(&i3cdev->dev, 0, (uintptr_t)id->data, regmap); +} + +static struct i3c_driver st_lsm6dsx_driver = { + .driver = { + .name = "st_lsm6dsx_i3c", + .pm = pm_sleep_ptr(&st_lsm6dsx_pm_ops), + }, + .probe = st_lsm6dsx_i3c_probe, + .id_table = st_lsm6dsx_i3c_ids, +}; +module_i3c_driver(st_lsm6dsx_driver); + +MODULE_AUTHOR("Vitor Soares "); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx i3c driver"); +MODULE_LICENSE("GPL v2"); +MODULE_IMPORT_NS(IIO_LSM6DSX); diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c new file mode 100644 index 000000000..99562ba85 --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c @@ -0,0 +1,919 @@ +/* + * STMicroelectronics st_lsm6dsx i2c controller driver + * + * i2c controller embedded in lsm6dx series can connect up to four + * slave devices using accelerometer sensor as trigger for i2c + * read/write operations. Current implementation relies on SLV0 channel + * for slave configuration and SLV{1,2,3} to read data and push them into + * the hw FIFO + * + * Copyright (C) 2018 Lorenzo Bianconi + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ +#include +#include +#include +#include +#include + +#include "st_lsm6dsx.h" + +#define ST_LSM6DSX_SLV_ADDR(n, base) ((base) + (n) * 3) +#define ST_LSM6DSX_SLV_SUB_ADDR(n, base) ((base) + 1 + (n) * 3) +#define ST_LSM6DSX_SLV_CONFIG(n, base) ((base) + 2 + (n) * 3) + +#define ST_LS6DSX_READ_OP_MASK GENMASK(2, 0) + +static const struct st_lsm6dsx_ext_dev_settings st_lsm6dsx_ext_dev_table[] = { + /* LIS2MDL */ + { + .i2c_addr = { 0x1e }, + .wai = { + .addr = 0x4f, + .val = 0x40, + }, + .id = ST_LSM6DSX_ID_MAGN, + .odr_table = { + .reg = { + .addr = 0x60, + .mask = GENMASK(3, 2), + }, + .odr_avl[0] = { 10000, 0x0 }, + .odr_avl[1] = { 20000, 0x1 }, + .odr_avl[2] = { 50000, 0x2 }, + .odr_avl[3] = { 100000, 0x3 }, + .odr_len = 4, + }, + .fs_table = { + .fs_avl[0] = { + .gain = 1500, + .val = 0x0, + }, /* 1500 uG/LSB */ + .fs_len = 1, + }, + .temp_comp = { + .addr = 0x60, + .mask = BIT(7), + }, + .pwr_table = { + .reg = { + .addr = 0x60, + .mask = GENMASK(1, 0), + }, + .off_val = 0x2, + .on_val = 0x0, + }, + .off_canc = { + .addr = 0x61, + .mask = BIT(1), + }, + .bdu = { + .addr = 0x62, + .mask = BIT(4), + }, + .out = { + .addr = 0x68, + .len = 6, + }, + }, + /* LIS3MDL */ + { + .i2c_addr = { 0x1e }, + .wai = { + .addr = 0x0f, + .val = 0x3d, + }, + .id = ST_LSM6DSX_ID_MAGN, + .odr_table = { + .reg = { + .addr = 0x20, + .mask = GENMASK(4, 2), + }, + .odr_avl[0] = { 1000, 0x0 }, + .odr_avl[1] = { 2000, 0x1 }, + .odr_avl[2] = { 3000, 0x2 }, + .odr_avl[3] = { 5000, 0x3 }, + .odr_avl[4] = { 10000, 0x4 }, + .odr_avl[5] = { 20000, 0x5 }, + .odr_avl[6] = { 40000, 0x6 }, + .odr_avl[7] = { 80000, 0x7 }, + .odr_len = 8, + }, + .fs_table = { + .reg = { + .addr = 0x21, + .mask = GENMASK(6, 5), + }, + .fs_avl[0] = { + .gain = 146, + .val = 0x00, + }, /* 4000 uG/LSB */ + .fs_avl[1] = { + .gain = 292, + .val = 0x01, + }, /* 8000 uG/LSB */ + .fs_avl[2] = { + .gain = 438, + .val = 0x02, + }, /* 12000 uG/LSB */ + .fs_avl[3] = { + .gain = 584, + .val = 0x03, + }, /* 16000 uG/LSB */ + .fs_len = 4, + }, + .pwr_table = { + .reg = { + .addr = 0x22, + .mask = GENMASK(1, 0), + }, + .off_val = 0x2, + .on_val = 0x0, + }, + .bdu = { + .addr = 0x24, + .mask = BIT(6), + }, + .out = { + .addr = 0x28, + .len = 6, + }, + }, +}; + +static void st_lsm6dsx_shub_wait_complete(struct st_lsm6dsx_hw *hw) +{ + struct st_lsm6dsx_sensor *sensor; + u32 odr, timeout; + + sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]); + odr = (hw->enable_mask & BIT(ST_LSM6DSX_ID_ACC)) ? sensor->odr : 12500; + /* set 10ms as minimum timeout for i2c slave configuration */ + timeout = max_t(u32, 2000000U / odr + 1, 10); + msleep(timeout); +} + +/* + * st_lsm6dsx_shub_read_output - read i2c controller register + * + * Read st_lsm6dsx i2c controller register + */ +static int +st_lsm6dsx_shub_read_output(struct st_lsm6dsx_hw *hw, u8 *data, + int len) +{ + const struct st_lsm6dsx_shub_settings *hub_settings; + int err; + + mutex_lock(&hw->page_lock); + + hub_settings = &hw->settings->shub_settings; + if (hub_settings->shub_out.sec_page) { + err = st_lsm6dsx_set_page(hw, true); + if (err < 0) + goto out; + } + + err = regmap_bulk_read(hw->regmap, hub_settings->shub_out.addr, + data, len); + + if (hub_settings->shub_out.sec_page) + st_lsm6dsx_set_page(hw, false); +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/* + * st_lsm6dsx_shub_write_reg - write i2c controller register + * + * Write st_lsm6dsx i2c controller register + */ +static int st_lsm6dsx_shub_write_reg(struct st_lsm6dsx_hw *hw, u8 addr, + u8 *data, int len) +{ + int err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsx_set_page(hw, true); + if (err < 0) + goto out; + + err = regmap_bulk_write(hw->regmap, addr, data, len); + + st_lsm6dsx_set_page(hw, false); +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +static int +st_lsm6dsx_shub_write_reg_with_mask(struct st_lsm6dsx_hw *hw, u8 addr, + u8 mask, u8 val) +{ + int err; + + mutex_lock(&hw->page_lock); + err = st_lsm6dsx_set_page(hw, true); + if (err < 0) + goto out; + + err = regmap_update_bits(hw->regmap, addr, mask, val); + + st_lsm6dsx_set_page(hw, false); +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +static int st_lsm6dsx_shub_master_enable(struct st_lsm6dsx_sensor *sensor, + bool enable) +{ + const struct st_lsm6dsx_shub_settings *hub_settings; + struct st_lsm6dsx_hw *hw = sensor->hw; + unsigned int data; + int err; + + /* enable acc sensor as trigger */ + err = st_lsm6dsx_sensor_set_enable(sensor, enable); + if (err < 0) + return err; + + mutex_lock(&hw->page_lock); + + hub_settings = &hw->settings->shub_settings; + if (hub_settings->master_en.sec_page) { + err = st_lsm6dsx_set_page(hw, true); + if (err < 0) + goto out; + } + + data = ST_LSM6DSX_SHIFT_VAL(enable, hub_settings->master_en.mask); + err = regmap_update_bits(hw->regmap, hub_settings->master_en.addr, + hub_settings->master_en.mask, data); + + if (hub_settings->master_en.sec_page) + st_lsm6dsx_set_page(hw, false); +out: + mutex_unlock(&hw->page_lock); + + return err; +} + +/* + * st_lsm6dsx_shub_read - read data from slave device register + * + * Read data from slave device register. SLV0 is used for + * one-shot read operation + */ +static int +st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr, + u8 *data, int len) +{ + const struct st_lsm6dsx_shub_settings *hub_settings; + u8 config[3], slv_addr, slv_config = 0; + struct st_lsm6dsx_hw *hw = sensor->hw; + const struct st_lsm6dsx_reg *aux_sens; + int err; + + hub_settings = &hw->settings->shub_settings; + slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr); + aux_sens = &hw->settings->shub_settings.aux_sens; + /* do not overwrite aux_sens */ + if (slv_addr + 2 == aux_sens->addr) + slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask); + + config[0] = (sensor->ext_info.addr << 1) | 1; + config[1] = addr; + config[2] = (len & ST_LS6DSX_READ_OP_MASK) | slv_config; + + err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config, + sizeof(config)); + if (err < 0) + return err; + + err = st_lsm6dsx_shub_master_enable(sensor, true); + if (err < 0) + return err; + + st_lsm6dsx_shub_wait_complete(hw); + + err = st_lsm6dsx_shub_read_output(hw, data, + len & ST_LS6DSX_READ_OP_MASK); + if (err < 0) + return err; + + st_lsm6dsx_shub_master_enable(sensor, false); + + config[0] = hub_settings->pause; + config[1] = 0; + config[2] = slv_config; + return st_lsm6dsx_shub_write_reg(hw, slv_addr, config, + sizeof(config)); +} + +/* + * st_lsm6dsx_shub_write - write data to slave device register + * + * Write data from slave device register. SLV0 is used for + * one-shot write operation + */ +static int +st_lsm6dsx_shub_write(struct st_lsm6dsx_sensor *sensor, u8 addr, + u8 *data, int len) +{ + const struct st_lsm6dsx_shub_settings *hub_settings; + struct st_lsm6dsx_hw *hw = sensor->hw; + u8 config[2], slv_addr; + int err, i; + + hub_settings = &hw->settings->shub_settings; + if (hub_settings->wr_once.addr) { + unsigned int data; + + data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->wr_once.mask); + err = st_lsm6dsx_shub_write_reg_with_mask(hw, + hub_settings->wr_once.addr, + hub_settings->wr_once.mask, + data); + if (err < 0) + return err; + } + + slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr); + config[0] = sensor->ext_info.addr << 1; + for (i = 0 ; i < len; i++) { + config[1] = addr + i; + + err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config, + sizeof(config)); + if (err < 0) + return err; + + err = st_lsm6dsx_shub_write_reg(hw, hub_settings->dw_slv0_addr, + &data[i], 1); + if (err < 0) + return err; + + err = st_lsm6dsx_shub_master_enable(sensor, true); + if (err < 0) + return err; + + st_lsm6dsx_shub_wait_complete(hw); + + st_lsm6dsx_shub_master_enable(sensor, false); + } + + config[0] = hub_settings->pause; + config[1] = 0; + return st_lsm6dsx_shub_write_reg(hw, slv_addr, config, sizeof(config)); +} + +static int +st_lsm6dsx_shub_write_with_mask(struct st_lsm6dsx_sensor *sensor, + u8 addr, u8 mask, u8 val) +{ + int err; + u8 data; + + err = st_lsm6dsx_shub_read(sensor, addr, &data, sizeof(data)); + if (err < 0) + return err; + + data = ((data & ~mask) | (val << __ffs(mask) & mask)); + + return st_lsm6dsx_shub_write(sensor, addr, &data, sizeof(data)); +} + +static int +st_lsm6dsx_shub_get_odr_val(struct st_lsm6dsx_sensor *sensor, + u32 odr, u16 *val) +{ + const struct st_lsm6dsx_ext_dev_settings *settings; + int i; + + settings = sensor->ext_info.settings; + for (i = 0; i < settings->odr_table.odr_len; i++) { + if (settings->odr_table.odr_avl[i].milli_hz == odr) + break; + } + + if (i == settings->odr_table.odr_len) + return -EINVAL; + + *val = settings->odr_table.odr_avl[i].val; + return 0; +} + +static int +st_lsm6dsx_shub_set_odr(struct st_lsm6dsx_sensor *sensor, u32 odr) +{ + const struct st_lsm6dsx_ext_dev_settings *settings; + u16 val; + int err; + + err = st_lsm6dsx_shub_get_odr_val(sensor, odr, &val); + if (err < 0) + return err; + + settings = sensor->ext_info.settings; + return st_lsm6dsx_shub_write_with_mask(sensor, + settings->odr_table.reg.addr, + settings->odr_table.reg.mask, + val); +} + +/* use SLV{1,2,3} for FIFO read operations */ +static int +st_lsm6dsx_shub_config_channels(struct st_lsm6dsx_sensor *sensor, + bool enable) +{ + const struct st_lsm6dsx_shub_settings *hub_settings; + const struct st_lsm6dsx_ext_dev_settings *settings; + u8 config[9] = {}, enable_mask, slv_addr; + struct st_lsm6dsx_hw *hw = sensor->hw; + struct st_lsm6dsx_sensor *cur_sensor; + int i, j = 0; + + hub_settings = &hw->settings->shub_settings; + if (enable) + enable_mask = hw->enable_mask | BIT(sensor->id); + else + enable_mask = hw->enable_mask & ~BIT(sensor->id); + + for (i = ST_LSM6DSX_ID_EXT0; i <= ST_LSM6DSX_ID_EXT2; i++) { + if (!hw->iio_devs[i]) + continue; + + cur_sensor = iio_priv(hw->iio_devs[i]); + if (!(enable_mask & BIT(cur_sensor->id))) + continue; + + settings = cur_sensor->ext_info.settings; + config[j] = (sensor->ext_info.addr << 1) | 1; + config[j + 1] = settings->out.addr; + config[j + 2] = (settings->out.len & ST_LS6DSX_READ_OP_MASK) | + hub_settings->batch_en; + j += 3; + } + + slv_addr = ST_LSM6DSX_SLV_ADDR(1, hub_settings->slv0_addr); + return st_lsm6dsx_shub_write_reg(hw, slv_addr, config, + sizeof(config)); +} + +int st_lsm6dsx_shub_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable) +{ + const struct st_lsm6dsx_ext_dev_settings *settings; + int err; + + err = st_lsm6dsx_shub_config_channels(sensor, enable); + if (err < 0) + return err; + + settings = sensor->ext_info.settings; + if (enable) { + err = st_lsm6dsx_shub_set_odr(sensor, + sensor->ext_info.slv_odr); + if (err < 0) + return err; + } else { + err = st_lsm6dsx_shub_write_with_mask(sensor, + settings->odr_table.reg.addr, + settings->odr_table.reg.mask, 0); + if (err < 0) + return err; + } + + if (settings->pwr_table.reg.addr) { + u8 val; + + val = enable ? settings->pwr_table.on_val + : settings->pwr_table.off_val; + err = st_lsm6dsx_shub_write_with_mask(sensor, + settings->pwr_table.reg.addr, + settings->pwr_table.reg.mask, val); + if (err < 0) + return err; + } + + return st_lsm6dsx_shub_master_enable(sensor, enable); +} + +static int +st_lsm6dsx_shub_read_oneshot(struct st_lsm6dsx_sensor *sensor, + struct iio_chan_spec const *ch, + int *val) +{ + int err, delay, len; + u8 data[4]; + + err = st_lsm6dsx_shub_set_enable(sensor, true); + if (err < 0) + return err; + + delay = 1000000000 / sensor->ext_info.slv_odr; + usleep_range(delay, 2 * delay); + + len = min_t(int, sizeof(data), ch->scan_type.realbits >> 3); + err = st_lsm6dsx_shub_read(sensor, ch->address, data, len); + if (err < 0) + return err; + + err = st_lsm6dsx_shub_set_enable(sensor, false); + if (err < 0) + return err; + + switch (len) { + case 2: + *val = (s16)le16_to_cpu(*((__le16 *)data)); + break; + default: + return -EINVAL; + } + + return IIO_VAL_INT; +} + +static int +st_lsm6dsx_shub_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + ret = iio_device_claim_direct_mode(iio_dev); + if (ret) + break; + + ret = st_lsm6dsx_shub_read_oneshot(sensor, ch, val); + iio_device_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = sensor->ext_info.slv_odr / 1000; + *val2 = (sensor->ext_info.slv_odr % 1000) * 1000; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sensor->gain; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +static int +st_lsm6dsx_shub_set_full_scale(struct st_lsm6dsx_sensor *sensor, + u32 gain) +{ + const struct st_lsm6dsx_fs_table_entry *fs_table; + int i, err; + + fs_table = &sensor->ext_info.settings->fs_table; + if (!fs_table->reg.addr) + return -ENOTSUPP; + + for (i = 0; i < fs_table->fs_len; i++) { + if (fs_table->fs_avl[i].gain == gain) + break; + } + + if (i == fs_table->fs_len) + return -EINVAL; + + err = st_lsm6dsx_shub_write_with_mask(sensor, fs_table->reg.addr, + fs_table->reg.mask, + fs_table->fs_avl[i].val); + if (err < 0) + return err; + + sensor->gain = gain; + + return 0; +} + +static int +st_lsm6dsx_shub_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + int err; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: { + u16 data; + + val = val * 1000 + val2 / 1000; + err = st_lsm6dsx_shub_get_odr_val(sensor, val, &data); + if (!err) { + struct st_lsm6dsx_hw *hw = sensor->hw; + struct st_lsm6dsx_sensor *ref_sensor; + u8 odr_val; + int odr; + + ref_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]); + odr = st_lsm6dsx_check_odr(ref_sensor, val, &odr_val); + if (odr < 0) { + err = odr; + goto release; + } + + sensor->ext_info.slv_odr = val; + sensor->odr = odr; + } + break; + } + case IIO_CHAN_INFO_SCALE: + err = st_lsm6dsx_shub_set_full_scale(sensor, val2); + break; + default: + err = -EINVAL; + break; + } + +release: + iio_device_release_direct_mode(iio_dev); + + return err; +} + +static ssize_t +st_lsm6dsx_shub_sampling_freq_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + const struct st_lsm6dsx_ext_dev_settings *settings; + int i, len = 0; + + settings = sensor->ext_info.settings; + for (i = 0; i < settings->odr_table.odr_len; i++) { + u32 val = settings->odr_table.odr_avl[i].milli_hz; + + len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%03d ", + val / 1000, val % 1000); + } + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_lsm6dsx_shub_scale_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + const struct st_lsm6dsx_ext_dev_settings *settings; + int i, len = 0; + + settings = sensor->ext_info.settings; + for (i = 0; i < settings->fs_table.fs_len; i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", + settings->fs_table.fs_avl[i].gain); + buf[len - 1] = '\n'; + + return len; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_shub_sampling_freq_avail); +static IIO_DEVICE_ATTR(in_scale_available, 0444, + st_lsm6dsx_shub_scale_avail, NULL, 0); +static struct attribute *st_lsm6dsx_ext_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_scale_available.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsx_ext_attribute_group = { + .attrs = st_lsm6dsx_ext_attributes, +}; + +static const struct iio_info st_lsm6dsx_ext_info = { + .attrs = &st_lsm6dsx_ext_attribute_group, + .read_raw = st_lsm6dsx_shub_read_raw, + .write_raw = st_lsm6dsx_shub_write_raw, + .hwfifo_set_watermark = st_lsm6dsx_set_watermark, +}; + +static struct iio_dev * +st_lsm6dsx_shub_alloc_iiodev(struct st_lsm6dsx_hw *hw, + enum st_lsm6dsx_sensor_id id, + const struct st_lsm6dsx_ext_dev_settings *info, + u8 i2c_addr, const char *name) +{ + enum st_lsm6dsx_sensor_id ref_id = ST_LSM6DSX_ID_ACC; + struct iio_chan_spec *ext_channels; + struct st_lsm6dsx_sensor *sensor; + struct iio_dev *iio_dev; + + iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor)); + if (!iio_dev) + return NULL; + + iio_dev->modes = INDIO_DIRECT_MODE; + iio_dev->info = &st_lsm6dsx_ext_info; + + sensor = iio_priv(iio_dev); + sensor->id = id; + sensor->hw = hw; + sensor->odr = hw->settings->odr_table[ref_id].odr_avl[0].milli_hz; + sensor->ext_info.slv_odr = info->odr_table.odr_avl[0].milli_hz; + sensor->gain = info->fs_table.fs_avl[0].gain; + sensor->ext_info.settings = info; + sensor->ext_info.addr = i2c_addr; + sensor->watermark = 1; + + switch (info->id) { + case ST_LSM6DSX_ID_MAGN: { + const struct iio_chan_spec magn_channels[] = { + ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr, + IIO_MOD_X, 0), + ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr + 2, + IIO_MOD_Y, 1), + ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr + 4, + IIO_MOD_Z, 2), + IIO_CHAN_SOFT_TIMESTAMP(3), + }; + + ext_channels = devm_kzalloc(hw->dev, sizeof(magn_channels), + GFP_KERNEL); + if (!ext_channels) + return NULL; + + memcpy(ext_channels, magn_channels, sizeof(magn_channels)); + iio_dev->available_scan_masks = st_lsm6dsx_available_scan_masks; + iio_dev->channels = ext_channels; + iio_dev->num_channels = ARRAY_SIZE(magn_channels); + + scnprintf(sensor->name, sizeof(sensor->name), "%s_magn", + name); + break; + } + default: + return NULL; + } + iio_dev->name = sensor->name; + + return iio_dev; +} + +static int st_lsm6dsx_shub_init_device(struct st_lsm6dsx_sensor *sensor) +{ + const struct st_lsm6dsx_ext_dev_settings *settings; + int err; + + settings = sensor->ext_info.settings; + if (settings->bdu.addr) { + err = st_lsm6dsx_shub_write_with_mask(sensor, + settings->bdu.addr, + settings->bdu.mask, 1); + if (err < 0) + return err; + } + + if (settings->temp_comp.addr) { + err = st_lsm6dsx_shub_write_with_mask(sensor, + settings->temp_comp.addr, + settings->temp_comp.mask, 1); + if (err < 0) + return err; + } + + if (settings->off_canc.addr) { + err = st_lsm6dsx_shub_write_with_mask(sensor, + settings->off_canc.addr, + settings->off_canc.mask, 1); + if (err < 0) + return err; + } + + return 0; +} + +static int +st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr, + const struct st_lsm6dsx_ext_dev_settings *settings) +{ + const struct st_lsm6dsx_shub_settings *hub_settings; + u8 config[3], data, slv_addr, slv_config = 0; + const struct st_lsm6dsx_reg *aux_sens; + struct st_lsm6dsx_sensor *sensor; + bool found = false; + int i, err; + + sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]); + hub_settings = &hw->settings->shub_settings; + aux_sens = &hw->settings->shub_settings.aux_sens; + slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr); + /* do not overwrite aux_sens */ + if (slv_addr + 2 == aux_sens->addr) + slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask); + + for (i = 0; i < ARRAY_SIZE(settings->i2c_addr); i++) { + if (!settings->i2c_addr[i]) + continue; + + /* read wai slave register */ + config[0] = (settings->i2c_addr[i] << 1) | 0x1; + config[1] = settings->wai.addr; + config[2] = 0x1 | slv_config; + + err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config, + sizeof(config)); + if (err < 0) + return err; + + err = st_lsm6dsx_shub_master_enable(sensor, true); + if (err < 0) + return err; + + st_lsm6dsx_shub_wait_complete(hw); + + err = st_lsm6dsx_shub_read_output(hw, &data, sizeof(data)); + + st_lsm6dsx_shub_master_enable(sensor, false); + + if (err < 0) + return err; + + if (data != settings->wai.val) + continue; + + *i2c_addr = settings->i2c_addr[i]; + found = true; + break; + } + + /* reset SLV0 channel */ + config[0] = hub_settings->pause; + config[1] = 0; + config[2] = slv_config; + err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config, + sizeof(config)); + if (err < 0) + return err; + + return found ? 0 : -ENODEV; +} + +int st_lsm6dsx_shub_probe(struct st_lsm6dsx_hw *hw, const char *name) +{ + enum st_lsm6dsx_sensor_id id = ST_LSM6DSX_ID_EXT0; + struct st_lsm6dsx_sensor *sensor; + int err, i, num_ext_dev = 0; + u8 i2c_addr = 0; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_ext_dev_table); i++) { + err = st_lsm6dsx_shub_check_wai(hw, &i2c_addr, + &st_lsm6dsx_ext_dev_table[i]); + if (err == -ENODEV) + continue; + else if (err < 0) + return err; + + hw->iio_devs[id] = st_lsm6dsx_shub_alloc_iiodev(hw, id, + &st_lsm6dsx_ext_dev_table[i], + i2c_addr, name); + if (!hw->iio_devs[id]) + return -ENOMEM; + + sensor = iio_priv(hw->iio_devs[id]); + err = st_lsm6dsx_shub_init_device(sensor); + if (err < 0) + return err; + + if (++num_ext_dev >= hw->settings->shub_settings.num_ext_dev) + break; + id++; + } + + return 0; +} diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c new file mode 100644 index 000000000..6a4eecf4b --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c @@ -0,0 +1,154 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lsm6dsx spi driver + * + * Copyright 2016 STMicroelectronics Inc. + * + * Lorenzo Bianconi + * Denis Ciocca + */ + +#include +#include +#include +#include +#include + +#include "st_lsm6dsx.h" + +static const struct regmap_config st_lsm6dsx_spi_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_lsm6dsx_spi_probe(struct spi_device *spi) +{ + const struct spi_device_id *id = spi_get_device_id(spi); + int hw_id = id->driver_data; + struct regmap *regmap; + + regmap = devm_regmap_init_spi(spi, &st_lsm6dsx_spi_regmap_config); + if (IS_ERR(regmap)) { + dev_err(&spi->dev, "Failed to register spi regmap %ld\n", PTR_ERR(regmap)); + return PTR_ERR(regmap); + } + + return st_lsm6dsx_probe(&spi->dev, spi->irq, hw_id, regmap); +} + +static const struct of_device_id st_lsm6dsx_spi_of_match[] = { + { + .compatible = "st,lsm6ds3", + .data = (void *)ST_LSM6DS3_ID, + }, + { + .compatible = "st,lsm6ds3h", + .data = (void *)ST_LSM6DS3H_ID, + }, + { + .compatible = "st,lsm6dsl", + .data = (void *)ST_LSM6DSL_ID, + }, + { + .compatible = "st,lsm6dsm", + .data = (void *)ST_LSM6DSM_ID, + }, + { + .compatible = "st,ism330dlc", + .data = (void *)ST_ISM330DLC_ID, + }, + { + .compatible = "st,lsm6dso", + .data = (void *)ST_LSM6DSO_ID, + }, + { + .compatible = "st,asm330lhh", + .data = (void *)ST_ASM330LHH_ID, + }, + { + .compatible = "st,lsm6dsox", + .data = (void *)ST_LSM6DSOX_ID, + }, + { + .compatible = "st,lsm6dsr", + .data = (void *)ST_LSM6DSR_ID, + }, + { + .compatible = "st,lsm6ds3tr-c", + .data = (void *)ST_LSM6DS3TRC_ID, + }, + { + .compatible = "st,ism330dhcx", + .data = (void *)ST_ISM330DHCX_ID, + }, + { + .compatible = "st,lsm9ds1-imu", + .data = (void *)ST_LSM9DS1_ID, + }, + { + .compatible = "st,lsm6ds0", + .data = (void *)ST_LSM6DS0_ID, + }, + { + .compatible = "st,lsm6dsrx", + .data = (void *)ST_LSM6DSRX_ID, + }, + { + .compatible = "st,lsm6dst", + .data = (void *)ST_LSM6DST_ID, + }, + { + .compatible = "st,lsm6dsop", + .data = (void *)ST_LSM6DSOP_ID, + }, + { + .compatible = "st,asm330lhhx", + .data = (void *)ST_ASM330LHHX_ID, + }, + { + .compatible = "st,lsm6dstx", + .data = (void *)ST_LSM6DSTX_ID, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lsm6dsx_spi_of_match); + +static const struct spi_device_id st_lsm6dsx_spi_id_table[] = { + { ST_LSM6DS3_DEV_NAME, ST_LSM6DS3_ID }, + { ST_LSM6DS3H_DEV_NAME, ST_LSM6DS3H_ID }, + { ST_LSM6DSL_DEV_NAME, ST_LSM6DSL_ID }, + { ST_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID }, + { ST_ISM330DLC_DEV_NAME, ST_ISM330DLC_ID }, + { ST_LSM6DSO_DEV_NAME, ST_LSM6DSO_ID }, + { ST_ASM330LHH_DEV_NAME, ST_ASM330LHH_ID }, + { ST_LSM6DSOX_DEV_NAME, ST_LSM6DSOX_ID }, + { ST_LSM6DSR_DEV_NAME, ST_LSM6DSR_ID }, + { ST_LSM6DS3TRC_DEV_NAME, ST_LSM6DS3TRC_ID }, + { ST_ISM330DHCX_DEV_NAME, ST_ISM330DHCX_ID }, + { ST_LSM9DS1_DEV_NAME, ST_LSM9DS1_ID }, + { ST_LSM6DS0_DEV_NAME, ST_LSM6DS0_ID }, + { ST_LSM6DSRX_DEV_NAME, ST_LSM6DSRX_ID }, + { ST_LSM6DST_DEV_NAME, ST_LSM6DST_ID }, + { ST_LSM6DSOP_DEV_NAME, ST_LSM6DSOP_ID }, + { ST_ASM330LHHX_DEV_NAME, ST_ASM330LHHX_ID }, + { ST_LSM6DSTX_DEV_NAME, ST_LSM6DSTX_ID }, + {}, +}; +MODULE_DEVICE_TABLE(spi, st_lsm6dsx_spi_id_table); + +static struct spi_driver st_lsm6dsx_driver = { + .driver = { + .name = "st_lsm6dsx_spi", + .pm = pm_sleep_ptr(&st_lsm6dsx_pm_ops), + .of_match_table = st_lsm6dsx_spi_of_match, + }, + .probe = st_lsm6dsx_spi_probe, + .id_table = st_lsm6dsx_spi_id_table, +}; +module_spi_driver(st_lsm6dsx_driver); + +MODULE_AUTHOR("Lorenzo Bianconi "); +MODULE_AUTHOR("Denis Ciocca "); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx spi driver"); +MODULE_LICENSE("GPL v2"); +MODULE_IMPORT_NS(IIO_LSM6DSX); diff --git a/drivers/iio/imu/st_lsm9ds0/Kconfig b/drivers/iio/imu/st_lsm9ds0/Kconfig new file mode 100644 index 000000000..d29558ede --- /dev/null +++ b/drivers/iio/imu/st_lsm9ds0/Kconfig @@ -0,0 +1,44 @@ +# SPDX-License-Identifier: GPL-2.0-only + +config IIO_ST_LSM9DS0 + tristate "STMicroelectronics LSM9DS0 IMU driver" + depends on (I2C || SPI_MASTER) && SYSFS + depends on !SENSORS_LIS3_I2C + depends on !SENSORS_LIS3_SPI + select IIO_ST_ACCEL_3AXIS + select IIO_ST_MAGN_3AXIS + + help + Say yes here to build support for STMicroelectronics LSM9DS0 IMU + sensor. Supported devices: accelerometer/magnetometer of lsm9ds0. + + To compile this driver as a module, choose M here: the module + will be called st_lsm9ds0. + + Also need to enable at least one of I2C and SPI interface drivers + +config IIO_ST_LSM9DS0_I2C + tristate "STMicroelectronics LSM9DS0 IMU I2C interface" + depends on I2C && IIO_ST_LSM9DS0 + default I2C && IIO_ST_LSM9DS0 + select IIO_ST_ACCEL_I2C_3AXIS + select IIO_ST_MAGN_I2C_3AXIS + select REGMAP_I2C + help + Build support for STMicroelectronics LSM9DS0 IMU I2C interface. + + To compile this driver as a module, choose M here. The module + will be called st_lsm9ds0_i2c. + +config IIO_ST_LSM9DS0_SPI + tristate "STMicroelectronics LSM9DS0 IMU SPI interface" + depends on SPI_MASTER && IIO_ST_LSM9DS0 + default SPI_MASTER && IIO_ST_LSM9DS0 + select IIO_ST_ACCEL_SPI_3AXIS + select IIO_ST_MAGN_SPI_3AXIS + select REGMAP_SPI + help + Build support for STMicroelectronics LSM9DS0 IMU I2C interface. + + To compile this driver as a module, choose M here. The module + will be called st_lsm9ds0_spi. diff --git a/drivers/iio/imu/st_lsm9ds0/Makefile b/drivers/iio/imu/st_lsm9ds0/Makefile new file mode 100644 index 000000000..488af523f --- /dev/null +++ b/drivers/iio/imu/st_lsm9ds0/Makefile @@ -0,0 +1,5 @@ +# SPDX-License-Identifier: GPL-2.0-only +obj-$(CONFIG_IIO_ST_LSM9DS0) += st_lsm9ds0.o +st_lsm9ds0-y := st_lsm9ds0_core.o +obj-$(CONFIG_IIO_ST_LSM9DS0_I2C) += st_lsm9ds0_i2c.o +obj-$(CONFIG_IIO_ST_LSM9DS0_SPI) += st_lsm9ds0_spi.o diff --git a/drivers/iio/imu/st_lsm9ds0/st_lsm9ds0.h b/drivers/iio/imu/st_lsm9ds0/st_lsm9ds0.h new file mode 100644 index 000000000..76678cdef --- /dev/null +++ b/drivers/iio/imu/st_lsm9ds0/st_lsm9ds0.h @@ -0,0 +1,22 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +// STMicroelectronics LSM9DS0 IMU driver + +#ifndef ST_LSM9DS0_H +#define ST_LSM9DS0_H + +struct iio_dev; +struct regulator; + +struct st_lsm9ds0 { + struct device *dev; + const char *name; + int irq; + struct iio_dev *accel; + struct iio_dev *magn; + struct regulator *vdd; + struct regulator *vdd_io; +}; + +int st_lsm9ds0_probe(struct st_lsm9ds0 *lsm9ds0, struct regmap *regmap); + +#endif /* ST_LSM9DS0_H */ diff --git a/drivers/iio/imu/st_lsm9ds0/st_lsm9ds0_core.c b/drivers/iio/imu/st_lsm9ds0/st_lsm9ds0_core.c new file mode 100644 index 000000000..ae7bc8153 --- /dev/null +++ b/drivers/iio/imu/st_lsm9ds0/st_lsm9ds0_core.c @@ -0,0 +1,150 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics LSM9DS0 IMU driver + * + * Copyright (C) 2021, Intel Corporation + * + * Author: Andy Shevchenko + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include "st_lsm9ds0.h" + +static int st_lsm9ds0_power_enable(struct device *dev, struct st_lsm9ds0 *lsm9ds0) +{ + int ret; + + /* Regulators not mandatory, but if requested we should enable them. */ + lsm9ds0->vdd = devm_regulator_get(dev, "vdd"); + if (IS_ERR(lsm9ds0->vdd)) + return dev_err_probe(dev, PTR_ERR(lsm9ds0->vdd), + "unable to get Vdd supply\n"); + + ret = regulator_enable(lsm9ds0->vdd); + if (ret) { + dev_warn(dev, "Failed to enable specified Vdd supply\n"); + return ret; + } + + lsm9ds0->vdd_io = devm_regulator_get(dev, "vddio"); + if (IS_ERR(lsm9ds0->vdd_io)) { + regulator_disable(lsm9ds0->vdd); + return dev_err_probe(dev, PTR_ERR(lsm9ds0->vdd_io), + "unable to get Vdd_IO supply\n"); + } + ret = regulator_enable(lsm9ds0->vdd_io); + if (ret) { + dev_warn(dev, "Failed to enable specified Vdd_IO supply\n"); + regulator_disable(lsm9ds0->vdd); + return ret; + } + + return 0; +} + +static void st_lsm9ds0_power_disable(void *data) +{ + struct st_lsm9ds0 *lsm9ds0 = data; + + regulator_disable(lsm9ds0->vdd_io); + regulator_disable(lsm9ds0->vdd); +} + +static int devm_st_lsm9ds0_power_enable(struct st_lsm9ds0 *lsm9ds0) +{ + struct device *dev = lsm9ds0->dev; + int ret; + + ret = st_lsm9ds0_power_enable(dev, lsm9ds0); + if (ret) + return ret; + + return devm_add_action_or_reset(dev, st_lsm9ds0_power_disable, lsm9ds0); +} + +static int st_lsm9ds0_probe_accel(struct st_lsm9ds0 *lsm9ds0, struct regmap *regmap) +{ + const struct st_sensor_settings *settings; + struct device *dev = lsm9ds0->dev; + struct st_sensor_data *data; + + settings = st_accel_get_settings(lsm9ds0->name); + if (!settings) { + dev_err(dev, "device name %s not recognized.\n", lsm9ds0->name); + return -ENODEV; + } + + lsm9ds0->accel = devm_iio_device_alloc(dev, sizeof(*data)); + if (!lsm9ds0->accel) + return -ENOMEM; + + lsm9ds0->accel->name = lsm9ds0->name; + + data = iio_priv(lsm9ds0->accel); + data->sensor_settings = (struct st_sensor_settings *)settings; + data->irq = lsm9ds0->irq; + data->regmap = regmap; + data->vdd = lsm9ds0->vdd; + data->vdd_io = lsm9ds0->vdd_io; + + return st_accel_common_probe(lsm9ds0->accel); +} + +static int st_lsm9ds0_probe_magn(struct st_lsm9ds0 *lsm9ds0, struct regmap *regmap) +{ + const struct st_sensor_settings *settings; + struct device *dev = lsm9ds0->dev; + struct st_sensor_data *data; + + settings = st_magn_get_settings(lsm9ds0->name); + if (!settings) { + dev_err(dev, "device name %s not recognized.\n", lsm9ds0->name); + return -ENODEV; + } + + lsm9ds0->magn = devm_iio_device_alloc(dev, sizeof(*data)); + if (!lsm9ds0->magn) + return -ENOMEM; + + lsm9ds0->magn->name = lsm9ds0->name; + + data = iio_priv(lsm9ds0->magn); + data->sensor_settings = (struct st_sensor_settings *)settings; + data->irq = lsm9ds0->irq; + data->regmap = regmap; + data->vdd = lsm9ds0->vdd; + data->vdd_io = lsm9ds0->vdd_io; + + return st_magn_common_probe(lsm9ds0->magn); +} + +int st_lsm9ds0_probe(struct st_lsm9ds0 *lsm9ds0, struct regmap *regmap) +{ + int ret; + + ret = devm_st_lsm9ds0_power_enable(lsm9ds0); + if (ret) + return ret; + + /* Setup accelerometer device */ + ret = st_lsm9ds0_probe_accel(lsm9ds0, regmap); + if (ret) + return ret; + + /* Setup magnetometer device */ + return st_lsm9ds0_probe_magn(lsm9ds0, regmap); +} +EXPORT_SYMBOL_NS_GPL(st_lsm9ds0_probe, IIO_ST_SENSORS); + +MODULE_AUTHOR("Andy Shevchenko "); +MODULE_DESCRIPTION("STMicroelectronics LSM9DS0 IMU core driver"); +MODULE_LICENSE("GPL v2"); +MODULE_IMPORT_NS(IIO_ST_SENSORS); diff --git a/drivers/iio/imu/st_lsm9ds0/st_lsm9ds0_i2c.c b/drivers/iio/imu/st_lsm9ds0/st_lsm9ds0_i2c.c new file mode 100644 index 000000000..a90138d8b --- /dev/null +++ b/drivers/iio/imu/st_lsm9ds0/st_lsm9ds0_i2c.c @@ -0,0 +1,80 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics LSM9DS0 IMU driver + * + * Copyright (C) 2021, Intel Corporation + * + * Author: Andy Shevchenko + */ + +#include +#include +#include +#include +#include + +#include + +#include "st_lsm9ds0.h" + +static const struct of_device_id st_lsm9ds0_of_match[] = { + { + .compatible = "st,lsm9ds0-imu", + .data = LSM9DS0_IMU_DEV_NAME, + }, + {} +}; +MODULE_DEVICE_TABLE(of, st_lsm9ds0_of_match); + +static const struct i2c_device_id st_lsm9ds0_id_table[] = { + { LSM9DS0_IMU_DEV_NAME }, + {} +}; +MODULE_DEVICE_TABLE(i2c, st_lsm9ds0_id_table); + +static const struct regmap_config st_lsm9ds0_regmap_config = { + .reg_bits = 8, + .val_bits = 8, + .read_flag_mask = 0x80, +}; + +static int st_lsm9ds0_i2c_probe(struct i2c_client *client) +{ + const struct regmap_config *config = &st_lsm9ds0_regmap_config; + struct device *dev = &client->dev; + struct st_lsm9ds0 *lsm9ds0; + struct regmap *regmap; + + st_sensors_dev_name_probe(dev, client->name, sizeof(client->name)); + + lsm9ds0 = devm_kzalloc(dev, sizeof(*lsm9ds0), GFP_KERNEL); + if (!lsm9ds0) + return -ENOMEM; + + lsm9ds0->dev = dev; + lsm9ds0->name = client->name; + lsm9ds0->irq = client->irq; + + regmap = devm_regmap_init_i2c(client, config); + if (IS_ERR(regmap)) + return PTR_ERR(regmap); + + i2c_set_clientdata(client, lsm9ds0); + + return st_lsm9ds0_probe(lsm9ds0, regmap); +} + +static struct i2c_driver st_lsm9ds0_driver = { + .driver = { + .name = "st-lsm9ds0-i2c", + .of_match_table = st_lsm9ds0_of_match, + }, + .probe_new = st_lsm9ds0_i2c_probe, + .id_table = st_lsm9ds0_id_table, +}; +module_i2c_driver(st_lsm9ds0_driver); + +MODULE_AUTHOR("Andy Shevchenko "); +MODULE_DESCRIPTION("STMicroelectronics LSM9DS0 IMU I2C driver"); +MODULE_LICENSE("GPL v2"); +MODULE_IMPORT_NS(IIO_ST_SENSORS); diff --git a/drivers/iio/imu/st_lsm9ds0/st_lsm9ds0_spi.c b/drivers/iio/imu/st_lsm9ds0/st_lsm9ds0_spi.c new file mode 100644 index 000000000..b743bf354 --- /dev/null +++ b/drivers/iio/imu/st_lsm9ds0/st_lsm9ds0_spi.c @@ -0,0 +1,79 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics LSM9DS0 IMU driver + * + * Copyright (C) 2021, Intel Corporation + * + * Author: Andy Shevchenko + */ + +#include +#include +#include +#include +#include + +#include + +#include "st_lsm9ds0.h" + +static const struct of_device_id st_lsm9ds0_of_match[] = { + { + .compatible = "st,lsm9ds0-imu", + .data = LSM9DS0_IMU_DEV_NAME, + }, + {} +}; +MODULE_DEVICE_TABLE(of, st_lsm9ds0_of_match); + +static const struct spi_device_id st_lsm9ds0_id_table[] = { + { LSM9DS0_IMU_DEV_NAME }, + {} +}; +MODULE_DEVICE_TABLE(spi, st_lsm9ds0_id_table); + +static const struct regmap_config st_lsm9ds0_regmap_config = { + .reg_bits = 8, + .val_bits = 8, + .read_flag_mask = 0xc0, +}; + +static int st_lsm9ds0_spi_probe(struct spi_device *spi) +{ + struct device *dev = &spi->dev; + struct st_lsm9ds0 *lsm9ds0; + struct regmap *regmap; + + st_sensors_dev_name_probe(dev, spi->modalias, sizeof(spi->modalias)); + + lsm9ds0 = devm_kzalloc(dev, sizeof(*lsm9ds0), GFP_KERNEL); + if (!lsm9ds0) + return -ENOMEM; + + lsm9ds0->dev = dev; + lsm9ds0->name = spi->modalias; + lsm9ds0->irq = spi->irq; + + regmap = devm_regmap_init_spi(spi, &st_lsm9ds0_regmap_config); + if (IS_ERR(regmap)) + return PTR_ERR(regmap); + + spi_set_drvdata(spi, lsm9ds0); + + return st_lsm9ds0_probe(lsm9ds0, regmap); +} + +static struct spi_driver st_lsm9ds0_driver = { + .driver = { + .name = "st-lsm9ds0-spi", + .of_match_table = st_lsm9ds0_of_match, + }, + .probe = st_lsm9ds0_spi_probe, + .id_table = st_lsm9ds0_id_table, +}; +module_spi_driver(st_lsm9ds0_driver); + +MODULE_AUTHOR("Andy Shevchenko "); +MODULE_DESCRIPTION("STMicroelectronics LSM9DS0 IMU SPI driver"); +MODULE_LICENSE("GPL v2"); +MODULE_IMPORT_NS(IIO_ST_SENSORS); -- cgit v1.2.3