diff options
Diffstat (limited to 'drivers/iio/imu')
24 files changed, 9564 insertions, 0 deletions
diff --git a/drivers/iio/imu/Kconfig b/drivers/iio/imu/Kconfig new file mode 100644 index 00000000000..124aa491101 --- /dev/null +++ b/drivers/iio/imu/Kconfig @@ -0,0 +1,41 @@ +# +# IIO imu drivers configuration +# +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 ADIS16480 + tristate "Analog Devices ADIS16480 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 ADIS16375, ADIS16480, + ADIS16485, ADIS16488 inertial sensors. + +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. + +source "drivers/iio/imu/inv_mpu6050/Kconfig" +source "drivers/iio/imu/st_lsm6ds3/Kconfig" diff --git a/drivers/iio/imu/Makefile b/drivers/iio/imu/Makefile new file mode 100644 index 00000000000..ebbe6001a32 --- /dev/null +++ b/drivers/iio/imu/Makefile @@ -0,0 +1,16 @@ +# +# Makefile for Inertial Measurement Units +# + +adis16400-y := adis16400_core.o +adis16400-$(CONFIG_IIO_BUFFER) += adis16400_buffer.o +obj-$(CONFIG_ADIS16400) += adis16400.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 += inv_mpu6050/ +obj-y += st_lsm6ds3/ diff --git a/drivers/iio/imu/adis.c b/drivers/iio/imu/adis.c new file mode 100644 index 00000000000..911255d41c1 --- /dev/null +++ b/drivers/iio/imu/adis.c @@ -0,0 +1,440 @@ +/* + * Common library for ADIS16XXX devices + * + * Copyright 2012 Analog Devices Inc. + * Author: Lars-Peter Clausen <lars@metafoo.de> + * + * Licensed under the GPL-2 or later. + */ + +#include <linux/delay.h> +#include <linux/mutex.h> +#include <linux/device.h> +#include <linux/kernel.h> +#include <linux/spi/spi.h> +#include <linux/slab.h> +#include <linux/sysfs.h> +#include <linux/module.h> +#include <asm/unaligned.h> + +#include <linux/iio/iio.h> +#include <linux/iio/sysfs.h> +#include <linux/iio/buffer.h> +#include <linux/iio/imu/adis.h> + +#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) + +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_usecs = adis->data->write_delay, + }, { + .tx_buf = adis->tx + 2, + .bits_per_word = 8, + .len = 2, + .cs_change = 1, + .delay_usecs = adis->data->write_delay, + }, { + .tx_buf = adis->tx + 4, + .bits_per_word = 8, + .len = 2, + .cs_change = 1, + .delay_usecs = adis->data->write_delay, + }, { + .tx_buf = adis->tx + 6, + .bits_per_word = 8, + .len = 2, + .delay_usecs = adis->data->write_delay, + }, { + .tx_buf = adis->tx + 8, + .bits_per_word = 8, + .len = 2, + .delay_usecs = adis->data->write_delay, + }, + }; + + mutex_lock(&adis->txrx_lock); + + 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; + case 2: + adis->tx[4] = ADIS_WRITE_REG(reg + 1); + adis->tx[5] = (value >> 8) & 0xff; + case 1: + adis->tx[2] = ADIS_WRITE_REG(reg); + adis->tx[3] = value & 0xff; + break; + default: + ret = -EINVAL; + goto out_unlock; + } + + 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; + } + +out_unlock: + mutex_unlock(&adis->txrx_lock); + + return ret; +} +EXPORT_SYMBOL_GPL(adis_write_reg); + +/** + * adis_read_reg() - read 2 bytes from a 16-bit register + * @adis: The adis device + * @reg: The address of the lower of the two registers + * @val: The value read back from the device + */ +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_usecs = adis->data->write_delay, + }, { + .tx_buf = adis->tx + 2, + .bits_per_word = 8, + .len = 2, + .cs_change = 1, + .delay_usecs = adis->data->read_delay, + }, { + .tx_buf = adis->tx + 4, + .rx_buf = adis->rx, + .bits_per_word = 8, + .len = 2, + .cs_change = 1, + .delay_usecs = adis->data->read_delay, + }, { + .rx_buf = adis->rx + 2, + .bits_per_word = 8, + .len = 2, + .delay_usecs = adis->data->read_delay, + }, + }; + + mutex_lock(&adis->txrx_lock); + 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); + 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: + ret = -EINVAL; + goto out_unlock; + } + + ret = spi_sync(adis->spi, &msg); + if (ret) { + dev_err(&adis->spi->dev, "Failed to read register 0x%02X: %d\n", + reg, ret); + goto out_unlock; + } else { + 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; + } + +out_unlock: + mutex_unlock(&adis->txrx_lock); + + return ret; +} +EXPORT_SYMBOL_GPL(adis_read_reg); + +#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) { + uint16_t val16; + int ret; + + ret = adis_read_reg_16(adis, reg, &val16); + *readval = val16; + + return ret; + } else { + return adis_write_reg_16(adis, reg, writeval); + } +} +EXPORT_SYMBOL(adis_debugfs_reg_access); + +#endif + +/** + * adis_enable_irq() - Enable or disable data ready IRQ + * @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 = 0; + uint16_t msc; + + if (adis->data->enable_irq) + return adis->data->enable_irq(adis, enable); + + ret = adis_read_reg_16(adis, adis->data->msc_ctrl_reg, &msc); + if (ret) + goto error_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; + + ret = adis_write_reg_16(adis, adis->data->msc_ctrl_reg, msc); + +error_ret: + return ret; +} +EXPORT_SYMBOL(adis_enable_irq); + +/** + * adis_check_status() - Check the device for error conditions + * @adis: The adis device + * + * Returns 0 on success, a negative error code otherwise + */ +int adis_check_status(struct adis *adis) +{ + uint16_t status; + int ret; + int i; + + ret = adis_read_reg_16(adis, adis->data->diag_stat_reg, &status); + if (ret < 0) + 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_GPL(adis_check_status); + +/** + * adis_reset() - Reset the device + * @adis: The adis device + * + * Returns 0 on success, a negative error code otherwise + */ +int adis_reset(struct adis *adis) +{ + int ret; + + 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; +} +EXPORT_SYMBOL_GPL(adis_reset); + +static int adis_self_test(struct adis *adis) +{ + int ret; + + ret = adis_write_reg_16(adis, adis->data->msc_ctrl_reg, + adis->data->self_test_mask); + if (ret) { + dev_err(&adis->spi->dev, "Failed to initiate self test: %d\n", + ret); + return ret; + } + + msleep(adis->data->startup_delay); + + return adis_check_status(adis); +} + +/** + * adis_inital_startup() - Performs device self-test + * @adis: The adis device + * + * 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) +{ + int ret; + + ret = adis_self_test(adis); + if (ret) { + dev_err(&adis->spi->dev, "Self-test failed, trying reset.\n"); + adis_reset(adis); + msleep(adis->data->startup_delay); + ret = adis_self_test(adis); + if (ret) { + dev_err(&adis->spi->dev, "Second self-test failed, giving up.\n"); + return ret; + } + } + + return 0; +} +EXPORT_SYMBOL_GPL(adis_initial_startup); + +/** + * 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(&indio_dev->mlock); + + 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(&indio_dev->mlock); + return ret; +} +EXPORT_SYMBOL_GPL(adis_single_conversion); + +/** + * 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) +{ + mutex_init(&adis->txrx_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 adis_enable_irq(adis, false); +} +EXPORT_SYMBOL_GPL(adis_init); + +MODULE_LICENSE("GPL"); +MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>"); +MODULE_DESCRIPTION("Common library code for ADIS16XXX devices"); diff --git a/drivers/iio/imu/adis16400.h b/drivers/iio/imu/adis16400.h new file mode 100644 index 00000000000..2f8f9d63238 --- /dev/null +++ b/drivers/iio/imu/adis16400.h @@ -0,0 +1,212 @@ +/* + * adis16400.h support Analog Devices ADIS16400 + * 3d 18g accelerometers, + * 3d gyroscopes, + * 3d 2.5gauss magnetometers via SPI + * + * Copyright (c) 2009 Manuel Stahl <manuel.stahl@iis.fraunhofer.de> + * Copyright (c) 2007 Jonathan Cameron <jic23@kernel.org> + * + * Loosely based upon lis3l02dq.h + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + */ + +#ifndef SPI_ADIS16400_H_ +#define SPI_ADIS16400_H_ + +#include <linux/iio/imu/adis.h> + +#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) + +struct adis16400_state; + +struct adis16400_chip_info { + const struct iio_chan_spec *channels; + const int num_channels; + const long flags; + unsigned int gyro_scale_micro; + unsigned int accel_scale_micro; + int temp_scale_nano; + int temp_offset; + 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 + **/ +struct adis16400_state { + struct adis16400_chip_info *variant; + int filt_int; + + struct adis adis; +}; + +/* 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, +}; + +#ifdef CONFIG_IIO_BUFFER + +ssize_t adis16400_read_data_from_ring(struct device *dev, + struct device_attribute *attr, + char *buf); + + +int adis16400_update_scan_mode(struct iio_dev *indio_dev, + const unsigned long *scan_mask); +irqreturn_t adis16400_trigger_handler(int irq, void *p); + +#else /* CONFIG_IIO_BUFFER */ + +#define adis16400_update_scan_mode NULL +#define adis16400_trigger_handler NULL + +#endif /* CONFIG_IIO_BUFFER */ + +#endif /* SPI_ADIS16400_H_ */ diff --git a/drivers/iio/imu/adis16400_buffer.c b/drivers/iio/imu/adis16400_buffer.c new file mode 100644 index 00000000000..054c01d6e73 --- /dev/null +++ b/drivers/iio/imu/adis16400_buffer.c @@ -0,0 +1,96 @@ +#include <linux/interrupt.h> +#include <linux/mutex.h> +#include <linux/kernel.h> +#include <linux/spi/spi.h> +#include <linux/slab.h> +#include <linux/bitops.h> +#include <linux/export.h> + +#include <linux/iio/iio.h> +#include <linux/iio/buffer.h> +#include <linux/iio/triggered_buffer.h> +#include <linux/iio/trigger_consumer.h> + +#include "adis16400.h" + +int adis16400_update_scan_mode(struct iio_dev *indio_dev, + const unsigned long *scan_mask) +{ + struct adis16400_state *st = iio_priv(indio_dev); + struct adis *adis = &st->adis; + uint16_t *tx, *rx; + + if (st->variant->flags & ADIS16400_NO_BURST) + return adis_update_scan_mode(indio_dev, scan_mask); + + kfree(adis->xfer); + kfree(adis->buffer); + + adis->xfer = kcalloc(2, sizeof(*adis->xfer), GFP_KERNEL); + if (!adis->xfer) + return -ENOMEM; + + adis->buffer = kzalloc(indio_dev->scan_bytes + sizeof(u16), + GFP_KERNEL); + if (!adis->buffer) + return -ENOMEM; + + rx = adis->buffer; + tx = adis->buffer + indio_dev->scan_bytes; + + tx[0] = ADIS_READ_REG(ADIS16400_GLOB_CMD); + tx[1] = 0; + + adis->xfer[0].tx_buf = tx; + adis->xfer[0].bits_per_word = 8; + adis->xfer[0].len = 2; + adis->xfer[1].tx_buf = tx; + adis->xfer[1].bits_per_word = 8; + adis->xfer[1].len = indio_dev->scan_bytes; + + 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; +} + +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; + u32 old_speed_hz = st->adis.spi->max_speed_hz; + int ret; + + if (!adis->buffer) + return -ENOMEM; + + if (!(st->variant->flags & ADIS16400_NO_BURST) && + st->adis.spi->max_speed_hz > ADIS16400_SPI_BURST) { + st->adis.spi->max_speed_hz = ADIS16400_SPI_BURST; + spi_setup(st->adis.spi); + } + + 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_NO_BURST)) { + st->adis.spi->max_speed_hz = old_speed_hz; + spi_setup(st->adis.spi); + } + + /* Guaranteed to be aligned with 8 byte boundary */ + if (indio_dev->scan_timestamp) { + void *b = adis->buffer + indio_dev->scan_bytes - sizeof(s64); + *(s64 *)b = pf->timestamp; + } + + iio_push_to_buffers(indio_dev, adis->buffer); + + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; +} diff --git a/drivers/iio/imu/adis16400_core.c b/drivers/iio/imu/adis16400_core.c new file mode 100644 index 00000000000..f60591f0b92 --- /dev/null +++ b/drivers/iio/imu/adis16400_core.c @@ -0,0 +1,966 @@ +/* + * adis16400.c support Analog Devices ADIS16400/5 + * 3d 2g Linear Accelerometers, + * 3d Gyroscopes, + * 3d Magnetometers via SPI + * + * Copyright (c) 2009 Manuel Stahl <manuel.stahl@iis.fraunhofer.de> + * Copyright (c) 2007 Jonathan Cameron <jic23@kernel.org> + * Copyright (c) 2011 Analog Devices Inc. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + */ + +#include <linux/interrupt.h> +#include <linux/irq.h> +#include <linux/delay.h> +#include <linux/mutex.h> +#include <linux/device.h> +#include <linux/kernel.h> +#include <linux/spi/spi.h> +#include <linux/slab.h> +#include <linux/sysfs.h> +#include <linux/list.h> +#include <linux/module.h> +#include <linux/debugfs.h> + +#include <linux/iio/iio.h> +#include <linux/iio/sysfs.h> +#include <linux/iio/buffer.h> + +#include "adis16400.h" + +#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 < 0) + return ret; + + ret = adis_read_reg_16(&st->adis, ADIS16334_LOT_ID2, &lot2); + if (ret < 0) + return ret; + + ret = adis_read_reg_16(&st->adis, ADIS16334_SERIAL_NUMBER, + &serial_number); + if (ret < 0) + 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 < 0) + return ret; + + *val = prod_id; + + return 0; +} +DEFINE_SIMPLE_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 < 0) + return ret; + + *val = flash_count; + + return 0; +} +DEFINE_SIMPLE_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); + + if (st->variant->flags & ADIS16400_HAS_SERIAL_NUMBER) + debugfs_create_file("serial_number", 0400, + indio_dev->debugfs_dentry, st, + &adis16400_serial_number_fops); + if (st->variant->flags & ADIS16400_HAS_PROD_ID) + debugfs_create_file("product_id", 0400, + indio_dev->debugfs_dentry, st, + &adis16400_product_id_fops); + debugfs_create_file("flash_count", 0400, indio_dev->debugfs_dentry, + 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, + ADIS16400, + 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 < 0) + 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 < 0) + 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 ssize_t adis16400_read_frequency(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *indio_dev = dev_to_iio_dev(dev); + struct adis16400_state *st = iio_priv(indio_dev); + int ret; + + ret = st->variant->get_freq(st); + if (ret < 0) + return ret; + + return sprintf(buf, "%d.%.3d\n", ret / 1000, ret % 1000); +} + +static const unsigned 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 < 0) + return ret; + + ret = adis_write_reg_16(&st->adis, ADIS16400_SENS_AVG, + (val16 & ~0x07) | i); + return ret; +} + +static ssize_t adis16400_write_frequency(struct device *dev, + struct device_attribute *attr, const char *buf, size_t len) +{ + struct iio_dev *indio_dev = dev_to_iio_dev(dev); + struct adis16400_state *st = iio_priv(indio_dev); + int i, f, val; + int ret; + + ret = iio_str_to_fixpoint(buf, 100, &i, &f); + if (ret) + return ret; + + val = i * 1000 + f; + + if (val <= 0) + return -EINVAL; + + mutex_lock(&indio_dev->mlock); + st->variant->set_freq(st, val); + mutex_unlock(&indio_dev->mlock); + + return ret ? ret : len; +} + +/* 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; + + sscanf(indio_dev->name, "adis%u\n", &device_id); + + 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 IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, + adis16400_read_frequency, + adis16400_write_frequency); + +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: + mutex_lock(&indio_dev->mlock); + ret = adis_write_reg_16(&st->adis, + adis16400_addresses[chan->scan_index], val); + mutex_unlock(&indio_dev->mlock); + return ret; + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: + /* + * Need to cache values so we can update if the frequency + * changes. + */ + mutex_lock(&indio_dev->mlock); + st->filt_int = val; + /* Work out update to current value */ + sps = st->variant->get_freq(st); + if (sps < 0) { + mutex_unlock(&indio_dev->mlock); + return sps; + } + + ret = adis16400_set_filter(indio_dev, sps, + val * 1000 + val2 / 1000); + mutex_unlock(&indio_dev->mlock); + 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; + default: + return -EINVAL; + } + case IIO_CHAN_INFO_CALIBBIAS: + mutex_lock(&indio_dev->mlock); + ret = adis_read_reg_16(&st->adis, + adis16400_addresses[chan->scan_index], &val16); + mutex_unlock(&indio_dev->mlock); + if (ret) + return ret; + val16 = ((val16 & 0xFFF) << 4) >> 4; + *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: + mutex_lock(&indio_dev->mlock); + /* Need both the number of taps and the sampling frequency */ + ret = adis_read_reg_16(&st->adis, + ADIS16400_SENS_AVG, + &val16); + if (ret < 0) { + mutex_unlock(&indio_dev->mlock); + return ret; + } + ret = st->variant->get_freq(st); + if (ret >= 0) { + ret /= adis16400_3db_divisors[val16 & 0x07]; + *val = ret / 1000; + *val2 = (ret % 1000) * 1000; + } + mutex_unlock(&indio_dev->mlock); + if (ret < 0) + return ret; + return IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } +} + +#define ADIS16400_VOLTAGE_CHAN(addr, bits, name, si) { \ + .type = IIO_VOLTAGE, \ + .indexed = 1, \ + .channel = 0, \ + .extend_name = name, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .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) + +#define ADIS16400_AUX_ADC_CHAN(addr, bits) \ + ADIS16400_VOLTAGE_CHAN(addr, bits, NULL, ADIS16400_SCAN_ADC) + +#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), \ + .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), \ + .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), \ + .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), \ + .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), \ + .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), \ + .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(12) +}; + +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), + .address = ADIS16448_BARO_OUT, + .scan_index = ADIS16400_SCAN_BARO, + .scan_type = IIO_ST('s', 16, 16, 0), + }, + ADIS16400_TEMP_CHAN(ADIS16448_TEMP_OUT, 12), + IIO_CHAN_SOFT_TIMESTAMP(11) +}; + +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(11) +}; + +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(14) +}; + +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(8) +}; + +static struct attribute *adis16400_attributes[] = { + &iio_dev_attr_sampling_frequency.dev_attr.attr, + NULL +}; + +static const struct attribute_group adis16400_attribute_group = { + .attrs = adis16400_attributes, +}; + +static struct adis16400_chip_info adis16400_chips[] = { + [ADIS16300] = { + .channels = adis16300_channels, + .num_channels = ARRAY_SIZE(adis16300_channels), + .flags = ADIS16400_HAS_SLOW_MODE, + .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, + }, + [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, + }, + [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, + }, + [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, + }, + [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, + }, + [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, + }, + [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, + }, + [ADIS16448] = { + .channels = adis16448_channels, + .num_channels = ARRAY_SIZE(adis16448_channels), + .flags = ADIS16400_HAS_PROD_ID | + ADIS16400_HAS_SERIAL_NUMBER, + .gyro_scale_micro = IIO_DEGREE_TO_RAD(10000), /* 0.01 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, + } +}; + +static const struct iio_info adis16400_info = { + .driver_module = THIS_MODULE, + .read_raw = &adis16400_read_raw, + .write_raw = &adis16400_write_raw, + .attrs = &adis16400_attribute_group, + .update_scan_mode = adis16400_update_scan_mode, + .debugfs_reg_access = adis_debugfs_reg_access, +}; + +static const unsigned long adis16400_burst_scan_mask[] = { + ~0UL, + 0, +}; + +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", +}; + +static const struct adis_data adis16400_data = { + .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, + .startup_delay = ADIS16400_STARTUP_DELAY, + + .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), +}; + +static int adis16400_probe(struct spi_device *spi) +{ + struct adis16400_state *st; + struct iio_dev *indio_dev; + int ret; + + indio_dev = iio_device_alloc(sizeof(*st)); + if (indio_dev == NULL) + return -ENOMEM; + + st = iio_priv(indio_dev); + /* this is only used for removal purposes */ + spi_set_drvdata(spi, indio_dev); + + /* setup the industrialio driver allocated elements */ + st->variant = &adis16400_chips[spi_get_device_id(spi)->driver_data]; + indio_dev->dev.parent = &spi->dev; + 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)) + indio_dev->available_scan_masks = adis16400_burst_scan_mask; + + ret = adis_init(&st->adis, indio_dev, spi, &adis16400_data); + if (ret) + goto error_free_dev; + + ret = adis_setup_buffer_and_trigger(&st->adis, indio_dev, + adis16400_trigger_handler); + if (ret) + goto error_free_dev; + + /* Get the device into a sane initial state */ + ret = adis16400_initial_setup(indio_dev); + if (ret) + goto error_cleanup_buffer; + ret = iio_device_register(indio_dev); + if (ret) + goto error_cleanup_buffer; + + adis16400_debugfs_init(indio_dev); + return 0; + +error_cleanup_buffer: + adis_cleanup_buffer_and_trigger(&st->adis, indio_dev); +error_free_dev: + iio_device_free(indio_dev); + return ret; +} + +static int adis16400_remove(struct spi_device *spi) +{ + struct iio_dev *indio_dev = spi_get_drvdata(spi); + struct adis16400_state *st = iio_priv(indio_dev); + + iio_device_unregister(indio_dev); + adis16400_stop_device(indio_dev); + + adis_cleanup_buffer_and_trigger(&st->adis, indio_dev); + + iio_device_free(indio_dev); + + return 0; +} + +static const struct spi_device_id adis16400_id[] = { + {"adis16300", ADIS16300}, + {"adis16334", ADIS16334}, + {"adis16350", ADIS16350}, + {"adis16354", ADIS16350}, + {"adis16355", ADIS16350}, + {"adis16360", ADIS16360}, + {"adis16362", ADIS16362}, + {"adis16364", ADIS16364}, + {"adis16365", ADIS16360}, + {"adis16400", ADIS16400}, + {"adis16405", ADIS16400}, + {"adis16448", ADIS16448}, + {} +}; +MODULE_DEVICE_TABLE(spi, adis16400_id); + +static struct spi_driver adis16400_driver = { + .driver = { + .name = "adis16400", + .owner = THIS_MODULE, + }, + .id_table = adis16400_id, + .probe = adis16400_probe, + .remove = adis16400_remove, +}; +module_spi_driver(adis16400_driver); + +MODULE_AUTHOR("Manuel Stahl <manuel.stahl@iis.fraunhofer.de>"); +MODULE_DESCRIPTION("Analog Devices ADIS16400/5 IMU SPI driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/imu/adis16480.c b/drivers/iio/imu/adis16480.c new file mode 100644 index 00000000000..b7db3837629 --- /dev/null +++ b/drivers/iio/imu/adis16480.c @@ -0,0 +1,924 @@ +/* + * ADIS16480 and similar IMUs driver + * + * Copyright 2012 Analog Devices Inc. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + */ + +#include <linux/interrupt.h> +#include <linux/delay.h> +#include <linux/mutex.h> +#include <linux/device.h> +#include <linux/kernel.h> +#include <linux/spi/spi.h> +#include <linux/slab.h> +#include <linux/sysfs.h> +#include <linux/module.h> + +#include <linux/iio/iio.h> +#include <linux/iio/sysfs.h> +#include <linux/iio/buffer.h> +#include <linux/iio/imu/adis.h> + +#include <linux/debugfs.h> + +#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) + +#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)) + +struct adis16480_chip_info { + unsigned int num_channels; + const struct iio_chan_spec *channels; +}; + +struct adis16480 { + const struct adis16480_chip_info *chip_info; + + struct adis adis; +}; + +#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 < 0) + 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 < 0) + return ret; + + ret = adis_read_reg_16(&adis16480->adis, ADIS16480_REG_FIRM_DM, &md); + if (ret < 0) + 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 < 0) + return ret; + + *val = serial; + + return 0; +} +DEFINE_SIMPLE_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 < 0) + return ret; + + *val = prod_id; + + return 0; +} +DEFINE_SIMPLE_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 < 0) + return ret; + + *val = flash_count; + + return 0; +} +DEFINE_SIMPLE_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); + + debugfs_create_file("firmware_revision", 0400, + indio_dev->debugfs_dentry, adis16480, + &adis16480_firmware_revision_fops); + debugfs_create_file("firmware_date", 0400, indio_dev->debugfs_dentry, + adis16480, &adis16480_firmware_date_fops); + debugfs_create_file("serial_number", 0400, indio_dev->debugfs_dentry, + adis16480, &adis16480_serial_number_fops); + debugfs_create_file("product_id", 0400, indio_dev->debugfs_dentry, + adis16480, &adis16480_product_id_fops); + debugfs_create_file("flash_count", 0400, indio_dev->debugfs_dentry, + 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 adis16480 *st, unsigned int freq) +{ + unsigned int t; + + t = 2460000 / freq; + if (t > 2048) + t = 2048; + + if (t != 0) + t--; + + return adis_write_reg_16(&st->adis, ADIS16480_REG_DEC_RATE, t); +} + +static int adis16480_get_freq(struct adis16480 *st, unsigned int *freq) +{ + uint16_t t; + int ret; + + ret = adis_read_reg_16(&st->adis, ADIS16480_REG_DEC_RATE, &t); + if (ret < 0) + return ret; + + *freq = 2460000 / (t + 1); + + return 0; +} + +static ssize_t adis16480_read_frequency(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *indio_dev = dev_to_iio_dev(dev); + struct adis16480 *st = iio_priv(indio_dev); + unsigned int freq; + int ret; + + ret = adis16480_get_freq(st, &freq); + if (ret < 0) + return ret; + + return sprintf(buf, "%d.%.3d\n", freq / 1000, freq % 1000); +} + +static ssize_t adis16480_write_frequency(struct device *dev, + struct device_attribute *attr, + const char *buf, + size_t len) +{ + struct iio_dev *indio_dev = dev_to_iio_dev(dev); + struct adis16480 *st = iio_priv(indio_dev); + int freq_int, freq_fract; + long val; + int ret; + + ret = iio_str_to_fixpoint(buf, 100, &freq_int, &freq_fract); + if (ret) + return ret; + + val = freq_int * 1000 + freq_fract; + + if (val <= 0) + return -EINVAL; + + ret = adis16480_set_freq(st, val); + + return ret ? ret : len; +} + +static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, + adis16480_read_frequency, + adis16480_write_frequency); + +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); + *bias = sign_extend32(val16, 15); + break; + case IIO_ANGL_VEL: + case IIO_ACCEL: + ret = adis_read_reg_32(&st->adis, reg, &val32); + *bias = sign_extend32(val32, 31); + break; + default: + ret = -EINVAL; + } + + if (ret < 0) + 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 < 0) + 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 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 < 0) + return ret; + + if (!(val & enable_mask)) + *freq = 0; + else + *freq = adis16480_def_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); + + ret = adis_read_reg_16(&st->adis, reg, &val); + if (ret < 0) + return ret; + + if (freq == 0) { + val &= ~enable_mask; + } else { + best_freq = 0; + best_diff = 310; + for (i = 0; i < ARRAY_SIZE(adis16480_def_filter_freqs); i++) { + if (adis16480_def_filter_freqs[i] >= freq) { + diff = adis16480_def_filter_freqs[i] - freq; + if (diff < best_diff) { + best_diff = diff; + best_freq = i; + } + } + } + + val &= ~(0x3 << offset); + val |= best_freq << offset; + val |= enable_mask; + } + + return adis_write_reg_16(&st->adis, reg, val); +} + +static int adis16480_read_raw(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, int *val, int *val2, long info) +{ + 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 = IIO_DEGREE_TO_RAD(20000); /* 0.02 degree/sec */ + return IIO_VAL_INT_PLUS_MICRO; + case IIO_ACCEL: + *val = 0; + *val2 = IIO_G_TO_M_S_2(800); /* 0.8 mg */ + return IIO_VAL_INT_PLUS_MICRO; + case IIO_MAGN: + *val = 0; + *val2 = 100; /* 0.0001 gauss */ + return IIO_VAL_INT_PLUS_MICRO; + case IIO_TEMP: + *val = 5; + *val2 = 650000; /* 5.65 milli degree Celsius */ + return IIO_VAL_INT_PLUS_MICRO; + case IIO_PRESSURE: + *val = 0; + *val2 = 4000; /* 40ubar = 0.004 kPa */ + return IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } + case IIO_CHAN_INFO_OFFSET: + /* Only the temperature channel has a offset */ + *val = 4425; /* 25 degree Celsius = 0x0000 */ + 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); + 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); + 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), \ + .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), \ + .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), \ + .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, +}; + +static const struct adis16480_chip_info adis16480_chip_info[] = { + [ADIS16375] = { + .channels = adis16485_channels, + .num_channels = ARRAY_SIZE(adis16485_channels), + }, + [ADIS16480] = { + .channels = adis16480_channels, + .num_channels = ARRAY_SIZE(adis16480_channels), + }, + [ADIS16485] = { + .channels = adis16485_channels, + .num_channels = ARRAY_SIZE(adis16485_channels), + }, + [ADIS16488] = { + .channels = adis16480_channels, + .num_channels = ARRAY_SIZE(adis16480_channels), + }, +}; + +static struct attribute *adis16480_attributes[] = { + &iio_dev_attr_sampling_frequency.dev_attr.attr, + NULL +}; + +static const struct attribute_group adis16480_attribute_group = { + .attrs = adis16480_attributes, +}; + +static const struct iio_info adis16480_info = { + .attrs = &adis16480_attribute_group, + .read_raw = &adis16480_read_raw, + .write_raw = &adis16480_write_raw, + .update_scan_mode = adis_update_scan_mode, + .driver_module = THIS_MODULE, +}; + +static int adis16480_stop_device(struct iio_dev *indio_dev) +{ + struct adis16480 *st = iio_priv(indio_dev); + int ret; + + ret = adis_write_reg_16(&st->adis, ADIS16480_REG_SLP_CNT, BIT(9)); + if (ret) + dev_err(&indio_dev->dev, + "Could not power down device: %d\n", ret); + + return ret; +} + +static int adis16480_enable_irq(struct adis *adis, bool enable) +{ + return adis_write_reg_16(adis, ADIS16480_REG_FNCTIO_CTRL, + enable ? BIT(3) : 0); +} + +static int adis16480_initial_setup(struct iio_dev *indio_dev) +{ + struct adis16480 *st = iio_priv(indio_dev); + uint16_t prod_id; + unsigned int device_id; + int ret; + + adis_reset(&st->adis); + msleep(70); + + ret = adis_write_reg_16(&st->adis, ADIS16480_REG_GLOB_CMD, BIT(1)); + if (ret) + return ret; + msleep(30); + + ret = adis_check_status(&st->adis); + if (ret) + return ret; + + ret = adis_read_reg_16(&st->adis, ADIS16480_REG_PROD_ID, &prod_id); + if (ret) + return ret; + + sscanf(indio_dev->name, "adis%u\n", &device_id); + + if (prod_id != device_id) + dev_warn(&indio_dev->dev, "Device ID(%u) and product ID(%u) do not match.", + device_id, prod_id); + + return 0; +} + +#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 const struct adis_data adis16480_data = { + .diag_stat_reg = ADIS16480_REG_DIAG_STS, + .glob_cmd_reg = ADIS16480_REG_GLOB_CMD, + .has_paging = true, + + .read_delay = 5, + .write_delay = 5, + + .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, +}; + +static int adis16480_probe(struct spi_device *spi) +{ + const struct spi_device_id *id = spi_get_device_id(spi); + struct iio_dev *indio_dev; + struct adis16480 *st; + int ret; + + indio_dev = iio_device_alloc(sizeof(*st)); + if (indio_dev == NULL) + return -ENOMEM; + + spi_set_drvdata(spi, indio_dev); + + st = iio_priv(indio_dev); + + st->chip_info = &adis16480_chip_info[id->driver_data]; + indio_dev->dev.parent = &spi->dev; + 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; + + ret = adis_init(&st->adis, indio_dev, spi, &adis16480_data); + if (ret) + goto error_free_dev; + + ret = adis_setup_buffer_and_trigger(&st->adis, indio_dev, NULL); + if (ret) + goto error_free_dev; + + ret = adis16480_initial_setup(indio_dev); + if (ret) + goto error_cleanup_buffer; + + ret = iio_device_register(indio_dev); + if (ret) + goto error_stop_device; + + adis16480_debugfs_init(indio_dev); + + return 0; + +error_stop_device: + adis16480_stop_device(indio_dev); +error_cleanup_buffer: + adis_cleanup_buffer_and_trigger(&st->adis, indio_dev); +error_free_dev: + iio_device_free(indio_dev); + return ret; +} + +static int adis16480_remove(struct spi_device *spi) +{ + struct iio_dev *indio_dev = spi_get_drvdata(spi); + struct adis16480 *st = iio_priv(indio_dev); + + iio_device_unregister(indio_dev); + adis16480_stop_device(indio_dev); + + adis_cleanup_buffer_and_trigger(&st->adis, indio_dev); + + iio_device_free(indio_dev); + + return 0; +} + +static const struct spi_device_id adis16480_ids[] = { + { "adis16375", ADIS16375 }, + { "adis16480", ADIS16480 }, + { "adis16485", ADIS16485 }, + { "adis16488", ADIS16488 }, + { } +}; +MODULE_DEVICE_TABLE(spi, adis16480_ids); + +static struct spi_driver adis16480_driver = { + .driver = { + .name = "adis16480", + .owner = THIS_MODULE, + }, + .id_table = adis16480_ids, + .probe = adis16480_probe, + .remove = adis16480_remove, +}; +module_spi_driver(adis16480_driver); + +MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>"); +MODULE_DESCRIPTION("Analog Devices ADIS16480 IMU driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/imu/adis_buffer.c b/drivers/iio/imu/adis_buffer.c new file mode 100644 index 00000000000..99d8e0b0dd3 --- /dev/null +++ b/drivers/iio/imu/adis_buffer.c @@ -0,0 +1,176 @@ +/* + * Common library for ADIS16XXX devices + * + * Copyright 2012 Analog Devices Inc. + * Author: Lars-Peter Clausen <lars@metafoo.de> + * + * Licensed under the GPL-2 or later. + */ + +#include <linux/export.h> +#include <linux/interrupt.h> +#include <linux/mutex.h> +#include <linux/kernel.h> +#include <linux/spi/spi.h> +#include <linux/slab.h> + +#include <linux/iio/iio.h> +#include <linux/iio/buffer.h> +#include <linux/iio/trigger_consumer.h> +#include <linux/iio/triggered_buffer.h> +#include <linux/iio/imu/adis.h> + +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); + + 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 = kzalloc(indio_dev->scan_bytes * 2, GFP_KERNEL); + if (!adis->buffer) + return -ENOMEM; + + rx = adis->buffer; + tx = rx + indio_dev->scan_bytes; + + 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_usecs = adis->data->read_delay; + 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_GPL(adis_update_scan_mode); + +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->buffer) + return -ENOMEM; + + if (adis->data->has_paging) { + mutex_lock(&adis->txrx_lock); + if (adis->current_page != 0) { + adis->tx[0] = ADIS_WRITE_REG(ADIS_REG_PAGE_ID); + adis->tx[1] = 0; + spi_write(adis->spi, adis->tx, 2); + } + } + + ret = spi_sync(adis->spi, &adis->msg); + if (ret) + dev_err(&adis->spi->dev, "Failed to read data: %d", ret); + + + if (adis->data->has_paging) { + adis->current_page = 0; + mutex_unlock(&adis->txrx_lock); + } + + /* Guaranteed to be aligned with 8 byte boundary */ + if (indio_dev->scan_timestamp) { + void *b = adis->buffer + indio_dev->scan_bytes - sizeof(s64); + *(s64 *)b = pf->timestamp; + } + + iio_push_to_buffers(indio_dev, adis->buffer); + + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; +} + +/** + * adis_setup_buffer_and_trigger() - Sets up buffer and trigger for the 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. + * + * adis_cleanup_buffer_and_trigger() should be called to free the resources + * allocated by this function. + */ +int adis_setup_buffer_and_trigger(struct adis *adis, struct iio_dev *indio_dev, + irqreturn_t (*trigger_handler)(int, void *)) +{ + int ret; + + if (!trigger_handler) + trigger_handler = adis_trigger_handler; + + ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time, + trigger_handler, NULL); + if (ret) + return ret; + + if (adis->spi->irq) { + ret = adis_probe_trigger(adis, indio_dev); + if (ret) + goto error_buffer_cleanup; + } + return 0; + +error_buffer_cleanup: + iio_triggered_buffer_cleanup(indio_dev); + return ret; +} +EXPORT_SYMBOL_GPL(adis_setup_buffer_and_trigger); + +/** + * adis_cleanup_buffer_and_trigger() - Free buffer and trigger resources + * @adis: The adis device. + * @indio_dev: The IIO device. + * + * Frees resources allocated by adis_setup_buffer_and_trigger() + */ +void adis_cleanup_buffer_and_trigger(struct adis *adis, + struct iio_dev *indio_dev) +{ + if (adis->spi->irq) + adis_remove_trigger(adis); + kfree(adis->buffer); + kfree(adis->xfer); + iio_triggered_buffer_cleanup(indio_dev); +} +EXPORT_SYMBOL_GPL(adis_cleanup_buffer_and_trigger); diff --git a/drivers/iio/imu/adis_trigger.c b/drivers/iio/imu/adis_trigger.c new file mode 100644 index 00000000000..e0017c22bb9 --- /dev/null +++ b/drivers/iio/imu/adis_trigger.c @@ -0,0 +1,89 @@ +/* + * Common library for ADIS16XXX devices + * + * Copyright 2012 Analog Devices Inc. + * Author: Lars-Peter Clausen <lars@metafoo.de> + * + * Licensed under the GPL-2 or later. + */ + +#include <linux/interrupt.h> +#include <linux/kernel.h> +#include <linux/spi/spi.h> +#include <linux/export.h> + +#include <linux/iio/iio.h> +#include <linux/iio/trigger.h> +#include <linux/iio/imu/adis.h> + +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 = { + .owner = THIS_MODULE, + .set_trigger_state = &adis_data_rdy_trigger_set_state, +}; + +/** + * adis_probe_trigger() - Sets up trigger for a adis device + * @adis: The adis device + * @indio_dev: The IIO device + * + * Returns 0 on success or a negative error code + * + * adis_remove_trigger() should be used to free the trigger. + */ +int adis_probe_trigger(struct adis *adis, struct iio_dev *indio_dev) +{ + int ret; + + adis->trig = iio_trigger_alloc("%s-dev%d", indio_dev->name, + indio_dev->id); + if (adis->trig == NULL) + return -ENOMEM; + + ret = request_irq(adis->spi->irq, + &iio_trigger_generic_data_rdy_poll, + IRQF_TRIGGER_RISING, + indio_dev->name, + adis->trig); + if (ret) + goto error_free_trig; + + adis->trig->dev.parent = &adis->spi->dev; + adis->trig->ops = &adis_trigger_ops; + iio_trigger_set_drvdata(adis->trig, adis); + ret = iio_trigger_register(adis->trig); + + indio_dev->trig = adis->trig; + if (ret) + goto error_free_irq; + + return 0; + +error_free_irq: + free_irq(adis->spi->irq, adis->trig); +error_free_trig: + iio_trigger_free(adis->trig); + return ret; +} +EXPORT_SYMBOL_GPL(adis_probe_trigger); + +/** + * adis_remove_trigger() - Remove trigger for a adis devices + * @adis: The adis device + * + * Removes the trigger previously registered with adis_probe_trigger(). + */ +void adis_remove_trigger(struct adis *adis) +{ + iio_trigger_unregister(adis->trig); + free_irq(adis->spi->irq, adis->trig); + iio_trigger_free(adis->trig); +} +EXPORT_SYMBOL_GPL(adis_remove_trigger); diff --git a/drivers/iio/imu/inv_mpu6050/Kconfig b/drivers/iio/imu/inv_mpu6050/Kconfig new file mode 100644 index 00000000000..361b2328453 --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/Kconfig @@ -0,0 +1,14 @@ +# +# inv-mpu6050 drivers for Invensense MPU devices and combos +# + +config INV_MPU6050_IIO + tristate "Invensense MPU6050 devices" + depends on I2C && SYSFS + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + help + This driver supports the Invensense MPU6050 devices. + It is a gyroscope/accelerometer combo device. + This driver can be built as a module. The module will be called + inv-mpu6050. diff --git a/drivers/iio/imu/inv_mpu6050/Makefile b/drivers/iio/imu/inv_mpu6050/Makefile new file mode 100644 index 00000000000..3a677c778af --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/Makefile @@ -0,0 +1,6 @@ +# +# Makefile for Invensense MPU6050 device. +# + +obj-$(CONFIG_INV_MPU6050_IIO) += inv-mpu6050.o +inv-mpu6050-objs := inv_mpu_core.o inv_mpu_ring.o inv_mpu_trigger.o 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 00000000000..fe4c61e219f --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c @@ -0,0 +1,795 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +*/ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/spinlock.h> +#include "inv_mpu_iio.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_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, +}; + +static const struct inv_mpu6050_chip_config chip_config_6050 = { + .fsr = INV_MPU6050_FSR_2000DPS, + .lpf = INV_MPU6050_FILTER_20HZ, + .fifo_rate = INV_MPU6050_INIT_FIFO_RATE, + .gyro_fifo_enable = false, + .accl_fifo_enable = false, + .accl_fs = INV_MPU6050_FS_02G, +}; + +static const struct inv_mpu6050_hw hw_info[INV_NUM_PARTS] = { + { + .num_reg = 117, + .name = "MPU6050", + .reg = ®_set_6050, + .config = &chip_config_6050, + }, +}; + +int inv_mpu6050_write_reg(struct inv_mpu6050_state *st, int reg, u8 d) +{ + return i2c_smbus_write_i2c_block_data(st->client, reg, 1, &d); +} + +int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask) +{ + u8 d, mgmt_1; + int result; + + /* switch clock needs to be careful. Only when gyro is on, can + clock source be switched to gyro. Otherwise, it must be set to + internal clock */ + if (INV_MPU6050_BIT_PWR_GYRO_STBY == mask) { + result = i2c_smbus_read_i2c_block_data(st->client, + st->reg->pwr_mgmt_1, 1, &mgmt_1); + if (result != 1) + return result; + + mgmt_1 &= ~INV_MPU6050_BIT_CLK_MASK; + } + + if ((INV_MPU6050_BIT_PWR_GYRO_STBY == mask) && (!en)) { + /* turning off gyro requires switch to internal clock first. + Then turn off gyro engine */ + mgmt_1 |= INV_CLK_INTERNAL; + result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_1, mgmt_1); + if (result) + return result; + } + + result = i2c_smbus_read_i2c_block_data(st->client, + st->reg->pwr_mgmt_2, 1, &d); + if (result != 1) + return result; + if (en) + d &= ~mask; + else + d |= mask; + result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_2, d); + if (result) + return result; + + if (en) { + /* Wait for output stablize */ + msleep(INV_MPU6050_TEMP_UP_TIME); + if (INV_MPU6050_BIT_PWR_GYRO_STBY == mask) { + /* switch internal clock to PLL */ + mgmt_1 |= INV_CLK_PLL; + result = inv_mpu6050_write_reg(st, + st->reg->pwr_mgmt_1, mgmt_1); + if (result) + return result; + } + } + + return 0; +} + +int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on) +{ + int result; + + if (power_on) + result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_1, 0); + else + result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_1, + INV_MPU6050_BIT_SLEEP); + if (result) + return result; + + if (power_on) + msleep(INV_MPU6050_REG_UP_TIME); + + return 0; +} + +/** + * 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_power_itg(st, true); + if (result) + return result; + d = (INV_MPU6050_FSR_2000DPS << INV_MPU6050_GYRO_CONFIG_FSR_SHIFT); + result = inv_mpu6050_write_reg(st, st->reg->gyro_config, d); + if (result) + return result; + + d = INV_MPU6050_FILTER_20HZ; + result = inv_mpu6050_write_reg(st, st->reg->lpf, d); + if (result) + return result; + + d = INV_MPU6050_ONE_K_HZ / INV_MPU6050_INIT_FIFO_RATE - 1; + result = inv_mpu6050_write_reg(st, st->reg->sample_rate_div, d); + if (result) + return result; + + d = (INV_MPU6050_FS_02G << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT); + result = inv_mpu6050_write_reg(st, st->reg->accl_config, d); + if (result) + return result; + + memcpy(&st->chip_config, hw_info[st->chip_type].config, + sizeof(struct inv_mpu6050_chip_config)); + result = inv_mpu6050_set_power_itg(st, false); + + return result; +} + +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 = i2c_smbus_read_i2c_block_data(st->client, reg + ind, 2, + (u8 *)&d); + if (result != 2) + return -EINVAL; + *val = (short)be16_to_cpup(&d); + + return IIO_VAL_INT; +} + +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); + + switch (mask) { + case IIO_CHAN_INFO_RAW: + { + int ret, result; + + ret = IIO_VAL_INT; + result = 0; + mutex_lock(&indio_dev->mlock); + if (!st->chip_config.enable) { + result = inv_mpu6050_set_power_itg(st, true); + if (result) + goto error_read_raw; + } + /* when enable is on, power is already on */ + switch (chan->type) { + case IIO_ANGL_VEL: + if (!st->chip_config.gyro_fifo_enable || + !st->chip_config.enable) { + result = inv_mpu6050_switch_engine(st, true, + INV_MPU6050_BIT_PWR_GYRO_STBY); + if (result) + goto error_read_raw; + } + ret = inv_mpu6050_sensor_show(st, st->reg->raw_gyro, + chan->channel2, val); + if (!st->chip_config.gyro_fifo_enable || + !st->chip_config.enable) { + result = inv_mpu6050_switch_engine(st, false, + INV_MPU6050_BIT_PWR_GYRO_STBY); + if (result) + goto error_read_raw; + } + break; + case IIO_ACCEL: + if (!st->chip_config.accl_fifo_enable || + !st->chip_config.enable) { + result = inv_mpu6050_switch_engine(st, true, + INV_MPU6050_BIT_PWR_ACCL_STBY); + if (result) + goto error_read_raw; + } + ret = inv_mpu6050_sensor_show(st, st->reg->raw_accl, + chan->channel2, val); + if (!st->chip_config.accl_fifo_enable || + !st->chip_config.enable) { + result = inv_mpu6050_switch_engine(st, false, + INV_MPU6050_BIT_PWR_ACCL_STBY); + if (result) + goto error_read_raw; + } + break; + case IIO_TEMP: + /* wait for stablization */ + msleep(INV_MPU6050_SENSOR_UP_TIME); + inv_mpu6050_sensor_show(st, st->reg->temperature, + IIO_MOD_X, val); + break; + default: + ret = -EINVAL; + break; + } +error_read_raw: + if (!st->chip_config.enable) + result |= inv_mpu6050_set_power_itg(st, false); + mutex_unlock(&indio_dev->mlock); + if (result) + return result; + + return ret; + } + case IIO_CHAN_INFO_SCALE: + switch (chan->type) { + case IIO_ANGL_VEL: + *val = 0; + *val2 = gyro_scale_6050[st->chip_config.fsr]; + + return IIO_VAL_INT_PLUS_NANO; + case IIO_ACCEL: + *val = 0; + *val2 = accel_scale[st->chip_config.accl_fs]; + + return IIO_VAL_INT_PLUS_MICRO; + case IIO_TEMP: + *val = 0; + *val2 = INV_MPU6050_TEMP_SCALE; + + return IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } + case IIO_CHAN_INFO_OFFSET: + switch (chan->type) { + case IIO_TEMP: + *val = INV_MPU6050_TEMP_OFFSET; + + return IIO_VAL_INT; + default: + return -EINVAL; + } + default: + return -EINVAL; + } +} + +static int inv_mpu6050_write_fsr(struct inv_mpu6050_state *st, int fsr) +{ + int result; + u8 d; + + if (fsr < 0 || fsr > INV_MPU6050_MAX_GYRO_FS_PARAM) + return -EINVAL; + if (fsr == st->chip_config.fsr) + return 0; + + d = (fsr << INV_MPU6050_GYRO_CONFIG_FSR_SHIFT); + result = inv_mpu6050_write_reg(st, st->reg->gyro_config, d); + if (result) + return result; + st->chip_config.fsr = fsr; + + return 0; +} + +static int inv_mpu6050_write_accel_fs(struct inv_mpu6050_state *st, int fs) +{ + int result; + u8 d; + + if (fs < 0 || fs > INV_MPU6050_MAX_ACCL_FS_PARAM) + return -EINVAL; + if (fs == st->chip_config.accl_fs) + return 0; + + d = (fs << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT); + result = inv_mpu6050_write_reg(st, st->reg->accl_config, d); + if (result) + return result; + st->chip_config.accl_fs = fs; + + return 0; +} + +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); + int result; + + mutex_lock(&indio_dev->mlock); + /* we should only update scale when the chip is disabled, i.e., + not running */ + if (st->chip_config.enable) { + result = -EBUSY; + goto error_write_raw; + } + result = inv_mpu6050_set_power_itg(st, true); + if (result) + goto error_write_raw; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + switch (chan->type) { + case IIO_ANGL_VEL: + result = inv_mpu6050_write_fsr(st, val); + break; + case IIO_ACCEL: + result = inv_mpu6050_write_accel_fs(st, val); + break; + default: + result = -EINVAL; + break; + } + break; + default: + result = -EINVAL; + break; + } + +error_write_raw: + result |= inv_mpu6050_set_power_itg(st, false); + mutex_unlock(&indio_dev->mlock); + + return result; +} + +/** + * inv_mpu6050_set_lpf() - set low pass filer based on fifo rate. + * + * Based on the Nyquist principle, the sampling rate must + * exceed twice of the bandwidth of the signal, or there + * would be alising. This function basically search for the + * correct low pass parameters based on the fifo rate, e.g, + * sampling frequency. + */ +static int inv_mpu6050_set_lpf(struct inv_mpu6050_state *st, int rate) +{ + const int hz[] = {188, 98, 42, 20, 10, 5}; + const int d[] = {INV_MPU6050_FILTER_188HZ, INV_MPU6050_FILTER_98HZ, + INV_MPU6050_FILTER_42HZ, INV_MPU6050_FILTER_20HZ, + INV_MPU6050_FILTER_10HZ, INV_MPU6050_FILTER_5HZ}; + int i, h, result; + u8 data; + + h = (rate >> 1); + i = 0; + while ((h < hz[i]) && (i < ARRAY_SIZE(d) - 1)) + i++; + data = d[i]; + result = inv_mpu6050_write_reg(st, st->reg->lpf, 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) +{ + s32 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); + + 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; + if (fifo_rate == st->chip_config.fifo_rate) + return count; + + mutex_lock(&indio_dev->mlock); + if (st->chip_config.enable) { + result = -EBUSY; + goto fifo_rate_fail; + } + result = inv_mpu6050_set_power_itg(st, true); + if (result) + goto fifo_rate_fail; + + d = INV_MPU6050_ONE_K_HZ / fifo_rate - 1; + result = inv_mpu6050_write_reg(st, st->reg->sample_rate_div, d); + if (result) + goto fifo_rate_fail; + st->chip_config.fifo_rate = fifo_rate; + + result = inv_mpu6050_set_lpf(st, fifo_rate); + if (result) + goto fifo_rate_fail; + +fifo_rate_fail: + result |= inv_mpu6050_set_power_itg(st, false); + mutex_unlock(&indio_dev->mlock); + 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)); + + return sprintf(buf, "%d\n", st->chip_config.fifo_rate); +} + +/** + * inv_attr_show() - calling this function will show current + * parameters. + */ +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 sprintf(buf, "%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; +} + +#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), \ + .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), + /* + * Note that temperature should only be via polled reading only, + * not the final scan elements output. + */ + { + .type = IIO_TEMP, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) + | BIT(IIO_CHAN_INFO_OFFSET) + | BIT(IIO_CHAN_INFO_SCALE), + .scan_index = -1, + }, + 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), +}; + +/* constant IIO attribute */ +static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("10 20 50 100 200 500"); +static IIO_DEV_ATTR_SAMP_FREQ(S_IRUGO | S_IWUSR, inv_fifo_rate_show, + inv_mpu6050_fifo_rate_store); +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, + &iio_dev_attr_in_accel_matrix.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_const_attr_sampling_frequency_available.dev_attr.attr, + NULL, +}; + +static const struct attribute_group inv_attribute_group = { + .attrs = inv_attributes +}; + +static const struct iio_info mpu_info = { + .driver_module = THIS_MODULE, + .read_raw = &inv_mpu6050_read_raw, + .write_raw = &inv_mpu6050_write_raw, + .attrs = &inv_attribute_group, + .validate_trigger = inv_mpu6050_validate_trigger, +}; + +/** + * inv_check_and_setup_chip() - check and setup chip. + */ +static int inv_check_and_setup_chip(struct inv_mpu6050_state *st, + const struct i2c_device_id *id) +{ + int result; + + st->chip_type = INV_MPU6050; + st->hw = &hw_info[st->chip_type]; + st->reg = hw_info[st->chip_type].reg; + + /* reset to make sure previous state are not there */ + result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_1, + INV_MPU6050_BIT_H_RESET); + if (result) + return result; + msleep(INV_MPU6050_POWER_UP_TIME); + /* toggle power state. After reset, the sleep bit could be on + or off depending on the OTP settings. Toggling power would + 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, false); + if (result) + return result; + result = inv_mpu6050_set_power_itg(st, true); + if (result) + return result; + + result = inv_mpu6050_switch_engine(st, false, + INV_MPU6050_BIT_PWR_ACCL_STBY); + if (result) + return result; + result = inv_mpu6050_switch_engine(st, false, + INV_MPU6050_BIT_PWR_GYRO_STBY); + if (result) + return result; + + 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) +{ + struct inv_mpu6050_state *st; + struct iio_dev *indio_dev; + int result; + + if (!i2c_check_functionality(client->adapter, + I2C_FUNC_SMBUS_READ_I2C_BLOCK | + I2C_FUNC_SMBUS_WRITE_I2C_BLOCK)) { + result = -ENOSYS; + goto out_no_free; + } + indio_dev = iio_device_alloc(sizeof(*st)); + if (indio_dev == NULL) { + result = -ENOMEM; + goto out_no_free; + } + st = iio_priv(indio_dev); + st->client = client; + st->plat_data = *(struct inv_mpu6050_platform_data + *)dev_get_platdata(&client->dev); + /* power is turned on inside check chip type*/ + result = inv_check_and_setup_chip(st, id); + if (result) + goto out_free; + + result = inv_mpu6050_init_config(indio_dev); + if (result) { + dev_err(&client->dev, + "Could not initialize device.\n"); + goto out_free; + } + + i2c_set_clientdata(client, indio_dev); + indio_dev->dev.parent = &client->dev; + indio_dev->name = id->name; + indio_dev->channels = inv_mpu_channels; + indio_dev->num_channels = ARRAY_SIZE(inv_mpu_channels); + + indio_dev->info = &mpu_info; + indio_dev->modes = INDIO_BUFFER_TRIGGERED; + + result = iio_triggered_buffer_setup(indio_dev, + inv_mpu6050_irq_handler, + inv_mpu6050_read_fifo, + NULL); + if (result) { + dev_err(&st->client->dev, "configure buffer fail %d\n", + result); + goto out_free; + } + result = inv_mpu6050_probe_trigger(indio_dev); + if (result) { + dev_err(&st->client->dev, "trigger probe fail %d\n", result); + goto out_unreg_ring; + } + + INIT_KFIFO(st->timestamps); + spin_lock_init(&st->time_stamp_lock); + result = iio_device_register(indio_dev); + if (result) { + dev_err(&st->client->dev, "IIO register fail %d\n", result); + goto out_remove_trigger; + } + + return 0; + +out_remove_trigger: + inv_mpu6050_remove_trigger(st); +out_unreg_ring: + iio_triggered_buffer_cleanup(indio_dev); +out_free: + iio_device_free(indio_dev); +out_no_free: + + return result; +} + +static int 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); + + iio_device_unregister(indio_dev); + inv_mpu6050_remove_trigger(st); + iio_triggered_buffer_cleanup(indio_dev); + iio_device_free(indio_dev); + + return 0; +} +#ifdef CONFIG_PM_SLEEP + +static int inv_mpu_resume(struct device *dev) +{ + return inv_mpu6050_set_power_itg( + iio_priv(i2c_get_clientdata(to_i2c_client(dev))), true); +} + +static int inv_mpu_suspend(struct device *dev) +{ + return inv_mpu6050_set_power_itg( + iio_priv(i2c_get_clientdata(to_i2c_client(dev))), false); +} +static SIMPLE_DEV_PM_OPS(inv_mpu_pmops, inv_mpu_suspend, inv_mpu_resume); + +#define INV_MPU6050_PMOPS (&inv_mpu_pmops) +#else +#define INV_MPU6050_PMOPS NULL +#endif /* CONFIG_PM_SLEEP */ + +/* + * 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}, + {} +}; + +MODULE_DEVICE_TABLE(i2c, inv_mpu_id); + +static struct i2c_driver inv_mpu_driver = { + .probe = inv_mpu_probe, + .remove = inv_mpu_remove, + .id_table = inv_mpu_id, + .driver = { + .owner = THIS_MODULE, + .name = "inv-mpu6050", + .pm = INV_MPU6050_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 00000000000..f38395529a4 --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h @@ -0,0 +1,246 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +*/ +#include <linux/i2c.h> +#include <linux/kfifo.h> +#include <linux/spinlock.h> +#include <linux/iio/iio.h> +#include <linux/iio/buffer.h> +#include <linux/iio/sysfs.h> +#include <linux/iio/kfifo_buf.h> +#include <linux/iio/trigger.h> +#include <linux/iio/triggered_buffer.h> +#include <linux/iio/trigger_consumer.h> +#include <linux/platform_data/invensense_mpu6050.h> + +/** + * struct inv_mpu6050_reg_map - Notable registers. + * @sample_rate_div: Divider applied to gyro output rate. + * @lpf: Configures internal 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. + * @pwr_mgmt_1: Controls chip's power state and clock source. + * @pwr_mgmt_2: Controls power state of individual sensors. + */ +struct inv_mpu6050_reg_map { + u8 sample_rate_div; + u8 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 pwr_mgmt_1; + u8 pwr_mgmt_2; +}; + +/*device enum */ +enum inv_devices { + INV_MPU6050, + INV_NUM_PARTS +}; + +/** + * struct inv_mpu6050_chip_config - Cached chip configuration data. + * @fsr: Full scale range. + * @lpf: Digital low pass filter frequency. + * @accl_fs: accel full scale range. + * @enable: master enable state. + * @accl_fifo_enable: enable accel data output + * @gyro_fifo_enable: enable gyro data output + * @fifo_rate: FIFO update rate. + */ +struct inv_mpu6050_chip_config { + unsigned int fsr:2; + unsigned int lpf:3; + unsigned int accl_fs:2; + unsigned int enable:1; + unsigned int accl_fifo_enable:1; + unsigned int gyro_fifo_enable:1; + u16 fifo_rate; +}; + +/** + * struct inv_mpu6050_hw - Other important hardware information. + * @num_reg: Number of registers on device. + * @name: name of the chip. + * @reg: register map of the chip. + * @config: configuration of the chip. + */ +struct inv_mpu6050_hw { + u8 num_reg; + u8 *name; + const struct inv_mpu6050_reg_map *reg; + const struct inv_mpu6050_chip_config *config; +}; + +/* + * struct inv_mpu6050_state - Driver state variables. + * @TIMESTAMP_FIFO_SIZE: fifo size for timestamp. + * @trig: IIO trigger. + * @chip_config: Cached attribute information. + * @reg: Map of important registers. + * @hw: Other hardware-specific information. + * @chip_type: chip type. + * @time_stamp_lock: spin lock to time stamp. + * @client: i2c client handle. + * @plat_data: platform data. + * @timestamps: kfifo queue to store time stamp. + */ +struct inv_mpu6050_state { +#define TIMESTAMP_FIFO_SIZE 16 + 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; + spinlock_t time_stamp_lock; + struct i2c_client *client; + struct inv_mpu6050_platform_data plat_data; + DECLARE_KFIFO(timestamps, long long, TIMESTAMP_FIFO_SIZE); +}; + +/*register and associated bit definition*/ +#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_ACCEL_OUT 0x08 +#define INV_MPU6050_BITS_GYRO_OUT 0x70 + +#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_USER_CTRL 0x6A +#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_REG_PWR_MGMT_1 0x6B +#define INV_MPU6050_BIT_H_RESET 0x80 +#define INV_MPU6050_BIT_SLEEP 0x40 +#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 + +#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 +#define INV_MPU6050_FIFO_THRESHOLD 500 +#define INV_MPU6050_POWER_UP_TIME 100 +#define INV_MPU6050_TEMP_UP_TIME 100 +#define INV_MPU6050_SENSOR_UP_TIME 30 +#define INV_MPU6050_REG_UP_TIME 5 + +#define INV_MPU6050_TEMP_OFFSET 12421 +#define INV_MPU6050_TEMP_SCALE 2941 +#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_MPU6050_ACCL_CONFIG_FSR_SHIFT 3 + +/* 6 + 6 round up and plus 8 */ +#define INV_MPU6050_OUTPUT_DATA_SIZE 24 + +/* init parameters */ +#define INV_MPU6050_INIT_FIFO_RATE 50 +#define INV_MPU6050_TIME_STAMP_TOR 5 +#define INV_MPU6050_MAX_FIFO_RATE 1000 +#define INV_MPU6050_MIN_FIFO_RATE 4 +#define INV_MPU6050_ONE_K_HZ 1000 + +/* scan element definition */ +enum inv_mpu6050_scan { + INV_MPU6050_SCAN_ACCL_X, + INV_MPU6050_SCAN_ACCL_Y, + INV_MPU6050_SCAN_ACCL_Z, + INV_MPU6050_SCAN_GYRO_X, + INV_MPU6050_SCAN_GYRO_Y, + INV_MPU6050_SCAN_GYRO_Z, + INV_MPU6050_SCAN_TIMESTAMP, +}; + +enum inv_mpu6050_filter_e { + INV_MPU6050_FILTER_256HZ_NOLPF2 = 0, + INV_MPU6050_FILTER_188HZ, + INV_MPU6050_FILTER_98HZ, + INV_MPU6050_FILTER_42HZ, + INV_MPU6050_FILTER_20HZ, + INV_MPU6050_FILTER_10HZ, + INV_MPU6050_FILTER_5HZ, + INV_MPU6050_FILTER_2100HZ_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_irq_handler(int irq, void *p); +irqreturn_t inv_mpu6050_read_fifo(int irq, void *p); +int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev); +void inv_mpu6050_remove_trigger(struct inv_mpu6050_state *st); +int inv_reset_fifo(struct iio_dev *indio_dev); +int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask); +int inv_mpu6050_write_reg(struct inv_mpu6050_state *st, int reg, u8 val); +int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on); 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 00000000000..7da0832f187 --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c @@ -0,0 +1,195 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +*/ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include "inv_mpu_iio.h" + +int inv_reset_fifo(struct iio_dev *indio_dev) +{ + int result; + u8 d; + struct inv_mpu6050_state *st = iio_priv(indio_dev); + + /* disable interrupt */ + result = inv_mpu6050_write_reg(st, st->reg->int_enable, 0); + if (result) { + dev_err(&st->client->dev, "int_enable failed %d\n", result); + return result; + } + /* disable the sensor output to FIFO */ + result = inv_mpu6050_write_reg(st, st->reg->fifo_en, 0); + if (result) + goto reset_fifo_fail; + /* disable fifo reading */ + result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, 0); + if (result) + goto reset_fifo_fail; + + /* reset FIFO*/ + result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, + INV_MPU6050_BIT_FIFO_RST); + if (result) + goto reset_fifo_fail; + /* enable interrupt */ + if (st->chip_config.accl_fifo_enable || + st->chip_config.gyro_fifo_enable) { + result = inv_mpu6050_write_reg(st, st->reg->int_enable, + INV_MPU6050_BIT_DATA_RDY_EN); + if (result) + return result; + } + /* enable FIFO reading and I2C master interface*/ + result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, + INV_MPU6050_BIT_FIFO_EN); + if (result) + goto reset_fifo_fail; + /* 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; + result = inv_mpu6050_write_reg(st, st->reg->fifo_en, d); + if (result) + goto reset_fifo_fail; + + return 0; + +reset_fifo_fail: + dev_err(&st->client->dev, "reset fifo failed %d\n", result); + result = inv_mpu6050_write_reg(st, st->reg->int_enable, + INV_MPU6050_BIT_DATA_RDY_EN); + + return result; +} + +static void inv_clear_kfifo(struct inv_mpu6050_state *st) +{ + unsigned long flags; + + /* take the spin lock sem to avoid interrupt kick in */ + spin_lock_irqsave(&st->time_stamp_lock, flags); + kfifo_reset(&st->timestamps); + spin_unlock_irqrestore(&st->time_stamp_lock, flags); +} + +/** + * inv_mpu6050_irq_handler() - Cache a timestamp at each data ready interrupt. + */ +irqreturn_t inv_mpu6050_irq_handler(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); + s64 timestamp; + + timestamp = iio_get_time_ns(); + kfifo_in_spinlocked(&st->timestamps, ×tamp, 1, + &st->time_stamp_lock); + + return IRQ_WAKE_THREAD; +} + +/** + * 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; + u8 data[INV_MPU6050_OUTPUT_DATA_SIZE]; + u16 fifo_count; + s64 timestamp; + u64 *tmp; + + mutex_lock(&indio_dev->mlock); + if (!(st->chip_config.accl_fifo_enable | + st->chip_config.gyro_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; + + /* + * read fifo_count register to know how many bytes inside FIFO + * right now + */ + result = i2c_smbus_read_i2c_block_data(st->client, + st->reg->fifo_count_h, + INV_MPU6050_FIFO_COUNT_BYTE, data); + if (result != INV_MPU6050_FIFO_COUNT_BYTE) + goto end_session; + fifo_count = be16_to_cpup((__be16 *)(&data[0])); + if (fifo_count < bytes_per_datum) + goto end_session; + /* fifo count can't be odd number, if it is odd, reset fifo*/ + if (fifo_count & 1) + goto flush_fifo; + if (fifo_count > INV_MPU6050_FIFO_THRESHOLD) + goto flush_fifo; + /* Timestamp mismatch. */ + if (kfifo_len(&st->timestamps) > + fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR) + goto flush_fifo; + while (fifo_count >= bytes_per_datum) { + result = i2c_smbus_read_i2c_block_data(st->client, + st->reg->fifo_r_w, + bytes_per_datum, data); + if (result != bytes_per_datum) + goto flush_fifo; + + result = kfifo_out(&st->timestamps, ×tamp, 1); + /* when there is no timestamp, put timestamp as 0 */ + if (0 == result) + timestamp = 0; + + tmp = (u64 *)data; + tmp[DIV_ROUND_UP(bytes_per_datum, 8)] = timestamp; + result = iio_push_to_buffers(indio_dev, data); + if (result) + goto flush_fifo; + fifo_count -= bytes_per_datum; + } + +end_session: + mutex_unlock(&indio_dev->mlock); + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; + +flush_fifo: + /* Flush HW and SW FIFOs. */ + inv_reset_fifo(indio_dev); + inv_clear_kfifo(st); + mutex_unlock(&indio_dev->mlock); + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; +} 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 00000000000..03b9372c121 --- /dev/null +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c @@ -0,0 +1,155 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +*/ + +#include "inv_mpu_iio.h" + +static void inv_scan_query(struct iio_dev *indio_dev) +{ + struct inv_mpu6050_state *st = iio_priv(indio_dev); + + 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); +} + +/** + * 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); + int result; + + if (enable) { + result = inv_mpu6050_set_power_itg(st, true); + if (result) + return result; + inv_scan_query(indio_dev); + if (st->chip_config.gyro_fifo_enable) { + result = inv_mpu6050_switch_engine(st, true, + INV_MPU6050_BIT_PWR_GYRO_STBY); + if (result) + return result; + } + if (st->chip_config.accl_fifo_enable) { + result = inv_mpu6050_switch_engine(st, true, + INV_MPU6050_BIT_PWR_ACCL_STBY); + if (result) + return result; + } + result = inv_reset_fifo(indio_dev); + if (result) + return result; + } else { + result = inv_mpu6050_write_reg(st, st->reg->fifo_en, 0); + if (result) + return result; + + result = inv_mpu6050_write_reg(st, st->reg->int_enable, 0); + if (result) + return result; + + result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, 0); + if (result) + return result; + + result = inv_mpu6050_switch_engine(st, false, + INV_MPU6050_BIT_PWR_GYRO_STBY); + if (result) + return result; + + result = inv_mpu6050_switch_engine(st, false, + INV_MPU6050_BIT_PWR_ACCL_STBY); + if (result) + return result; + result = inv_mpu6050_set_power_itg(st, false); + if (result) + return result; + } + st->chip_config.enable = enable; + + return 0; +} + +/** + * 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) +{ + return inv_mpu6050_set_enable(iio_trigger_get_drvdata(trig), state); +} + +static const struct iio_trigger_ops inv_mpu_trigger_ops = { + .owner = THIS_MODULE, + .set_trigger_state = &inv_mpu_data_rdy_trigger_set_state, +}; + +int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev) +{ + int ret; + struct inv_mpu6050_state *st = iio_priv(indio_dev); + + st->trig = iio_trigger_alloc("%s-dev%d", + indio_dev->name, + indio_dev->id); + if (st->trig == NULL) { + ret = -ENOMEM; + goto error_ret; + } + ret = request_irq(st->client->irq, &iio_trigger_generic_data_rdy_poll, + IRQF_TRIGGER_RISING, + "inv_mpu", + st->trig); + if (ret) + goto error_free_trig; + st->trig->dev.parent = &st->client->dev; + st->trig->ops = &inv_mpu_trigger_ops; + iio_trigger_set_drvdata(st->trig, indio_dev); + ret = iio_trigger_register(st->trig); + if (ret) + goto error_free_irq; + indio_dev->trig = st->trig; + + return 0; + +error_free_irq: + free_irq(st->client->irq, st->trig); +error_free_trig: + iio_trigger_free(st->trig); +error_ret: + return ret; +} + +void inv_mpu6050_remove_trigger(struct inv_mpu6050_state *st) +{ + iio_trigger_unregister(st->trig); + free_irq(st->client->irq, st->trig); + iio_trigger_free(st->trig); +} diff --git a/drivers/iio/imu/st_lsm6ds3/Kconfig b/drivers/iio/imu/st_lsm6ds3/Kconfig new file mode 100644 index 00000000000..cb350b25d12 --- /dev/null +++ b/drivers/iio/imu/st_lsm6ds3/Kconfig @@ -0,0 +1,75 @@ +# +# st-lsm6ds3 drivers for STMicroelectronics combo sensor +# + +config ST_LSM6DS3_IIO + tristate "STMicroelectronics LSM6DS3 sensor" + depends on (I2C || SPI) && SYSFS + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + help + This driver supports the STMicroelectronics LSM6DS3 sensor. + It is a gyroscope/accelerometer combo device. + This driver can be built as a module. The module will be called + st-lsm6ds3. + +config ST_LSM6DS3_IIO_LIMIT_FIFO + int "Limit fifo read lenght (#n byte)" + depends on ST_LSM6DS3_IIO + range 0 8192 + default 0 + help + Limit atomic fifo read to #n byte. In some platform i2c/spi read + can be limited by software or hardware. + + Set 0 to disable the limit. + +config ST_LSM6DS3_IIO_SENSORS_WAKEUP + bool "All sensors can wake-up system during suspend" + depends on ST_LSM6DS3_IIO + default n + help + If disabled only tilt and significant motion can wake-up system + during suspend. + + If enabled all sensors can wake-up system during suspend. + +menuconfig ST_LSM6DS3_IIO_MASTER_SUPPORT + bool "I2C master controller" + depends on I2C && ST_LSM6DS3_IIO + default n + help + Added support for I2C master controller. Supported sensors up + to 4. + +if ST_LSM6DS3_IIO_MASTER_SUPPORT + +config ST_LSM6DS3_ENABLE_INTERNAL_PULLUP + bool "Enabled internals pull-up resistors" + default y + +choice + prompt "External sensor 0" + default ST_LSM6DS3_IIO_EXT0_LIS3MDL + help + Choose the external sensor 0 connected to LSM6DS3. + +config ST_LSM6DS3_IIO_EXT0_LIS3MDL + bool "LIS3MDL" +config ST_LSM6DS3_IIO_EXT0_AKM09912 + bool "AKM09912" +endchoice + +choice + prompt "External sensor 1" + default ST_LSM6DS3_IIO_EXT1_DISABLED + help + Choose the external sensor 1 connected to LSM6DS3. + +config ST_LSM6DS3_IIO_EXT1_DISABLED + bool "Disabled" +config ST_LSM6DS3_IIO_EXT1_LPS22HB + bool "LPS22HB" +endchoice + +endif diff --git a/drivers/iio/imu/st_lsm6ds3/Makefile b/drivers/iio/imu/st_lsm6ds3/Makefile new file mode 100644 index 00000000000..36348a34e89 --- /dev/null +++ b/drivers/iio/imu/st_lsm6ds3/Makefile @@ -0,0 +1,11 @@ +# +# Makefile for STMicroelectronics LSM6DS3 sensor. +# + +obj-$(CONFIG_ST_LSM6DS3_IIO) += st_lsm6ds3.o +st_lsm6ds3-objs := st_lsm6ds3_core.o + +obj-$(CONFIG_ST_LSM6DS3_IIO) += st_lsm6ds3_spi.o st_lsm6ds3_i2c.o +st_lsm6ds3-$(CONFIG_IIO_BUFFER) += st_lsm6ds3_buffer.o +st_lsm6ds3-$(CONFIG_IIO_TRIGGER) += st_lsm6ds3_trigger.o +st_lsm6ds3-$(CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT) += st_lsm6ds3_i2c_master.o diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h new file mode 100644 index 00000000000..479b9188273 --- /dev/null +++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h @@ -0,0 +1,296 @@ +/* + * STMicroelectronics lsm6ds3 driver + * + * Copyright 2014 STMicroelectronics Inc. + * + * Denis Ciocca <denis.ciocca@st.com> + * v. 2.1.2 + * Licensed under the GPL-2. + */ + +#ifndef ST_LSM6DS3_H +#define ST_LSM6DS3_H + +#include <linux/types.h> +#include <linux/iio/trigger.h> +#include <linux/wakelock.h> +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT +#include <linux/i2c.h> +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + +#define LSM6DS3_DEV_NAME "lsm6ds3" + +#define ST_INDIO_DEV_ACCEL 0 +#define ST_INDIO_DEV_GYRO 1 +#define ST_INDIO_DEV_SIGN_MOTION 2 +#define ST_INDIO_DEV_STEP_COUNTER 3 +#define ST_INDIO_DEV_STEP_DETECTOR 4 +#define ST_INDIO_DEV_TILT 5 +#define ST_INDIO_DEV_NUM 6 + +#define ST_INDIO_DEV_EXT0 ST_INDIO_DEV_NUM +#define ST_INDIO_DEV_EXT1 (ST_INDIO_DEV_NUM + 1) + +#define ST_LSM6DS3_ACCEL_DEPENDENCY ((1 << ST_INDIO_DEV_ACCEL) | \ + (1 << ST_INDIO_DEV_STEP_COUNTER) | \ + (1 << ST_INDIO_DEV_TILT) | \ + (1 << ST_INDIO_DEV_SIGN_MOTION) | \ + (1 << ST_INDIO_DEV_STEP_DETECTOR) | \ + (1 << ST_INDIO_DEV_EXT0) | \ + (1 << ST_INDIO_DEV_EXT1)) + +#define ST_LSM6DS3_PEDOMETER_DEPENDENCY ((1 << ST_INDIO_DEV_STEP_COUNTER) | \ + (1 << ST_INDIO_DEV_STEP_DETECTOR) | \ + (1 << ST_INDIO_DEV_SIGN_MOTION)) + +#define ST_LSM6DS3_EXTRA_DEPENDENCY ((1 << ST_INDIO_DEV_STEP_COUNTER) | \ + (1 << ST_INDIO_DEV_TILT) | \ + (1 << ST_INDIO_DEV_SIGN_MOTION) | \ + (1 << ST_INDIO_DEV_STEP_DETECTOR) | \ + (1 << ST_INDIO_DEV_EXT0) | \ + (1 << ST_INDIO_DEV_EXT1)) + +#define ST_LSM6DS3_USE_BUFFER ((1 << ST_INDIO_DEV_ACCEL) | \ + (1 << ST_INDIO_DEV_GYRO) | \ + (1 << ST_INDIO_DEV_STEP_COUNTER)) + +#define ST_LSM6DS3_EXT_SENSORS ((1 << ST_INDIO_DEV_EXT0) | \ + (1 << ST_INDIO_DEV_EXT1)) + +#ifdef CONFIG_ST_LSM6DS3_IIO_SENSORS_WAKEUP +#define ST_LSM6DS3_WAKE_UP_SENSORS ((1 << ST_INDIO_DEV_SIGN_MOTION) | \ + (1 << ST_INDIO_DEV_TILT)) +#else /* CONFIG_ST_LSM6DS3_IIO_SENSORS_WAKEUP */ +#define ST_LSM6DS3_WAKE_UP_SENSORS ((1 << ST_INDIO_DEV_SIGN_MOTION) | \ + (1 << ST_INDIO_DEV_TILT) | \ + (1 << ST_INDIO_DEV_ACCEL) | \ + (1 << ST_INDIO_DEV_GYRO) | \ + (1 << ST_INDIO_DEV_STEP_COUNTER) | \ + (1 << ST_INDIO_DEV_STEP_DETECTOR) | \ + (1 << ST_INDIO_DEV_EXT0) | \ + (1 << ST_INDIO_DEV_EXT1)) +#endif /* CONFIG_ST_LSM6DS3_IIO_SENSORS_WAKEUP */ + +#define ST_LSM6DS3_TX_MAX_LENGTH 12 +#define ST_LSM6DS3_RX_MAX_LENGTH 8193 + +#define ST_LSM6DS3_BYTE_FOR_CHANNEL 2 +#define ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE 6 + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT +#define ST_LSM6DS3_NUM_CLIENTS 2 +#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ +#define ST_LSM6DS3_NUM_CLIENTS 0 +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + +#define ST_LSM6DS3_LSM_CHANNELS(device_type, modif, index, mod, \ + endian, sbits, rbits, addr, s) \ +{ \ + .type = device_type, \ + .modified = modif, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .scan_index = index, \ + .channel2 = mod, \ + .address = addr, \ + .scan_type = { \ + .sign = s, \ + .realbits = rbits, \ + .shift = sbits - rbits, \ + .storagebits = sbits, \ + .endianness = endian, \ + }, \ +} + +#define ST_LSM6DS3_FIFO_LENGHT() \ + IIO_DEVICE_ATTR(hw_fifo_lenght, S_IRUGO, \ + st_lsm6ds3_sysfs_get_hw_fifo_lenght, NULL, 0); + +#define ST_LSM6DS3_FIFO_FLUSH() \ + IIO_DEVICE_ATTR(flush, S_IWUSR, NULL, st_lsm6ds3_sysfs_flush_fifo, 0); + +enum fifo_mode { + BYPASS = 0, + CONTINUOS, +}; + +struct st_lsm6ds3_transfer_buffer { + struct mutex buf_lock; + u8 rx_buf[ST_LSM6DS3_RX_MAX_LENGTH]; + u8 tx_buf[ST_LSM6DS3_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +enum +{ + LSM6DS3_WAKEUP_NONE = 0, + LSM6DS3_WAKEUP_TAP = 1, + LSM6DS3_WAKEUP_6D = 2, + LSM6DS3_WAKEUP_OTHER = 4 +}; + +struct lsm6ds3_data { + const char *name; + + bool reset_steps; + bool sign_motion_event_ready; + int last_wakeup_source; + int wake_lock_initialized; + struct wake_lock wlock; + u8 first_irq_from_resume; + u8 reg_read; +#define SIXD_MASK_VALID_BITS (0x3f) + u8 sixd_mask; + u8 int1_save; + + u8 *fifo_data; + u8 sensors_enabled; + u8 gyro_selftest_status; + u8 accel_selftest_status; + u8 accel_samples_in_pattern; + u8 gyro_samples_in_pattern; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + u8 ext0_samples_in_pattern; + u8 ext1_samples_in_pattern; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + u8 accel_samples_to_discard; + u8 gyro_samples_to_discard; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + u8 ext_samples_to_discard[ST_LSM6DS3_NUM_CLIENTS]; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + u16 fifo_threshold; + + int irq; + + s64 timestamp; + int64_t accel_deltatime; + int64_t accel_timestamp; + int64_t gyro_deltatime; + int64_t gyro_timestamp; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + int64_t ext0_deltatime; + int64_t ext0_timestamp; + int64_t ext1_deltatime; + int64_t ext1_timestamp; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + struct work_struct data_work; + + struct device *dev; + struct iio_dev *indio_dev[ST_INDIO_DEV_NUM + ST_LSM6DS3_NUM_CLIENTS]; + struct iio_trigger *trig[ST_INDIO_DEV_NUM + ST_LSM6DS3_NUM_CLIENTS]; + struct mutex bank_registers_lock; + struct mutex fifo_lock; + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + struct i2c_client *master_client[ST_LSM6DS3_NUM_CLIENTS]; + struct mutex passthrough_lock; + bool ext0_available; + bool ext1_available; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + const struct st_lsm6ds3_transfer_function *tf; + struct st_lsm6ds3_transfer_buffer tb; +}; + +struct st_lsm6ds3_transfer_function { + int (*write) (struct lsm6ds3_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock); + int (*read) (struct lsm6ds3_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock); +}; + +struct lsm6ds3_sensor_data { + struct lsm6ds3_data *cdata; + + unsigned int c_odr; + unsigned int c_gain[3]; + + u8 num_data_channels; + u8 sindex; + u8 *buffer_data; +}; + +int st_lsm6ds3_write_data_with_mask(struct lsm6ds3_data *cdata, + u8 reg_addr, u8 mask, u8 data, bool b_lock); + +int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq); +void st_lsm6ds3_common_remove(struct lsm6ds3_data *cdata, int irq); + +int st_lsm6ds3_set_enable(struct lsm6ds3_sensor_data *sdata, bool enable); +int st_lsm6ds3_set_axis_enable(struct lsm6ds3_sensor_data *sdata, u8 value); +int st_lsm6ds3_set_drdy_irq(struct lsm6ds3_sensor_data *sdata, bool state); +int st_lsm6ds3_set_fifo_mode(struct lsm6ds3_data *cdata, enum fifo_mode fm); +int st_lsm6ds3_reconfigure_fifo(struct lsm6ds3_data *cdata, + bool disable_irq_and_flush); + +ssize_t st_lsm6ds3_sysfs_get_hw_fifo_lenght(struct device *dev, + struct device_attribute *attr, char *buf); +ssize_t st_lsm6ds3_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size); + +#ifdef CONFIG_IIO_BUFFER +int st_lsm6ds3_allocate_rings(struct lsm6ds3_data *cdata); +void st_lsm6ds3_deallocate_rings(struct lsm6ds3_data *cdata); +int st_lsm6ds3_trig_set_state(struct iio_trigger *trig, bool state); +void st_lsm6ds3_read_fifo(struct lsm6ds3_data *cdata, bool check_fifo_len); +int st_lsm6ds3_set_fifo_decimators_and_threshold(struct lsm6ds3_data *cdata); +#define ST_LSM6DS3_TRIGGER_SET_STATE (&st_lsm6ds3_trig_set_state) +#else /* CONFIG_IIO_BUFFER */ +static inline int st_lsm6ds3_allocate_rings(struct lsm6ds3_data *cdata) +{ + return 0; +} +static inline void st_lsm6ds3_deallocate_rings(struct lsm6ds3_data *cdata) +{ +} +#define ST_LSM6DS3_TRIGGER_SET_STATE NULL +#endif /* CONFIG_IIO_BUFFER */ + +#ifdef CONFIG_IIO_TRIGGER +void st_lsm6ds3_flush_works(void); +int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata, + const struct iio_trigger_ops *trigger_ops); + +void st_lsm6ds3_deallocate_triggers(struct lsm6ds3_data *cdata); + +#else /* CONFIG_IIO_TRIGGER */ +static inline int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata, + const struct iio_trigger_ops *trigger_ops, int irq) +{ + return 0; +} +static inline void st_lsm6ds3_deallocate_triggers(struct lsm6ds3_data *cdata, + int irq) +{ + return; +} +static inline void st_lsm6ds3_flush_works() +{ + return; +} +#endif /* CONFIG_IIO_TRIGGER */ + +#ifdef CONFIG_PM +int st_lsm6ds3_common_suspend(struct lsm6ds3_data *cdata); +int st_lsm6ds3_common_resume(struct lsm6ds3_data *cdata); +#endif /* CONFIG_PM */ + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT +int st_lsm6ds3_i2c_master_probe(struct lsm6ds3_data *cdata); +int st_lsm6ds3_i2c_master_exit(struct lsm6ds3_data *cdata); +int st_lsm6ds3_enable_passthrough(struct lsm6ds3_data *cdata, bool enable); +int st_lsm6ds3_enable_accel_dependency(struct lsm6ds3_sensor_data *sdata, + bool enable); +#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ +static inline int st_lsm6ds3_i2c_master_probe(struct lsm6ds3_data *cdata) +{ + return 0; +} +static inline int st_lsm6ds3_i2c_master_exit(struct lsm6ds3_data *cdata) +{ + return 0; +} +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + +#endif /* ST_LSM6DS3_H */ diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c new file mode 100644 index 00000000000..138868dd5ba --- /dev/null +++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c @@ -0,0 +1,452 @@ +/* + * STMicroelectronics lsm6ds3 buffer driver + * + * Copyright 2014 STMicroelectronics Inc. + * + * Denis Ciocca <denis.ciocca@st.com> + * + * Licensed under the GPL-2. + */ +#define DEBUG + +#include <linux/module.h> +#include <linux/kernel.h> +#include <linux/slab.h> +#include <linux/stat.h> +#include <linux/interrupt.h> +#include <linux/i2c.h> +#include <linux/delay.h> +#include <linux/iio/iio.h> +#include <linux/iio/buffer.h> +#include <linux/iio/trigger_consumer.h> +#include <linux/iio/triggered_buffer.h> + +#include "st_lsm6ds3.h" + +#define ST_LSM6DS3_ENABLE_AXIS 0x07 +#define ST_LSM6DS3_FIFO_DIFF_L 0x3a +#define ST_LSM6DS3_FIFO_DIFF_MASK 0x0fff +#define ST_LSM6DS3_FIFO_DATA_OUT_L 0x3e +#define ST_LSM6DS3_FIFO_DATA_OVR_2REGS 0x4000 + +static void st_lsm6ds3_push_data_with_timestamp(struct lsm6ds3_data *cdata, + u8 index, u8 *data, int64_t timestamp) +{ + int i, n = 0; + struct iio_chan_spec const *chs = cdata->indio_dev[index]->channels; + uint16_t bfch, bfchs_out = 0, bfchs_in = 0; + struct lsm6ds3_sensor_data *sdata = iio_priv(cdata->indio_dev[index]); + + for (i = 0; i < sdata->num_data_channels; i++) { + bfch = chs[i].scan_type.storagebits >> 3; + + if (test_bit(i, cdata->indio_dev[index]->active_scan_mask)) { + memcpy(&sdata->buffer_data[bfchs_out], + &data[bfchs_in], bfch); + n++; + bfchs_out += bfch; + } + + bfchs_in += bfch; + } + + if (cdata->indio_dev[index]->scan_timestamp) + *(s64 *)((u8 *)sdata->buffer_data + + ALIGN(bfchs_out, sizeof(s64))) = timestamp; + + iio_push_to_buffers(cdata->indio_dev[index], sdata->buffer_data); +} + +static void st_lsm6ds3_parse_fifo_data(struct lsm6ds3_data *cdata, u16 read_len) +{ + u16 fifo_offset = 0; + u8 gyro_sip, accel_sip; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + u8 ext0_sip, ext1_sip; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + while (fifo_offset < read_len) { + gyro_sip = cdata->gyro_samples_in_pattern; + accel_sip = cdata->accel_samples_in_pattern; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + ext0_sip = cdata->ext0_samples_in_pattern; + ext1_sip = cdata->ext1_samples_in_pattern; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + do { + if (gyro_sip > 0) { + if (cdata->gyro_samples_to_discard > 0) + cdata->gyro_samples_to_discard--; + else { + st_lsm6ds3_push_data_with_timestamp( + cdata, ST_INDIO_DEV_GYRO, + &cdata->fifo_data[fifo_offset], + cdata->gyro_timestamp); + } + + cdata->gyro_timestamp += cdata->gyro_deltatime; + fifo_offset += ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE; + gyro_sip--; + } + + if (accel_sip > 0) { + if (cdata->accel_samples_to_discard > 0) + cdata->accel_samples_to_discard--; + else { + st_lsm6ds3_push_data_with_timestamp( + cdata, ST_INDIO_DEV_ACCEL, + &cdata->fifo_data[fifo_offset], + cdata->accel_timestamp); + } + + cdata->accel_timestamp += + cdata->accel_deltatime; + fifo_offset += ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE; + accel_sip--; + } + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + if (ext0_sip > 0) { + if (cdata->ext_samples_to_discard[0] > 0) + cdata->ext_samples_to_discard[0]--; + else { + st_lsm6ds3_push_data_with_timestamp( + cdata, ST_INDIO_DEV_EXT0, + &cdata->fifo_data[fifo_offset], + cdata->ext0_timestamp); + } + + cdata->ext0_timestamp += cdata->ext0_deltatime; + fifo_offset += ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE; + ext0_sip--; + } + + if (ext1_sip > 0) { + if (cdata->ext_samples_to_discard[1] > 0) + cdata->ext_samples_to_discard[1]--; + else { + st_lsm6ds3_push_data_with_timestamp( + cdata, ST_INDIO_DEV_EXT1, + &cdata->fifo_data[fifo_offset], + cdata->ext1_timestamp); + } + + cdata->ext1_timestamp += cdata->ext1_deltatime; + fifo_offset += ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE; + ext1_sip--; + } + + } while ((accel_sip > 0) || (gyro_sip > 0) || + (ext0_sip > 0) || (ext1_sip > 0)); +#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + } while ((accel_sip > 0) || (gyro_sip > 0)); +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + } + + return; +} + +void st_lsm6ds3_read_fifo(struct lsm6ds3_data *cdata, bool check_fifo_len) +{ + int err; +#if (CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO > 0) + u16 data_remaining, data_to_read; +#endif /* CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO */ + u16 read_len = cdata->fifo_threshold, byte_in_pattern; + if (!cdata->fifo_data){ + dev_err(cdata->dev, "No cdata->fifo_data! can't use fifo"); + return; + } + if (check_fifo_len) { + err = cdata->tf->read(cdata, ST_LSM6DS3_FIFO_DIFF_L, + 2, (u8 *)&read_len, true); + if (err < 0){ + dev_dbg(cdata->dev, "could not read the fifo length. err: %i", err); + return; + } + + if (read_len & ST_LSM6DS3_FIFO_DATA_OVR_2REGS) { + dev_dbg(cdata->dev, + "DATA overrun, setting read_len to threshold\n"); + read_len = cdata->fifo_threshold; + //return; + } + + read_len &= ST_LSM6DS3_FIFO_DIFF_MASK; + read_len *= ST_LSM6DS3_BYTE_FOR_CHANNEL; + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + byte_in_pattern = (cdata->accel_samples_in_pattern + + cdata->gyro_samples_in_pattern + + cdata->ext0_samples_in_pattern + + cdata->ext1_samples_in_pattern) * + ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE; +#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + byte_in_pattern = (cdata->accel_samples_in_pattern + + cdata->gyro_samples_in_pattern) * + ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + read_len = (read_len / byte_in_pattern) * byte_in_pattern; + dev_dbg(cdata->dev, "Fifo avail: %i thresh: %i", read_len, cdata->fifo_threshold); + if (read_len > cdata->fifo_threshold) + read_len = cdata->fifo_threshold; + } + + if (read_len == 0) + return; + +#if (CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO == 0) + err = cdata->tf->read(cdata, ST_LSM6DS3_FIFO_DATA_OUT_L, + read_len, cdata->fifo_data, true); + if (err < 0) + return; +#else /* CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO */ + data_remaining = read_len; + + do { + if (data_remaining > CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO) + data_to_read = CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO; + else + data_to_read = data_remaining; + + err = cdata->tf->read(cdata, ST_LSM6DS3_FIFO_DATA_OUT_L, + data_to_read, &cdata->fifo_data[read_len - data_remaining], true); + if (err < 0) + return; + + data_remaining -= data_to_read; + } while (data_remaining > 0); +#endif /* CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO */ + + st_lsm6ds3_parse_fifo_data(cdata, read_len); +} + +static irqreturn_t st_lsm6ds3_step_counter_trigger_handler(int irq, void *p) +{ + int err; + struct timespec ts; + int64_t timestamp = 0; + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + if (!sdata->cdata->reset_steps) { + err = sdata->cdata->tf->read(sdata->cdata, + (u8)indio_dev->channels[0].address, + ST_LSM6DS3_BYTE_FOR_CHANNEL, + sdata->buffer_data, true); + if (err < 0) + goto st_lsm6ds3_step_counter_done; + + timestamp = sdata->cdata->timestamp; + } else { + memset(sdata->buffer_data, 0, ST_LSM6DS3_BYTE_FOR_CHANNEL); + get_monotonic_boottime(&ts); + timestamp = timespec_to_ns(&ts); + sdata->cdata->reset_steps = false; + } + + if (indio_dev->scan_timestamp) + *(s64 *)((u8 *)sdata->buffer_data + + ALIGN(ST_LSM6DS3_BYTE_FOR_CHANNEL, + sizeof(s64))) = timestamp; + + iio_push_to_buffers(indio_dev, sdata->buffer_data); + +st_lsm6ds3_step_counter_done: + iio_trigger_notify_done(indio_dev->trig); + return IRQ_HANDLED; +} + +static inline irqreturn_t st_lsm6ds3_handler_empty(int irq, void *p) +{ + return IRQ_HANDLED; +} + +int st_lsm6ds3_trig_set_state(struct iio_trigger *trig, bool state) +{ + int err; + struct lsm6ds3_sensor_data *sdata; + + sdata = iio_priv(iio_trigger_get_drvdata(trig)); + + err = st_lsm6ds3_set_drdy_irq(sdata, state); + + return err < 0 ? err : 0; +} + +static int st_lsm6ds3_buffer_preenable(struct iio_dev *indio_dev) +{ + int err; + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + err = st_lsm6ds3_set_enable(sdata, true); + if (err < 0) + return err; + + err = st_lsm6ds3_reconfigure_fifo(sdata->cdata, true); + if (err < 0) + return err; + + return iio_sw_buffer_preenable(indio_dev); +} + +static int st_lsm6ds3_buffer_postenable(struct iio_dev *indio_dev) +{ + int err; + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + if ((1 << sdata->sindex) & ST_LSM6DS3_USE_BUFFER) { + sdata->buffer_data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL); + if (sdata->buffer_data == NULL) + return -ENOMEM; + } + + if ((sdata->sindex == ST_INDIO_DEV_ACCEL) || + (sdata->sindex == ST_INDIO_DEV_GYRO)) { + err = st_lsm6ds3_set_axis_enable(sdata, + (u8)indio_dev->active_scan_mask[0]); + if (err < 0) + goto free_buffer_data; + } + + err = iio_triggered_buffer_postenable(indio_dev); + if (err < 0) + goto free_buffer_data; + + if (sdata->sindex == ST_INDIO_DEV_STEP_COUNTER) { + iio_trigger_poll_chained( + sdata->cdata->trig[ST_INDIO_DEV_STEP_COUNTER], 0); + } + + if (sdata->sindex == ST_INDIO_DEV_SIGN_MOTION) + sdata->cdata->sign_motion_event_ready = true; + + return 0; + +free_buffer_data: + if ((1 << sdata->sindex) & ST_LSM6DS3_USE_BUFFER) + kfree(sdata->buffer_data); + + return err; +} + +static int st_lsm6ds3_buffer_predisable(struct iio_dev *indio_dev) +{ + int err; + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + err = iio_triggered_buffer_predisable(indio_dev); + if (err < 0) + return err; + + if ((sdata->sindex == ST_INDIO_DEV_ACCEL) || + (sdata->sindex == ST_INDIO_DEV_GYRO)) { + err = st_lsm6ds3_set_axis_enable(sdata, ST_LSM6DS3_ENABLE_AXIS); + if (err < 0) + return err; + } + + if (sdata->sindex == ST_INDIO_DEV_SIGN_MOTION) + sdata->cdata->sign_motion_event_ready = false; + + err = st_lsm6ds3_set_enable(sdata, false); + if (err < 0) + return err; + + err = st_lsm6ds3_reconfigure_fifo(sdata->cdata, true); + if (err < 0) + return err; + + if ((1 << sdata->sindex) & ST_LSM6DS3_USE_BUFFER) + kfree(sdata->buffer_data); + + return 0; +} + +static const struct iio_buffer_setup_ops st_lsm6ds3_buffer_setup_ops = { + .preenable = &st_lsm6ds3_buffer_preenable, + .postenable = &st_lsm6ds3_buffer_postenable, + .predisable = &st_lsm6ds3_buffer_predisable, +}; + +int st_lsm6ds3_allocate_rings(struct lsm6ds3_data *cdata) +{ + int err; + + err = iio_triggered_buffer_setup(cdata->indio_dev[ST_INDIO_DEV_ACCEL], + &st_lsm6ds3_handler_empty, NULL, + &st_lsm6ds3_buffer_setup_ops); + if (err < 0) + return err; + + err = iio_triggered_buffer_setup(cdata->indio_dev[ST_INDIO_DEV_GYRO], + &st_lsm6ds3_handler_empty, NULL, + &st_lsm6ds3_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_accel; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_INDIO_DEV_SIGN_MOTION], + &st_lsm6ds3_handler_empty, NULL, + &st_lsm6ds3_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_gyro; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_INDIO_DEV_STEP_COUNTER], + NULL, + &st_lsm6ds3_step_counter_trigger_handler, + &st_lsm6ds3_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_sign_motion; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_INDIO_DEV_STEP_DETECTOR], + &st_lsm6ds3_handler_empty, NULL, + &st_lsm6ds3_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_step_counter; + + err = iio_triggered_buffer_setup( + cdata->indio_dev[ST_INDIO_DEV_TILT], + &st_lsm6ds3_handler_empty, NULL, + &st_lsm6ds3_buffer_setup_ops); + if (err < 0) + goto buffer_cleanup_step_detector; + + return 0; + +buffer_cleanup_step_detector: + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_INDIO_DEV_STEP_DETECTOR]); +buffer_cleanup_step_counter: + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_INDIO_DEV_STEP_COUNTER]); +buffer_cleanup_sign_motion: + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_INDIO_DEV_SIGN_MOTION]); +buffer_cleanup_gyro: + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_INDIO_DEV_GYRO]); +buffer_cleanup_accel: + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_INDIO_DEV_ACCEL]); + return err; +} + +void st_lsm6ds3_deallocate_rings(struct lsm6ds3_data *cdata) +{ + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_INDIO_DEV_TILT]); + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_INDIO_DEV_STEP_DETECTOR]); + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_INDIO_DEV_STEP_COUNTER]); + iio_triggered_buffer_cleanup( + cdata->indio_dev[ST_INDIO_DEV_SIGN_MOTION]); + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_INDIO_DEV_ACCEL]); + iio_triggered_buffer_cleanup(cdata->indio_dev[ST_INDIO_DEV_GYRO]); +} + +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>"); +MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 buffer driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c new file mode 100644 index 00000000000..bbb024e3a4d --- /dev/null +++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c @@ -0,0 +1,2383 @@ +/* + * STMicroelectronics lsm6ds3 core driver + * + * Copyright 2014 STMicroelectronics Inc. + * + * Denis Ciocca <denis.ciocca@st.com> + * + * Licensed under the GPL-2. + */ + +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/slab.h> +#include <linux/errno.h> +#include <linux/types.h> +#include <linux/mutex.h> +#include <linux/interrupt.h> +#include <linux/gpio.h> +#include <linux/delay.h> +#include <linux/of.h> +#include <linux/irq.h> +#include <linux/iio/iio.h> +#include <linux/iio/sysfs.h> +#include <linux/iio/trigger.h> +#include <linux/iio/buffer.h> +#include <linux/iio/events.h> +#include <asm/unaligned.h> +#include <linux/wakelock.h> +#include <linux/iio/common/st_sensors.h> +#include "st_lsm6ds3.h" + +#define MS_TO_NS(msec) ((msec) * 1000 * 1000) + +#ifndef MAX +#define MAX(a, b) (((a) > (b)) ? (a) : (b)) +#endif + +#define MIN_BNZ(a, b) (((a) < (b)) ? ((a == 0) ? \ + (b) : (a)) : ((b == 0) ? \ + (a) : (b))) + +/* COMMON VALUES FOR ACCEL-GYRO SENSORS */ +#define ST_LSM6DS3_WAI_ADDRESS 0x0f +#define ST_LSM6DS3_WAI_EXP 0x69 +#define ST_LSM6DS3_AXIS_EN_MASK 0x38 +#define ST_LSM6DS3_INT1_ADDR 0x0d +#define ST_LSM6DS3_INT2_ADDR 0x0e +#define ST_LSM6DS3_MD1_ADDR 0x5e +#define ST_LSM6DS3_ODR_LIST_NUM 5 +#define ST_LSM6DS3_ODR_POWER_OFF_VAL 0x00 +#define ST_LSM6DS3_ODR_26HZ_VAL 0x02 +#define ST_LSM6DS3_ODR_52HZ_VAL 0x03 +#define ST_LSM6DS3_ODR_104HZ_VAL 0x04 +#define ST_LSM6DS3_ODR_208HZ_VAL 0x05 +#define ST_LSM6DS3_ODR_416HZ_VAL 0x06 +#define ST_LSM6DS3_FS_LIST_NUM 4 +#define ST_LSM6DS3_BDU_ADDR 0x12 +#define ST_LSM6DS3_BDU_MASK 0x40 +#define ST_LSM6DS3_EN_BIT 0x01 +#define ST_LSM6DS3_DIS_BIT 0x00 +#define ST_LSM6DS3_FUNC_EN_ADDR 0x19 +#define ST_LSM6DS3_FUNC_EN_MASK 0x04 +#define ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR 0x01 +#define ST_LSM6DS3_FUNC_CFG_ACCESS_MASK 0x01 +#define ST_LSM6DS3_FUNC_CFG_ACCESS_MASK2 0x04 +#define ST_LSM6DS3_FUNC_CFG_REG2_MASK 0x80 +#define ST_LSM6DS3_FUNC_CFG_START1_ADDR 0x62 +#define ST_LSM6DS3_FUNC_CFG_START2_ADDR 0x63 +#define ST_LSM6DS3_PASS_THROUGH_MODE_ADDR 0x1a +#define ST_LSM6DS3_PASS_THROUGH_MODE_MASK 0x04 +#define ST_LSM6DS3_INTER_PULLUP_ADDR 0x1a +#define ST_LSM6DS3_INTER_PULLUP_MASK 0x08 +#define ST_LSM6DS3_SENSORHUB_ADDR 0x1a +#define ST_LSM6DS3_SENSORHUB_MASK 0x01 +#define ST_LSM6DS3_STARTCONFIG_ADDR 0x1a +#define ST_LSM6DS3_STARTCONFIG_MASK 0x10 +#define ST_LSM6DS3_SELFTEST_ADDR 0x14 +#define ST_LSM6DS3_SELFTEST_ACCEL_MASK 0x03 +#define ST_LSM6DS3_SELFTEST_GYRO_MASK 0x0c +#define ST_LSM6DS3_SELF_TEST_DISABLED_VAL 0x00 +#define ST_LSM6DS3_SELF_TEST_POS_SIGN_VAL 0x01 +#define ST_LSM6DS3_SELF_TEST_NEG_ACCEL_SIGN_VAL 0x02 +#define ST_LSM6DS3_SELF_TEST_NEG_GYRO_SIGN_VAL 0x03 +#define ST_LSM6DS3_LIR_ADDR 0x58 +#define ST_LSM6DS3_LIR_MASK 0x01 +#define ST_LSM6DS3_TIMER_EN_ADDR 0x58 +#define ST_LSM6DS3_TIMER_EN_MASK 0x80 +#define ST_LSM6DS3_PEDOMETER_EN_ADDR 0x58 +#define ST_LSM6DS3_PEDOMETER_EN_MASK 0x40 +#define ST_LSM6DS3_INT2_ON_INT1_ADDR 0x13 +#define ST_LSM6DS3_INT2_ON_INT1_MASK 0x20 +#define ST_LSM6DS3_MIN_DURATION_MS 1638 +#define ST_LSM6DS3_ROUNDING_ADDR 0x16 +#define ST_LSM6DS3_ROUNDING_MASK 0x04 +#define ST_LSM6DS3_FIFO_MODE_ADDR 0x0a +#define ST_LSM6DS3_FIFO_MODE_MASK 0x07 +#define ST_LSM6DS3_FIFO_MODE_BYPASS 0x00 +#define ST_LSM6DS3_FIFO_MODE_CONTINUOS 0x06 +#define ST_LSM6DS3_FIFO_THRESHOLD_IRQ_MASK 0x08 +#define ST_LSM6DS3_FIFO_ODR_ADDR 0x0a +#define ST_LSM6DS3_FIFO_ODR_MASK 0x78 +#define ST_LSM6DS3_FIFO_ODR_MAX 0x04 // prev max:0x08 +#define ST_LSM6DS3_FIFO_ODR_OFF 0x00 +#define ST_LSM6DS3_FIFO_DECIMATOR_ADDR 0x08 +#define ST_LSM6DS3_FIFO_ACCEL_DECIMATOR_MASK 0x07 +#define ST_LSM6DS3_FIFO_GYRO_DECIMATOR_MASK 0x38 +#define ST_LSM6DS3_FIFO_DECIMATOR2_ADDR 0x09 +#define ST_LSM6DS3_FIFO_DS3_DECIMATOR_MASK 0x07 +#define ST_LSM6DS3_FIFO_DS4_DECIMATOR_MASK 0x38 +#define ST_LSM6DS3_FIFO_THR_L_ADDR 0x06 +#define ST_LSM6DS3_FIFO_THR_H_ADDR 0x07 +#define ST_LSM6DS3_FIFO_THR_H_MASK 0x0f +#define ST_LSM6DS3_FIFO_THR_IRQ_MASK 0x08 +#define ST_LSM6DS3_RESET_ADDR 0x12 +#define ST_LSM6DS3_RESET_MASK 0x01 +#define ST_LSM6DS3_MAX_FIFO_SIZE 8192 +#define ST_LSM6DS3_MAX_FIFO_LENGHT (ST_LSM6DS3_MAX_FIFO_SIZE / \ + ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE) + +/* CUSTOM VALUES FOR ACCEL SENSOR */ +#define ST_LSM6DS3_ACCEL_ODR_ADDR 0x10 +#define ST_LSM6DS3_ACCEL_ODR_MASK 0xf0 +#define ST_LSM6DS3_ACCEL_FS_ADDR 0x10 +#define ST_LSM6DS3_ACCEL_FS_MASK 0x0c +#define ST_LSM6DS3_ACCEL_FS_2G_VAL 0x00 +#define ST_LSM6DS3_ACCEL_FS_4G_VAL 0x02 +#define ST_LSM6DS3_ACCEL_FS_8G_VAL 0x03 +#define ST_LSM6DS3_ACCEL_FS_16G_VAL 0x01 +#define ST_LSM6DS3_ACCEL_FS_2G_GAIN IIO_G_TO_M_S_2(61) +#define ST_LSM6DS3_ACCEL_FS_4G_GAIN IIO_G_TO_M_S_2(122) +#define ST_LSM6DS3_ACCEL_FS_8G_GAIN IIO_G_TO_M_S_2(244) +#define ST_LSM6DS3_ACCEL_FS_16G_GAIN IIO_G_TO_M_S_2(488) +#define ST_LSM6DS3_ACCEL_OUT_X_L_ADDR 0x28 +#define ST_LSM6DS3_ACCEL_OUT_Y_L_ADDR 0x2a +#define ST_LSM6DS3_ACCEL_OUT_Z_L_ADDR 0x2c +#define ST_LSM6DS3_ACCEL_AXIS_EN_ADDR 0x18 +#define ST_LSM6DS3_ACCEL_STD 3 + +/* CUSTOM VALUES FOR GYRO SENSOR */ +#define ST_LSM6DS3_GYRO_ODR_ADDR 0x11 +#define ST_LSM6DS3_GYRO_ODR_MASK 0xf0 +#define ST_LSM6DS3_GYRO_FS_ADDR 0x11 +#define ST_LSM6DS3_GYRO_FS_MASK 0x0c +#define ST_LSM6DS3_GYRO_FS_245_VAL 0x00 +#define ST_LSM6DS3_GYRO_FS_500_VAL 0x01 +#define ST_LSM6DS3_GYRO_FS_1000_VAL 0x02 +#define ST_LSM6DS3_GYRO_FS_2000_VAL 0x03 +#define ST_LSM6DS3_GYRO_FS_245_GAIN IIO_DEGREE_TO_RAD(4375) +#define ST_LSM6DS3_GYRO_FS_500_GAIN IIO_DEGREE_TO_RAD(8750) +#define ST_LSM6DS3_GYRO_FS_1000_GAIN IIO_DEGREE_TO_RAD(17500) +#define ST_LSM6DS3_GYRO_FS_2000_GAIN IIO_DEGREE_TO_RAD(70000) +#define ST_LSM6DS3_GYRO_OUT_X_L_ADDR 0x22 +#define ST_LSM6DS3_GYRO_OUT_Y_L_ADDR 0x24 +#define ST_LSM6DS3_GYRO_OUT_Z_L_ADDR 0x26 +#define ST_LSM6DS3_GYRO_AXIS_EN_ADDR 0x19 +#define ST_LSM6DS3_GYRO_STD 6 + +/* CUSTOM VALUES FOR SIGNIFICANT MOTION SENSOR */ +#define ST_LSM6DS3_SIGN_MOTION_EN_ADDR 0x19 +#define ST_LSM6DS3_SIGN_MOTION_EN_MASK 0x01 + +/* CUSTOM VALUES FOR STEP DETECTOR SENSOR */ +#define ST_LSM6DS3_STEP_DETECTOR_DRDY_IRQ_MASK 0x80 + +/* CUSTOM VALUES FOR STEP COUNTER SENSOR */ +#define ST_LSM6DS3_STEP_COUNTER_DRDY_IRQ_MASK 0x80 +#define ST_LSM6DS3_STEP_COUNTER_OUT_L_ADDR 0x4b +#define ST_LSM6DS3_STEP_COUNTER_RES_ADDR 0x19 +#define ST_LSM6DS3_STEP_COUNTER_RES_MASK 0x06 +#define ST_LSM6DS3_STEP_COUNTER_RES_ALL_EN 0x03 +#define ST_LSM6DS3_STEP_COUNTER_RES_FUNC_EN 0x02 +#define ST_LSM6DS3_STEP_COUNTER_DURATION_ADDR 0x15 + +/* CUSTOM VALUES FOR TILT SENSOR */ +#define ST_LSM6DS3_TILT_EN_ADDR 0x58 +#define ST_LSM6DS3_TILT_EN_MASK 0x20 +#define ST_LSM6DS3_TILT_DRDY_IRQ_MASK 0x02 + +/* 6d Constants */ + + +#define ST_LSM6DS3_6D_MD1_INT_MASK 0x04 +#define ST_LSM6DS3_SINGTAP_MD1_INT_MASK 0x40 + +#define ST_LSM6DS3_ACCEL_SUFFIX_NAME "accel" +#define ST_LSM6DS3_GYRO_SUFFIX_NAME "gyro" +#define ST_LSM6DS3_STEP_COUNTER_SUFFIX_NAME "step_c" +#define ST_LSM6DS3_STEP_DETECTOR_SUFFIX_NAME "step_d" +#define ST_LSM6DS3_SIGN_MOTION_SUFFIX_NAME "sign_motion" +#define ST_LSM6DS3_TILT_SUFFIX_NAME "tilt" + +#define ST_LSM6DS3_DEV_ATTR_SAMP_FREQ() \ + IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, \ + st_lsm6ds3_sysfs_get_sampling_frequency, \ + st_lsm6ds3_sysfs_set_sampling_frequency) + +#define ST_LSM6DS3_DEV_ATTR_SAMP_FREQ_AVAIL() \ + IIO_DEV_ATTR_SAMP_FREQ_AVAIL( \ + st_lsm6ds3_sysfs_sampling_frequency_avail) + +#define ST_LSM6DS3_DEV_ATTR_SCALE_AVAIL(name) \ + IIO_DEVICE_ATTR(name, S_IRUGO, \ + st_lsm6ds3_sysfs_scale_avail, NULL , 0); + +static struct st_lsm6ds3_selftest_table { + char *string_mode; + u8 accel_value; + u8 gyro_value; + u8 accel_mask; + u8 gyro_mask; +} st_lsm6ds3_selftest_table[] = { + [0] = { + .string_mode = "disabled", + .accel_value = ST_LSM6DS3_SELF_TEST_DISABLED_VAL, + .gyro_value = ST_LSM6DS3_SELF_TEST_DISABLED_VAL, + }, + [1] = { + .string_mode = "positive-sign", + .accel_value = ST_LSM6DS3_SELF_TEST_POS_SIGN_VAL, + .gyro_value = ST_LSM6DS3_SELF_TEST_POS_SIGN_VAL + }, + [2] = { + .string_mode = "negative-sign", + .accel_value = ST_LSM6DS3_SELF_TEST_NEG_ACCEL_SIGN_VAL, + .gyro_value = ST_LSM6DS3_SELF_TEST_NEG_GYRO_SIGN_VAL + }, +}; + +struct st_lsm6ds3_odr_reg { + unsigned int hz; + u8 value; +}; + +static struct st_lsm6ds3_odr_table { + u8 addr[2]; + u8 mask[2]; + struct st_lsm6ds3_odr_reg odr_avl[ST_LSM6DS3_ODR_LIST_NUM]; +} st_lsm6ds3_odr_table = { + .addr[ST_INDIO_DEV_ACCEL] = ST_LSM6DS3_ACCEL_ODR_ADDR, + .mask[ST_INDIO_DEV_ACCEL] = ST_LSM6DS3_ACCEL_ODR_MASK, + .addr[ST_INDIO_DEV_GYRO] = ST_LSM6DS3_GYRO_ODR_ADDR, + .mask[ST_INDIO_DEV_GYRO] = ST_LSM6DS3_GYRO_ODR_MASK, + //hack , remove + .odr_avl[0] = { .hz = 416, .value = ST_LSM6DS3_ODR_416HZ_VAL }, + .odr_avl[1] = { .hz = 52, .value = ST_LSM6DS3_ODR_52HZ_VAL }, + .odr_avl[2] = { .hz = 104, .value = ST_LSM6DS3_ODR_104HZ_VAL }, + .odr_avl[3] = { .hz = 208, .value = ST_LSM6DS3_ODR_208HZ_VAL }, + .odr_avl[4] = { .hz = 416, .value = ST_LSM6DS3_ODR_416HZ_VAL }, +}; + +struct st_lsm6ds3_fs_reg { + unsigned int gain; + u8 value; +}; + +static struct st_lsm6ds3_fs_table { + u8 addr; + u8 mask; + struct st_lsm6ds3_fs_reg fs_avl[ST_LSM6DS3_FS_LIST_NUM]; +} st_lsm6ds3_fs_table[ST_INDIO_DEV_NUM] = { + [ST_INDIO_DEV_ACCEL] = { + .addr = ST_LSM6DS3_ACCEL_FS_ADDR, + .mask = ST_LSM6DS3_ACCEL_FS_MASK, + .fs_avl[0] = { .gain = ST_LSM6DS3_ACCEL_FS_2G_GAIN, + .value = ST_LSM6DS3_ACCEL_FS_2G_VAL }, + .fs_avl[1] = { .gain = ST_LSM6DS3_ACCEL_FS_4G_GAIN, + .value = ST_LSM6DS3_ACCEL_FS_4G_VAL }, + .fs_avl[2] = { .gain = ST_LSM6DS3_ACCEL_FS_8G_GAIN, + .value = ST_LSM6DS3_ACCEL_FS_8G_VAL }, + .fs_avl[3] = { .gain = ST_LSM6DS3_ACCEL_FS_16G_GAIN, + .value = ST_LSM6DS3_ACCEL_FS_16G_VAL }, + }, + [ST_INDIO_DEV_GYRO] = { + .addr = ST_LSM6DS3_GYRO_FS_ADDR, + .mask = ST_LSM6DS3_GYRO_FS_MASK, + .fs_avl[0] = { .gain = ST_LSM6DS3_GYRO_FS_245_GAIN, + .value = ST_LSM6DS3_GYRO_FS_245_VAL }, + .fs_avl[1] = { .gain = ST_LSM6DS3_GYRO_FS_500_GAIN, + .value = ST_LSM6DS3_GYRO_FS_500_VAL }, + .fs_avl[2] = { .gain = ST_LSM6DS3_GYRO_FS_1000_GAIN, + .value = ST_LSM6DS3_GYRO_FS_1000_VAL }, + .fs_avl[3] = { .gain = ST_LSM6DS3_GYRO_FS_2000_GAIN, + .value = ST_LSM6DS3_GYRO_FS_2000_VAL }, + } +}; + +static const struct iio_chan_spec st_lsm6ds3_accel_ch[] = { + ST_LSM6DS3_LSM_CHANNELS(IIO_ACCEL, 1, 0, IIO_MOD_X, IIO_LE, + 16, 16, ST_LSM6DS3_ACCEL_OUT_X_L_ADDR, 's'), + ST_LSM6DS3_LSM_CHANNELS(IIO_ACCEL, 1, 1, IIO_MOD_Y, IIO_LE, + 16, 16, ST_LSM6DS3_ACCEL_OUT_Y_L_ADDR, 's'), + ST_LSM6DS3_LSM_CHANNELS(IIO_ACCEL, 1, 2, IIO_MOD_Z, IIO_LE, + 16, 16, ST_LSM6DS3_ACCEL_OUT_Z_L_ADDR, 's'), + IIO_CHAN_SOFT_TIMESTAMP(3) +}; + +static const struct iio_chan_spec st_lsm6ds3_gyro_ch[] = { + ST_LSM6DS3_LSM_CHANNELS(IIO_ANGL_VEL, 1, 0, IIO_MOD_X, IIO_LE, + 16, 16, ST_LSM6DS3_GYRO_OUT_X_L_ADDR, 's'), + ST_LSM6DS3_LSM_CHANNELS(IIO_ANGL_VEL, 1, 1, IIO_MOD_Y, IIO_LE, + 16, 16, ST_LSM6DS3_GYRO_OUT_Y_L_ADDR, 's'), + ST_LSM6DS3_LSM_CHANNELS(IIO_ANGL_VEL, 1, 2, IIO_MOD_Z, IIO_LE, + 16, 16, ST_LSM6DS3_GYRO_OUT_Z_L_ADDR, 's'), + IIO_CHAN_SOFT_TIMESTAMP(3) +}; + +static const struct iio_chan_spec st_lsm6ds3_sign_motion_ch[] = { + { + .type = IIO_SIGN_MOTION, + .channel = 0, + .modified = 0, + .event_mask = IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING), + }, + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +static const struct iio_chan_spec st_lsm6ds3_step_c_ch[] = { + { + .type = IIO_STEP_COUNTER, + .modified = 0, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), + .address = ST_LSM6DS3_STEP_COUNTER_OUT_L_ADDR, + .scan_type = { + .sign = 'u', + .realbits = 16, + .storagebits = 16, + .endianness = IIO_LE, + }, + }, + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +static const struct iio_chan_spec st_lsm6ds3_step_d_ch[] = { + { + .type = IIO_STEP_DETECTOR, + .channel = 0, + .modified = 0, + .event_mask = IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING), + }, + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +static const struct iio_chan_spec st_lsm6ds3_tilt_ch[] = { + { + .type = IIO_TILT, + .channel = 0, + .modified = 0, + .event_mask = IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING), + }, + IIO_CHAN_SOFT_TIMESTAMP(1) +}; + +int st_lsm6ds3_write_data_with_mask(struct lsm6ds3_data *cdata, + u8 reg_addr, u8 mask, u8 data, bool b_lock) +{ + int err; + u8 new_data = 0x00, old_data = 0x00; + + err = cdata->tf->read(cdata, reg_addr, 1, &old_data, b_lock); + if (err < 0) + return err; + + new_data = ((old_data & (~mask)) | ((data << __ffs(mask)) & mask)); + + if (new_data == old_data) + return 1; + + return cdata->tf->write(cdata, reg_addr, 1, &new_data, b_lock); +} +EXPORT_SYMBOL(st_lsm6ds3_write_data_with_mask); + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT +static int st_lsm6ds3_enable_sensor_hub(struct lsm6ds3_data *cdata, bool enable) +{ + int err; + + if (enable) { + if (cdata->sensors_enabled & ST_LSM6DS3_EXT_SENSORS) { + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_STARTCONFIG_ADDR, + ST_LSM6DS3_STARTCONFIG_MASK, + ST_LSM6DS3_DIS_BIT, true); + if (err < 0) + return err; + } + + if (cdata->sensors_enabled & ST_LSM6DS3_EXTRA_DEPENDENCY) { + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_FUNC_EN_ADDR, + ST_LSM6DS3_FUNC_EN_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + } + + if (cdata->sensors_enabled & ST_LSM6DS3_EXT_SENSORS) { + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_SENSORHUB_ADDR, + ST_LSM6DS3_SENSORHUB_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + } + } else { + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_STARTCONFIG_ADDR, + ST_LSM6DS3_STARTCONFIG_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + + usleep_range(1500, 4000); + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_SENSORHUB_ADDR, + ST_LSM6DS3_SENSORHUB_MASK, + ST_LSM6DS3_DIS_BIT, true); + if (err < 0) + return err; + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_FUNC_EN_ADDR, + ST_LSM6DS3_FUNC_EN_MASK, + ST_LSM6DS3_DIS_BIT, true); + if (err < 0) + return err; + } + + return 0; +} + +int st_lsm6ds3_enable_passthrough(struct lsm6ds3_data *cdata, bool enable) +{ + int err; + u8 reg_value; + + if (enable) + reg_value = ST_LSM6DS3_EN_BIT; + else + reg_value = ST_LSM6DS3_DIS_BIT; + + if (enable) { + err = st_lsm6ds3_enable_sensor_hub(cdata, false); + if (err < 0) + return err; + +#ifdef CONFIG_ST_LSM6DS3_ENABLE_INTERNAL_PULLUP + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_INTER_PULLUP_ADDR, + ST_LSM6DS3_INTER_PULLUP_MASK, + ST_LSM6DS3_DIS_BIT, true); + if (err < 0) + return err; +#endif /* CONFIG_ST_LSM6DS3_ENABLE_INTERNAL_PULLUP */ + } + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_PASS_THROUGH_MODE_ADDR, + ST_LSM6DS3_PASS_THROUGH_MODE_MASK, + reg_value, enable); + if (err < 0) + return err; + + if (!enable) { +#ifdef CONFIG_ST_LSM6DS3_ENABLE_INTERNAL_PULLUP + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_INTER_PULLUP_ADDR, + ST_LSM6DS3_INTER_PULLUP_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; +#endif /* CONFIG_ST_LSM6DS3_ENABLE_INTERNAL_PULLUP */ + + err = st_lsm6ds3_enable_sensor_hub(cdata, true); + if (err < 0) + return err; + } + + return 0; +} +EXPORT_SYMBOL(st_lsm6ds3_enable_passthrough); +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + +static int st_lsm6ds3_set_fifo_enable(struct lsm6ds3_data *cdata, bool status) +{ + int err; + u8 reg_value; + struct timespec ts; + + if (status) + reg_value = ST_LSM6DS3_FIFO_ODR_MAX; + else + reg_value = ST_LSM6DS3_FIFO_ODR_OFF; + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_FIFO_ODR_ADDR, + ST_LSM6DS3_FIFO_ODR_MASK, + reg_value, true); + if (err < 0) + return err; + + get_monotonic_boottime(&ts); + cdata->gyro_timestamp = timespec_to_ns(&ts); + cdata->accel_timestamp = cdata->gyro_timestamp; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + cdata->ext0_timestamp = cdata->gyro_timestamp; + cdata->ext1_timestamp = cdata->gyro_timestamp; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + return 0; +} + +int st_lsm6ds3_set_fifo_mode(struct lsm6ds3_data *cdata, enum fifo_mode fm) +{ + int err; + u8 reg_value; + bool enable_fifo; + + switch (fm) { + case BYPASS: + reg_value = ST_LSM6DS3_FIFO_MODE_BYPASS; + enable_fifo = false; + break; + case CONTINUOS: + reg_value = ST_LSM6DS3_FIFO_MODE_CONTINUOS; + enable_fifo = true; + break; + default: + return -EINVAL; + } + + err = st_lsm6ds3_set_fifo_enable(cdata, enable_fifo); + if (err < 0) + return err; + + return st_lsm6ds3_write_data_with_mask(cdata, ST_LSM6DS3_FIFO_MODE_ADDR, + ST_LSM6DS3_FIFO_MODE_MASK, reg_value, true); +} +EXPORT_SYMBOL(st_lsm6ds3_set_fifo_mode); + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT +static int st_lsm6ds3_force_accel_odr(struct lsm6ds3_sensor_data *sdata, + unsigned int odr) +{ + int i; + + for (i = 0; i < ST_LSM6DS3_ODR_LIST_NUM; i++) { + if (st_lsm6ds3_odr_table.odr_avl[i].hz == odr) + break; + } + if (i == ST_LSM6DS3_ODR_LIST_NUM) + return -EINVAL; + + return st_lsm6ds3_write_data_with_mask(sdata->cdata, + st_lsm6ds3_odr_table.addr[sdata->sindex], + st_lsm6ds3_odr_table.mask[sdata->sindex], + st_lsm6ds3_odr_table.odr_avl[i].value, true); +} +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + +int st_lsm6ds3_set_fifo_decimators_and_threshold(struct lsm6ds3_data *cdata) +{ + int err; + struct iio_dev *indio_dev; + u16 min_num_pattern, max_num_pattern; + unsigned int min_odr = 416, max_odr = 0; + u8 accel_decimator = 0, gyro_decimator = 0; + u16 num_pattern_accel = 0, num_pattern_gyro = 0; + struct lsm6ds3_sensor_data *sdata_accel, *sdata_gyro; + u16 fifo_len, fifo_threshold, fifo_len_accel = 0, fifo_len_gyro = 0; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + int i; + bool force_accel_odr = false; + u8 ext0_decimator = 0, ext1_decimator = 0; + u16 num_pattern_ext1 = 0, fifo_len_ext1 = 0; + u16 num_pattern_ext0 = 0, fifo_len_ext0 = 0; + struct lsm6ds3_sensor_data *sdata_ext0 = NULL, *sdata_ext1 = NULL; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + indio_dev = cdata->indio_dev[ST_INDIO_DEV_ACCEL]; + sdata_accel = iio_priv(indio_dev); + if ((1 << sdata_accel->sindex) & cdata->sensors_enabled) { + if (min_odr > sdata_accel->c_odr) + min_odr = sdata_accel->c_odr; + + if (max_odr < sdata_accel->c_odr) + max_odr = sdata_accel->c_odr; + + fifo_len_accel = (indio_dev->buffer->length / 2); + } + + indio_dev = cdata->indio_dev[ST_INDIO_DEV_GYRO]; + sdata_gyro = iio_priv(indio_dev); + if ((1 << sdata_gyro->sindex) & cdata->sensors_enabled) { + if (min_odr > sdata_gyro->c_odr) + min_odr = sdata_gyro->c_odr; + + if (max_odr < sdata_gyro->c_odr) + max_odr = sdata_gyro->c_odr; + + fifo_len_gyro = (indio_dev->buffer->length / 2); + } + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + if (cdata->ext0_available) { + indio_dev = cdata->indio_dev[ST_INDIO_DEV_EXT0]; + sdata_ext0 = iio_priv(indio_dev); + if ((1 << sdata_ext0->sindex) & cdata->sensors_enabled) { + if (min_odr > sdata_ext0->c_odr) + min_odr = sdata_ext0->c_odr; + + if (max_odr < sdata_ext0->c_odr) { + force_accel_odr = true; + max_odr = sdata_ext0->c_odr; + } + + fifo_len_ext0 = (indio_dev->buffer->length / 2); + } + } + + if (cdata->ext1_available) { + indio_dev = cdata->indio_dev[ST_INDIO_DEV_EXT1]; + sdata_ext1 = iio_priv(indio_dev); + if ((1 << sdata_ext1->sindex) & cdata->sensors_enabled) { + if (min_odr > sdata_ext1->c_odr) + min_odr = sdata_ext1->c_odr; + + if (max_odr < sdata_ext1->c_odr) { + force_accel_odr = true; + max_odr = sdata_ext1->c_odr; + } + + fifo_len_ext1 = (indio_dev->buffer->length / 2); + } + } + + if (force_accel_odr) { + err = st_lsm6ds3_force_accel_odr(sdata_accel, max_odr); + if (err < 0) + return err; + } else { + for (i = 0; i < ST_LSM6DS3_ODR_LIST_NUM; i++) { + if (st_lsm6ds3_odr_table.odr_avl[i].hz == + sdata_accel->c_odr) + break; + } + if (i == ST_LSM6DS3_ODR_LIST_NUM) + return -EINVAL; + + if (cdata->sensors_enabled & (1 << sdata_accel->sindex)) { + cdata->accel_samples_to_discard = ST_LSM6DS3_ACCEL_STD; + + err = st_lsm6ds3_write_data_with_mask(cdata, + st_lsm6ds3_odr_table.addr[sdata_accel->sindex], + st_lsm6ds3_odr_table.mask[sdata_accel->sindex], + st_lsm6ds3_odr_table.odr_avl[i].value, true); + if (err < 0) + return err; + } + } +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + if ((1 << sdata_accel->sindex) & cdata->sensors_enabled) { + cdata->accel_samples_in_pattern = + (sdata_accel->c_odr / min_odr); + num_pattern_accel = MAX(fifo_len_accel / + cdata->accel_samples_in_pattern, 1); + //TODO: this needs to reflect fifo odr rates and not c_odr + cdata->accel_deltatime = (1000000000ULL / sdata_accel->c_odr); + accel_decimator = max_odr / sdata_accel->c_odr; + } else + cdata->accel_samples_in_pattern = 0; + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_FIFO_DECIMATOR_ADDR, + ST_LSM6DS3_FIFO_ACCEL_DECIMATOR_MASK, + accel_decimator, true); + if (err < 0) + return err; + + if ((1 << sdata_gyro->sindex) & cdata->sensors_enabled) { + cdata->gyro_samples_in_pattern = (sdata_gyro->c_odr / min_odr); + num_pattern_gyro = MAX(fifo_len_gyro / + cdata->gyro_samples_in_pattern, 1); + cdata->gyro_deltatime = (1000000000ULL / sdata_gyro->c_odr); + gyro_decimator = max_odr / sdata_gyro->c_odr; + } else + cdata->gyro_samples_in_pattern = 0; + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_FIFO_DECIMATOR_ADDR, + ST_LSM6DS3_FIFO_GYRO_DECIMATOR_MASK, + gyro_decimator, true); + if (err < 0) + return err; + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + if (cdata->ext0_available) { + if ((1 << sdata_ext0->sindex) & cdata->sensors_enabled) { + cdata->ext0_samples_in_pattern = + (sdata_ext0->c_odr / min_odr); + num_pattern_ext0 = MAX(fifo_len_ext0 / + cdata->ext0_samples_in_pattern, 1); + cdata->ext0_deltatime = + (1000000000ULL / sdata_ext0->c_odr); + ext0_decimator = max_odr / sdata_ext0->c_odr; + } else + cdata->ext0_samples_in_pattern = 0; + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_FIFO_DECIMATOR2_ADDR, + ST_LSM6DS3_FIFO_DS3_DECIMATOR_MASK, + ext0_decimator, true); + if (err < 0) + return err; + } + + if (cdata->ext1_available) { + if ((1 << sdata_ext1->sindex) & cdata->sensors_enabled) { + cdata->ext1_samples_in_pattern = + (sdata_ext1->c_odr / min_odr); + num_pattern_ext1 = MAX(fifo_len_ext1 / + cdata->ext1_samples_in_pattern, 1); + cdata->ext1_deltatime = + (1000000000ULL / sdata_ext1->c_odr); + ext1_decimator = max_odr / sdata_ext1->c_odr; + } else + cdata->ext1_samples_in_pattern = 0; + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_FIFO_DECIMATOR2_ADDR, + ST_LSM6DS3_FIFO_DS4_DECIMATOR_MASK, + ext1_decimator, true); + if (err < 0) + return err; + } +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + min_num_pattern = MIN_BNZ(MIN_BNZ(MIN_BNZ(num_pattern_gyro, + num_pattern_accel), num_pattern_ext0), num_pattern_ext1); +#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + min_num_pattern = MIN_BNZ(num_pattern_gyro, num_pattern_accel); +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + if ((cdata->accel_samples_in_pattern + + cdata->gyro_samples_in_pattern + + cdata->ext0_samples_in_pattern + + cdata->ext1_samples_in_pattern) > 0) { + max_num_pattern = ST_LSM6DS3_MAX_FIFO_SIZE / + ((cdata->accel_samples_in_pattern + + cdata->gyro_samples_in_pattern + + cdata->ext0_samples_in_pattern + + cdata->ext1_samples_in_pattern) * + ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE); + + if (min_num_pattern > max_num_pattern) + min_num_pattern = max_num_pattern; + } + + fifo_len = (cdata->accel_samples_in_pattern + + cdata->gyro_samples_in_pattern + + cdata->ext0_samples_in_pattern + + cdata->ext1_samples_in_pattern) * + min_num_pattern * ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE; +#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + if ((cdata->accel_samples_in_pattern + + cdata->gyro_samples_in_pattern) > 0) { + max_num_pattern = ST_LSM6DS3_MAX_FIFO_SIZE / + ((cdata->accel_samples_in_pattern + + cdata->gyro_samples_in_pattern) * + ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE); + + if (min_num_pattern > max_num_pattern) + min_num_pattern = max_num_pattern; + } + + fifo_len = (cdata->accel_samples_in_pattern + + cdata->gyro_samples_in_pattern) * + min_num_pattern * ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + if (fifo_len > 0) { + fifo_threshold = fifo_len / 2; + dev_info(cdata->dev,"setting FIFO length/threshold: 0x%x 0x%x ", fifo_len, fifo_threshold); + + err = cdata->tf->write(cdata, ST_LSM6DS3_FIFO_THR_L_ADDR, + 1, (u8 *)&fifo_threshold, true); + if (err < 0) + return err; + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_FIFO_THR_H_ADDR, + ST_LSM6DS3_FIFO_THR_H_MASK, + *(((u8 *)&fifo_threshold) + 1), true); + if (err < 0) + return err; + + cdata->fifo_threshold = fifo_len; + } + kfree(cdata->fifo_data); + cdata->fifo_data = 0; + + if (fifo_len > 0) { + cdata->fifo_data = kmalloc(cdata->fifo_threshold, GFP_KERNEL); + if (!cdata->fifo_data) + return -ENOMEM; + } + + return fifo_len; +} +EXPORT_SYMBOL(st_lsm6ds3_set_fifo_decimators_and_threshold); + +int st_lsm6ds3_reconfigure_fifo(struct lsm6ds3_data *cdata, + bool disable_irq_and_flush) +{ + int err, fifo_len; + + if (disable_irq_and_flush) { + disable_irq(cdata->irq); + st_lsm6ds3_flush_works(); + } + + mutex_lock(&cdata->fifo_lock); + + st_lsm6ds3_read_fifo(cdata, true); + + err = st_lsm6ds3_set_fifo_mode(cdata, BYPASS); + if (err < 0) + goto reconfigure_fifo_irq_restore; + + fifo_len = st_lsm6ds3_set_fifo_decimators_and_threshold(cdata); + if (fifo_len < 0) { + err = fifo_len; + goto reconfigure_fifo_irq_restore; + } + + if (fifo_len > 0) { + err = st_lsm6ds3_set_fifo_mode(cdata, CONTINUOS); + if (err < 0) + goto reconfigure_fifo_irq_restore; + } + +reconfigure_fifo_irq_restore: + mutex_unlock(&cdata->fifo_lock); + + if (disable_irq_and_flush) + enable_irq(cdata->irq); + + return err; +} +EXPORT_SYMBOL(st_lsm6ds3_reconfigure_fifo); + +int st_lsm6ds3_set_drdy_irq(struct lsm6ds3_sensor_data *sdata, bool state) +{ + u8 reg_addr, mask, value; + + if (state) + value = ST_LSM6DS3_EN_BIT; + else + value = ST_LSM6DS3_DIS_BIT; + + switch (sdata->sindex) { + case ST_INDIO_DEV_ACCEL: +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + if ((sdata->cdata->sensors_enabled & + (1 << ST_INDIO_DEV_GYRO)) || + (sdata->cdata->sensors_enabled & + ST_LSM6DS3_EXT_SENSORS)) + return 0; +#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + if (sdata->cdata->sensors_enabled & (1 << ST_INDIO_DEV_GYRO)) + return 0; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + reg_addr = ST_LSM6DS3_INT1_ADDR; + mask = ST_LSM6DS3_FIFO_THR_IRQ_MASK; + break; + case ST_INDIO_DEV_GYRO: +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + if ((sdata->cdata->sensors_enabled & + (1 << ST_INDIO_DEV_ACCEL)) || + (sdata->cdata->sensors_enabled & + ST_LSM6DS3_EXT_SENSORS)) + return 0; +#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + if (sdata->cdata->sensors_enabled & (1 << ST_INDIO_DEV_ACCEL)) + return 0; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + reg_addr = ST_LSM6DS3_INT1_ADDR; + mask = ST_LSM6DS3_FIFO_THR_IRQ_MASK; + break; + case ST_INDIO_DEV_SIGN_MOTION: + if (sdata->cdata->sensors_enabled & + (1 << ST_INDIO_DEV_STEP_DETECTOR)){ + dev_err(sdata->cdata->dev, "can't set up significant motion irq, step detector enabled"); + return 0; + } + + dev_info(sdata->cdata->dev,"--sig mot irq setup: %i", value); + if(value == 0 ){ + //need this so that sig motion can reregister before we sleep, + //otherwise we'll sleep and never wake up + wake_lock_timeout(&sdata->cdata->wlock, msecs_to_jiffies(1000)); + dev_err(sdata->cdata->dev, "IRQ disabled for md1 sig motion"); + //dump_stack(); + + } +#if 0 + reg_addr = ST_LSM6DS3_INT1_ADDR; + mask = ST_LSM6DS3_STEP_DETECTOR_DRDY_IRQ_MASK; + st_lsm6ds3_write_data_with_mask(sdata->cdata, + reg_addr, mask, value, true); +#endif +//TODO: move this to init since we don't need to set up the parameters every time + reg_addr = ST_LSM6DS3_MD1_ADDR; + mask = ST_LSM6DS3_6D_MD1_INT_MASK; + st_lsm6ds3_write_data_with_mask(sdata->cdata, + reg_addr, mask, value, true); + + reg_addr = ST_LSM6DS3_MD1_ADDR; + mask = ST_LSM6DS3_SINGTAP_MD1_INT_MASK; + //written at end of func + break; + case ST_INDIO_DEV_STEP_COUNTER: + reg_addr = ST_LSM6DS3_INT2_ADDR; + mask = ST_LSM6DS3_STEP_COUNTER_DRDY_IRQ_MASK; + break; + case ST_INDIO_DEV_STEP_DETECTOR: + if (sdata->cdata->sensors_enabled & + (1 << ST_INDIO_DEV_SIGN_MOTION)) + return 0; + dev_info(sdata->cdata->dev, "setting up STEP count irq"); + reg_addr = ST_LSM6DS3_INT1_ADDR; + mask = ST_LSM6DS3_STEP_DETECTOR_DRDY_IRQ_MASK; + break; + case ST_INDIO_DEV_TILT: + reg_addr = ST_LSM6DS3_MD1_ADDR; + mask = ST_LSM6DS3_TILT_DRDY_IRQ_MASK; + break; +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + case ST_INDIO_DEV_EXT0: + if ((sdata->cdata->sensors_enabled & + (1 << ST_INDIO_DEV_ACCEL)) || + (sdata->cdata->sensors_enabled & + (1 << ST_INDIO_DEV_GYRO)) || + (sdata->cdata->sensors_enabled & + (1 << ST_INDIO_DEV_EXT1))) + return 0; + + reg_addr = ST_LSM6DS3_INT1_ADDR; + mask = ST_LSM6DS3_FIFO_THR_IRQ_MASK; + break; + case ST_INDIO_DEV_EXT1: + if ((sdata->cdata->sensors_enabled & + (1 << ST_INDIO_DEV_ACCEL)) || + (sdata->cdata->sensors_enabled & + (1 << ST_INDIO_DEV_GYRO)) || + (sdata->cdata->sensors_enabled & + (1 << ST_INDIO_DEV_EXT0))) + return 0; + + reg_addr = ST_LSM6DS3_INT1_ADDR; + mask = ST_LSM6DS3_FIFO_THR_IRQ_MASK; + break; +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + default: + return -EINVAL; + } + + return st_lsm6ds3_write_data_with_mask(sdata->cdata, + reg_addr, mask, value, true); +} +EXPORT_SYMBOL(st_lsm6ds3_set_drdy_irq); + +int st_lsm6ds3_set_axis_enable(struct lsm6ds3_sensor_data *sdata, u8 value) +{ + u8 reg_addr; + + switch (sdata->sindex) { + case ST_INDIO_DEV_ACCEL: + reg_addr = ST_LSM6DS3_ACCEL_AXIS_EN_ADDR; + break; + case ST_INDIO_DEV_GYRO: + reg_addr = ST_LSM6DS3_GYRO_AXIS_EN_ADDR; + break; + default: + return 0; + } + + return st_lsm6ds3_write_data_with_mask(sdata->cdata, + reg_addr, ST_LSM6DS3_AXIS_EN_MASK, value, true); +} +EXPORT_SYMBOL(st_lsm6ds3_set_axis_enable); + +int st_lsm6ds3_enable_accel_dependency(struct lsm6ds3_sensor_data *sdata, + bool enable) +{ + int err; + + if (!((sdata->cdata->sensors_enabled & + ST_LSM6DS3_ACCEL_DEPENDENCY) & ~(1 << sdata->sindex))) { + if (enable) { + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + st_lsm6ds3_odr_table.addr[ST_INDIO_DEV_ACCEL], + st_lsm6ds3_odr_table.mask[ST_INDIO_DEV_ACCEL], + st_lsm6ds3_odr_table.odr_avl[0].value, true); + if (err < 0) + return err; + } else { + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + st_lsm6ds3_odr_table.addr[ST_INDIO_DEV_ACCEL], + st_lsm6ds3_odr_table.mask[ST_INDIO_DEV_ACCEL], + ST_LSM6DS3_ODR_POWER_OFF_VAL, true); + if (err < 0) + return err; + } + } + + return 0; +} +EXPORT_SYMBOL(st_lsm6ds3_enable_accel_dependency); + +static int st_lsm6ds3_set_extra_dependency(struct lsm6ds3_sensor_data *sdata, + bool enable) +{ + int err; + + if (!((sdata->cdata->sensors_enabled & + ST_LSM6DS3_EXTRA_DEPENDENCY) & ~(1 << sdata->sindex))) { + if (enable) { + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_FUNC_EN_ADDR, + ST_LSM6DS3_FUNC_EN_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + } else { + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_FUNC_EN_ADDR, + ST_LSM6DS3_FUNC_EN_MASK, + ST_LSM6DS3_DIS_BIT, true); + if (err < 0) + return err; + } + } + + return st_lsm6ds3_enable_accel_dependency(sdata, enable); +} + +static int st_lsm6ds3_enable_pedometer(struct lsm6ds3_sensor_data *sdata, + bool enable) +{ + u8 value = ST_LSM6DS3_DIS_BIT; + + if ((sdata->cdata->sensors_enabled & ~(1 << sdata->sindex)) & + ST_LSM6DS3_PEDOMETER_DEPENDENCY) + return 0; + + if (enable) + value = ST_LSM6DS3_EN_BIT; + + return st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_PEDOMETER_EN_ADDR, + ST_LSM6DS3_PEDOMETER_EN_MASK, + value, true); + +} + +static int st_lsm6ds3_enable_sensors(struct lsm6ds3_sensor_data *sdata) +{ + int err, i; + + switch (sdata->sindex) { + case ST_INDIO_DEV_ACCEL: + case ST_INDIO_DEV_GYRO: + for (i = 0; i < ST_LSM6DS3_ODR_LIST_NUM; i++) { + if (st_lsm6ds3_odr_table.odr_avl[i].hz == sdata->c_odr) + break; + } + if (i == ST_LSM6DS3_ODR_LIST_NUM) + return -EINVAL; + + if (sdata->sindex == ST_INDIO_DEV_ACCEL) { + sdata->cdata->accel_samples_to_discard = + ST_LSM6DS3_ACCEL_STD; + } + + sdata->cdata->gyro_samples_to_discard = ST_LSM6DS3_GYRO_STD; + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + st_lsm6ds3_odr_table.addr[sdata->sindex], + st_lsm6ds3_odr_table.mask[sdata->sindex], + st_lsm6ds3_odr_table.odr_avl[i].value, true); + if (err < 0) + return err; + + sdata->c_odr = st_lsm6ds3_odr_table.odr_avl[i].hz; + + break; + case ST_INDIO_DEV_SIGN_MOTION: + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_SIGN_MOTION_EN_ADDR, + ST_LSM6DS3_SIGN_MOTION_EN_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + + if ((sdata->cdata->sensors_enabled & ~(1 << sdata->sindex)) & + ST_LSM6DS3_PEDOMETER_DEPENDENCY) { + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_PEDOMETER_EN_ADDR, + ST_LSM6DS3_PEDOMETER_EN_MASK, + ST_LSM6DS3_DIS_BIT, true); + if (err < 0) + return err; + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_PEDOMETER_EN_ADDR, + ST_LSM6DS3_PEDOMETER_EN_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + } else { + err = st_lsm6ds3_enable_pedometer(sdata, true); + if (err < 0) + return err; + } + + break; + case ST_INDIO_DEV_STEP_COUNTER: + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_TIMER_EN_ADDR, + ST_LSM6DS3_TIMER_EN_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + + case ST_INDIO_DEV_STEP_DETECTOR: + err = st_lsm6ds3_enable_pedometer(sdata, true); + if (err < 0) + return err; + + break; + case ST_INDIO_DEV_TILT: + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_TILT_EN_ADDR, + ST_LSM6DS3_TILT_EN_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + + break; + default: + return -EINVAL; + } + + err = st_lsm6ds3_set_extra_dependency(sdata, true); + if (err < 0) + return err; + + sdata->cdata->sensors_enabled |= (1 << sdata->sindex); + + return 0; +} + +static int st_lsm6ds3_disable_sensors(struct lsm6ds3_sensor_data *sdata) +{ + int err; + + switch (sdata->sindex) { + case ST_INDIO_DEV_ACCEL: + if (sdata->cdata->sensors_enabled & + ST_LSM6DS3_EXTRA_DEPENDENCY) { + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + st_lsm6ds3_odr_table.addr[sdata->sindex], + st_lsm6ds3_odr_table.mask[sdata->sindex], + st_lsm6ds3_odr_table.odr_avl[0].value, true); + } else { + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + st_lsm6ds3_odr_table.addr[sdata->sindex], + st_lsm6ds3_odr_table.mask[sdata->sindex], + ST_LSM6DS3_ODR_POWER_OFF_VAL, true); + } + if (err < 0) + return err; + + break; + case ST_INDIO_DEV_GYRO: + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + st_lsm6ds3_odr_table.addr[sdata->sindex], + st_lsm6ds3_odr_table.mask[sdata->sindex], + ST_LSM6DS3_ODR_POWER_OFF_VAL, true); + if (err < 0) + return err; + + break; + case ST_INDIO_DEV_SIGN_MOTION: + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_SIGN_MOTION_EN_ADDR, + ST_LSM6DS3_SIGN_MOTION_EN_MASK, + ST_LSM6DS3_DIS_BIT, true); + if (err < 0) + return err; + + err = st_lsm6ds3_enable_pedometer(sdata, false); + if (err < 0) + return err; + + break; + case ST_INDIO_DEV_STEP_COUNTER: + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_TIMER_EN_ADDR, + ST_LSM6DS3_TIMER_EN_MASK, + ST_LSM6DS3_DIS_BIT, true); + if (err < 0) + return err; + + case ST_INDIO_DEV_STEP_DETECTOR: + err = st_lsm6ds3_enable_pedometer(sdata, false); + if (err < 0) + return err; + + break; + case ST_INDIO_DEV_TILT: + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_TILT_EN_ADDR, + ST_LSM6DS3_TILT_EN_MASK, + ST_LSM6DS3_DIS_BIT, true); + if (err < 0) + return err; + + break; + default: + return -EINVAL; + } + + err = st_lsm6ds3_set_extra_dependency(sdata, false); + if (err < 0) + return err; + + sdata->cdata->sensors_enabled &= ~(1 << sdata->sindex); + + return 0; +} + +int st_lsm6ds3_set_enable(struct lsm6ds3_sensor_data *sdata, bool enable) +{ + if (enable) + return st_lsm6ds3_enable_sensors(sdata); + else + return st_lsm6ds3_disable_sensors(sdata); +} +EXPORT_SYMBOL(st_lsm6ds3_set_enable); + +static int st_lsm6ds3_set_odr(struct lsm6ds3_sensor_data *sdata, + unsigned int odr) +{ + int err, i; + + for (i = 0; i < ST_LSM6DS3_ODR_LIST_NUM; i++) { + if (st_lsm6ds3_odr_table.odr_avl[i].hz == odr) + break; + } + if (i == ST_LSM6DS3_ODR_LIST_NUM) + return -EINVAL; + + if (sdata->cdata->sensors_enabled & (1 << sdata->sindex)) { + disable_irq(sdata->cdata->irq); + st_lsm6ds3_flush_works(); + + if (sdata->sindex == ST_INDIO_DEV_ACCEL) + sdata->cdata->accel_samples_to_discard = + ST_LSM6DS3_ACCEL_STD; + + sdata->cdata->gyro_samples_to_discard = ST_LSM6DS3_GYRO_STD; + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + st_lsm6ds3_odr_table.addr[sdata->sindex], + st_lsm6ds3_odr_table.mask[sdata->sindex], + st_lsm6ds3_odr_table.odr_avl[i].value, true); + if (err < 0) { + enable_irq(sdata->cdata->irq); + return err; + } + + sdata->c_odr = st_lsm6ds3_odr_table.odr_avl[i].hz; + + st_lsm6ds3_reconfigure_fifo(sdata->cdata, false); + enable_irq(sdata->cdata->irq); + } else + sdata->c_odr = st_lsm6ds3_odr_table.odr_avl[i].hz; + + return 0; +} + +static int st_lsm6ds3_set_fs(struct lsm6ds3_sensor_data *sdata, + unsigned int gain) +{ + int err, i; + + for (i = 0; i < ST_LSM6DS3_FS_LIST_NUM; i++) { + if (st_lsm6ds3_fs_table[sdata->sindex].fs_avl[i].gain == gain) + break; + } + if (i == ST_LSM6DS3_FS_LIST_NUM) + return -EINVAL; + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + st_lsm6ds3_fs_table[sdata->sindex].addr, + st_lsm6ds3_fs_table[sdata->sindex].mask, + st_lsm6ds3_fs_table[sdata->sindex].fs_avl[i].value, true); + if (err < 0) + return err; + + sdata->c_gain[0] = gain; + + return 0; +} + +static int st_lsm6ds3_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, int *val, + int *val2, long mask) +{ + int err; + u8 outdata[ST_LSM6DS3_BYTE_FOR_CHANNEL]; + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_RAW: + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + err = st_lsm6ds3_set_enable(sdata, true); + if (err < 0) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + if (sdata->sindex == ST_INDIO_DEV_ACCEL) + msleep(40); + + if (sdata->sindex == ST_INDIO_DEV_GYRO) + msleep(120); + + err = sdata->cdata->tf->read(sdata->cdata, ch->address, + ST_LSM6DS3_BYTE_FOR_CHANNEL, outdata, true); + if (err < 0) { + mutex_unlock(&indio_dev->mlock); + return err; + } + + *val = (s16)get_unaligned_le16(outdata); + *val = *val >> ch->scan_type.shift; + + err = st_lsm6ds3_set_enable(sdata, false); + + mutex_unlock(&indio_dev->mlock); + + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sdata->c_gain[0]; + return IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } + + return 0; +} + +static int st_lsm6ds3_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, int val, int val2, long mask) +{ + int err; + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + err = st_lsm6ds3_set_fs(sdata, val2); + mutex_unlock(&indio_dev->mlock); + break; + default: + return -EINVAL; + } + + return err; +} + +static int st_lsm6ds3_reset_steps(struct lsm6ds3_data *cdata) +{ + int err; + u8 reg_value = 0x00; + + err = cdata->tf->read(cdata, + ST_LSM6DS3_STEP_COUNTER_RES_ADDR, 1, ®_value, true); + if (err < 0) + return err; + + if (reg_value & ST_LSM6DS3_FUNC_EN_MASK) + reg_value = ST_LSM6DS3_STEP_COUNTER_RES_FUNC_EN; + else + reg_value = ST_LSM6DS3_DIS_BIT; + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_STEP_COUNTER_RES_ADDR, + ST_LSM6DS3_STEP_COUNTER_RES_MASK, + ST_LSM6DS3_STEP_COUNTER_RES_ALL_EN, true); + if (err < 0) + return err; + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_STEP_COUNTER_RES_ADDR, + ST_LSM6DS3_STEP_COUNTER_RES_MASK, + reg_value, true); + if (err < 0) + return err; + + cdata->reset_steps = true; + + return 0; +} + +static int st_lsm6ds3_init_sensor(struct lsm6ds3_data *cdata) +{ + int err, i; + u8 default_reg_value = 0; + u8 regval = 0; + struct lsm6ds3_sensor_data *sdata; + + mutex_init(&cdata->tb.buf_lock); + + cdata->sensors_enabled = 0; + cdata->reset_steps = false; + cdata->sign_motion_event_ready = false; + + err = st_lsm6ds3_write_data_with_mask(cdata, ST_LSM6DS3_RESET_ADDR, + ST_LSM6DS3_RESET_MASK, ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + sdata = iio_priv(cdata->indio_dev[i]); + + err = st_lsm6ds3_set_enable(sdata, false); + if (err < 0) + return err; + + err = st_lsm6ds3_set_drdy_irq(sdata, false); + if (err < 0) + return err; + + switch (sdata->sindex) { + case ST_INDIO_DEV_ACCEL: + case ST_INDIO_DEV_GYRO: + sdata->num_data_channels = + ARRAY_SIZE(st_lsm6ds3_accel_ch) - 1; + + err = st_lsm6ds3_set_fs(sdata, sdata->c_gain[0]); + if (err < 0) + return err; + + break; + case ST_INDIO_DEV_STEP_COUNTER: + sdata->num_data_channels = + ARRAY_SIZE(st_lsm6ds3_step_c_ch) - 1; + break; + default: + break; + } + } + + cdata->gyro_selftest_status = 0; + cdata->accel_selftest_status = 0; + + err = st_lsm6ds3_write_data_with_mask(cdata, ST_LSM6DS3_LIR_ADDR, + ST_LSM6DS3_LIR_MASK, ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + + err = st_lsm6ds3_write_data_with_mask(cdata, ST_LSM6DS3_BDU_ADDR, + ST_LSM6DS3_BDU_MASK, ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + + err = st_lsm6ds3_set_fifo_enable(sdata->cdata, false); + if (err < 0) + return err; + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_ROUNDING_ADDR, + ST_LSM6DS3_ROUNDING_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + + err = st_lsm6ds3_write_data_with_mask(cdata, + ST_LSM6DS3_INT2_ON_INT1_ADDR, + ST_LSM6DS3_INT2_ON_INT1_MASK, + ST_LSM6DS3_EN_BIT, true); + if (err < 0) + return err; + + err = st_lsm6ds3_reset_steps(sdata->cdata); + if (err < 0) + return err; + + mutex_lock(&cdata->bank_registers_lock); + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR, + ST_LSM6DS3_FUNC_CFG_REG2_MASK, + ST_LSM6DS3_EN_BIT, false); + if (err < 0) + goto st_lsm6ds3_init_sensor_mutex_unlock; + + err = sdata->cdata->tf->write(sdata->cdata, + ST_LSM6DS3_STEP_COUNTER_DURATION_ADDR, + 1, &default_reg_value, false); + if (err < 0) + goto st_lsm6ds3_init_sensor_mutex_unlock; + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR, + ST_LSM6DS3_FUNC_CFG_REG2_MASK, + ST_LSM6DS3_DIS_BIT, false); + if (err < 0) + goto st_lsm6ds3_init_sensor_mutex_unlock; +//set up tap stuff +#define ST_LSM6DS3_TAP_CFG_ADDR 0x58 +#define ST_LSM6DS3_TAP_CFG_XYZ_MASK (0xE) + //1 is for latch enable +#define ST_LSM6DS3_TAP_THS_6D_ADDR 0x59 +#define ST_LSM6DS3_INT_DUR2_ADDR 0x5A +#define ST_LSM6DS3_CTRL8_ADDR 0x17 +#define ST_LSM6DS3_CTRL8_LPF_ON_6D 0x01 +#define ST_LSM6DS3_CTRL8_LPF_ON_ACCEL 0x80 + +#define ST_LSM6DS3_CTRL4C_ADDR 0x13 + + regval = 0x50;//tap threshold + err = sdata->cdata->tf->write(sdata->cdata, + ST_LSM6DS3_TAP_THS_6D_ADDR, + 1, ®val, false); + + regval = 0x6; //quiet and shock times + err = sdata->cdata->tf->write(sdata->cdata, + ST_LSM6DS3_INT_DUR2_ADDR, + 1, ®val, false); + + regval = 0x13; //enable slop_fd to get lpf2 for 6d + // also enable z tap and latch irqs + err = sdata->cdata->tf->write(sdata->cdata, + ST_LSM6DS3_TAP_CFG_ADDR, + 1, ®val, false); + + regval = ST_LSM6DS3_CTRL8_LPF_ON_6D | ST_LSM6DS3_CTRL8_LPF_ON_ACCEL; + err = sdata->cdata->tf->write(sdata->cdata, + ST_LSM6DS3_CTRL8_ADDR , + 1, ®val, false); + + regval = 1;//todo fix this because it overwrites int2_on_int1 bit + err = sdata->cdata->tf->write(sdata->cdata, + ST_LSM6DS3_CTRL4C_ADDR , + 1, ®val, false); + + if (err < 0) + goto st_lsm6ds3_init_sensor_mutex_unlock; + + + + // + mutex_unlock(&cdata->bank_registers_lock); + + sdata->c_odr = 0; + + + return 0; + +st_lsm6ds3_init_sensor_mutex_unlock: + mutex_unlock(&cdata->bank_registers_lock); + return err; +} + +static int st_lsm6ds3_set_selftest(struct lsm6ds3_sensor_data *sdata, int index) +{ + int err; + u8 mode, mask; + + switch (sdata->sindex) { + case ST_INDIO_DEV_ACCEL: + mask = ST_LSM6DS3_SELFTEST_ACCEL_MASK; + mode = st_lsm6ds3_selftest_table[index].accel_value; + break; + case ST_INDIO_DEV_GYRO: + mask = ST_LSM6DS3_SELFTEST_GYRO_MASK; + mode = st_lsm6ds3_selftest_table[index].gyro_value; + break; + default: + return -EINVAL; + } + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_SELFTEST_ADDR, mask, mode, true); + if (err < 0) + return err; + + return 0; +} + +static ssize_t st_lsm6ds3_sysfs_set_max_delivery_rate(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + u8 duration; + int err, err2; + unsigned int max_delivery_rate; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtouint(buf, 10, &max_delivery_rate); + if (err < 0) + return -EINVAL; + + if (max_delivery_rate == sdata->c_odr) + return size; + + duration = max_delivery_rate / ST_LSM6DS3_MIN_DURATION_MS; + + mutex_lock(&sdata->cdata->bank_registers_lock); + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR, + ST_LSM6DS3_FUNC_CFG_REG2_MASK, + ST_LSM6DS3_EN_BIT, false); + if (err < 0) + goto st_lsm6ds3_sysfs_set_max_delivery_rate_mutex_unlock; + + err = sdata->cdata->tf->write(sdata->cdata, + ST_LSM6DS3_STEP_COUNTER_DURATION_ADDR, + 1, &duration, false); + if (err < 0) + goto st_lsm6ds3_sysfs_set_max_delivery_rate_restore_bank; + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR, + ST_LSM6DS3_FUNC_CFG_REG2_MASK, + ST_LSM6DS3_DIS_BIT, false); + if (err < 0) + goto st_lsm6ds3_sysfs_set_max_delivery_rate_restore_bank; + + mutex_unlock(&sdata->cdata->bank_registers_lock); + + sdata->c_odr = max_delivery_rate; + + return size; + +st_lsm6ds3_sysfs_set_max_delivery_rate_restore_bank: + do { + err2 = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR, + ST_LSM6DS3_FUNC_CFG_REG2_MASK, + ST_LSM6DS3_DIS_BIT, false); + + msleep(500); + } while (err2 < 0); + +st_lsm6ds3_sysfs_set_max_delivery_rate_mutex_unlock: + mutex_unlock(&sdata->cdata->bank_registers_lock); + return err; +} + +static ssize_t st_lsm6ds3_sysfs_get_max_delivery_rate(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sdata->c_odr); +} + +static ssize_t st_lsm6ds3_sysfs_reset_counter(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err; + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + err = st_lsm6ds3_reset_steps(sdata->cdata); + if (err < 0) + return err; + + return size; +} + +static ssize_t st_lsm6ds3_sysfs_get_sampling_frequency(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sdata->c_odr); +} + +static ssize_t st_lsm6ds3_sysfs_set_sampling_frequency(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err; + unsigned int odr; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &odr); + if (err < 0) + return err; + + mutex_lock(&indio_dev->mlock); + + if (sdata->c_odr != odr) + err = st_lsm6ds3_set_odr(sdata, odr); + + mutex_unlock(&indio_dev->mlock); + + return err < 0 ? err : size; +} + +static ssize_t st_lsm6ds3_sysfs_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int i, len = 0; + + for (i = 0; i < ST_LSM6DS3_ODR_LIST_NUM; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + st_lsm6ds3_odr_table.odr_avl[i].hz); + } + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_lsm6ds3_sysfs_scale_avail(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int i, len = 0; + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + for (i = 0; i < ST_LSM6DS3_FS_LIST_NUM; i++) { + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", + st_lsm6ds3_fs_table[sdata->sindex].fs_avl[i].gain); + } + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_lsm6ds3_sysfs_get_selftest_available(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s, %s, %s\n", + st_lsm6ds3_selftest_table[0].string_mode, + st_lsm6ds3_selftest_table[1].string_mode, + st_lsm6ds3_selftest_table[2].string_mode); +} + +static ssize_t st_lsm6ds3_sysfs_get_selftest_status(struct device *dev, + struct device_attribute *attr, char *buf) +{ + u8 index; + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + switch (sdata->sindex) { + case ST_INDIO_DEV_ACCEL: + index = sdata->cdata->accel_selftest_status; + break; + case ST_INDIO_DEV_GYRO: + index = sdata->cdata->gyro_selftest_status; + break; + default: + return -EINVAL; + } + + return sprintf(buf, "%s\n", + st_lsm6ds3_selftest_table[index].string_mode); +} + +static ssize_t st_lsm6ds3_sysfs_set_selftest_status(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int err, i; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + for (i = 0; i < ARRAY_SIZE(st_lsm6ds3_selftest_table); i++) { + if (strncmp(buf, st_lsm6ds3_selftest_table[i].string_mode, + size - 2) == 0) + break; + } + if (i == ARRAY_SIZE(st_lsm6ds3_selftest_table)) + return -EINVAL; + + err = st_lsm6ds3_set_selftest(sdata, i); + if (err < 0) + return err; + + switch (sdata->sindex) { + case ST_INDIO_DEV_ACCEL: + sdata->cdata->accel_selftest_status = i; + break; + case ST_INDIO_DEV_GYRO: + sdata->cdata->gyro_selftest_status = i; + break; + default: + return -EINVAL; + } + + return size; +} + +ssize_t st_lsm6ds3_sysfs_get_hw_fifo_lenght(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%d\n", ST_LSM6DS3_MAX_FIFO_LENGHT); +} + +ssize_t st_lsm6ds3_sysfs_flush_fifo(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + disable_irq(sdata->cdata->irq); + st_lsm6ds3_flush_works(); + + mutex_lock(&sdata->cdata->fifo_lock); + + st_lsm6ds3_read_fifo(sdata->cdata, true); + + mutex_unlock(&sdata->cdata->fifo_lock); + + enable_irq(sdata->cdata->irq); + + return size; +} + +ssize_t st_lsm6ds3_sysfs_get_irq_src(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + ssize_t ret = sprintf(buf, "%d\n", sdata->cdata->last_wakeup_source); + sdata->cdata->last_wakeup_source = 0; //clear flags + return ret; +} + + +static ssize_t st_lsm6ds3_sysfs_write_reg(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int ret; + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + uint32_t waddr, wval; + ret = sscanf(buf, "%i %i", &waddr, &wval); + dev_info(sdata->cdata->dev, "read: 0x%x 0x%x", waddr, wval); + if(ret != 2){ + dev_err(sdata->cdata->dev,"error, write_reg usage: addr val"); + return -EINVAL; + } + + sdata->cdata->tf->write(sdata->cdata, waddr, 1, (u8 *)&wval, true); + + return size; +} + +static ssize_t st_lsm6ds3_sysfs_read_reg_set(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int ret ; + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + uint32_t waddr, wval; + ret = sscanf(buf, "%i", &waddr); + dev_info(dev, "read: 0x%x ", waddr); + if(ret != 1){ + dev_err(sdata->cdata->dev,"error, read_reg usage: addr "); + return -EINVAL; + } + + sdata->cdata->tf->read(sdata->cdata, waddr, 1, &sdata->cdata->reg_read, true); + + return size; +} + + +ssize_t st_lsm6ds3_sysfs_read_reg_get(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + ssize_t ret = sprintf(buf, "%d\n", sdata->cdata->reg_read); + return ret; +} + +ssize_t st_lsm6ds3_sysfs_sixd_wake_mask_get(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + ssize_t ret = sprintf(buf, "0x%02x\n", sdata->cdata->sixd_mask); + return ret; +} + + +static ssize_t st_lsm6ds3_sysfs_sixd_wake_mask_set(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int ret ; + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + uint32_t maskval; + ret = sscanf(buf, "%i", &maskval); + dev_info(sdata->cdata->dev, "read: 0x%x ", maskval); + if(ret != 1 || maskval ){ + dev_info(sdata->cdata->dev, "read: 0x%x ", maskval); + dev_err(sdata->cdata->dev,"error, sixd_mask usage: mask sig mot events from"); + dev_err(sdata->cdata->dev,"ZH | ZL | YH | YL | XH | XL \n"); + return -EINVAL; + } + + sdata->cdata->sixd_mask = maskval & SIXD_MASK_VALID_BITS; + + return size; +} +static ST_LSM6DS3_DEV_ATTR_SAMP_FREQ(); +static ST_LSM6DS3_DEV_ATTR_SAMP_FREQ_AVAIL(); +static ST_LSM6DS3_DEV_ATTR_SCALE_AVAIL(in_accel_scale_available); +static ST_LSM6DS3_DEV_ATTR_SCALE_AVAIL(in_anglvel_scale_available); +static ST_LSM6DS3_FIFO_LENGHT(); +static ST_LSM6DS3_FIFO_FLUSH(); + +static IIO_DEVICE_ATTR(reset_counter, S_IWUSR, + NULL, st_lsm6ds3_sysfs_reset_counter, 0); + +static IIO_DEVICE_ATTR(max_delivery_rate, S_IWUSR | S_IRUGO, + st_lsm6ds3_sysfs_get_max_delivery_rate, + st_lsm6ds3_sysfs_set_max_delivery_rate, 0); + +static IIO_DEVICE_ATTR(self_test_available, S_IRUGO, + st_lsm6ds3_sysfs_get_selftest_available, + NULL, 0); + +static IIO_DEVICE_ATTR(self_test, S_IWUSR | S_IRUGO, + st_lsm6ds3_sysfs_get_selftest_status, + st_lsm6ds3_sysfs_set_selftest_status, 0); + +static IIO_DEVICE_ATTR(irq_source, S_IRUGO, + st_lsm6ds3_sysfs_get_irq_src, + NULL, 0); + + +static IIO_DEVICE_ATTR(write_reg, S_IWUSR, + NULL, st_lsm6ds3_sysfs_write_reg, 0); + + +static IIO_DEVICE_ATTR(read_reg, S_IRUGO | S_IWUSR, + st_lsm6ds3_sysfs_read_reg_get, + st_lsm6ds3_sysfs_read_reg_set, 0); + +static IIO_DEVICE_ATTR(sixd_wake_mask, S_IRUGO | S_IWUSR, + st_lsm6ds3_sysfs_sixd_wake_mask_get, + st_lsm6ds3_sysfs_sixd_wake_mask_set, 0); + +static struct attribute *st_lsm6ds3_accel_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_accel_scale_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_self_test_available.dev_attr.attr, + &iio_dev_attr_self_test.dev_attr.attr, + &iio_dev_attr_hw_fifo_lenght.dev_attr.attr, + &iio_dev_attr_flush.dev_attr.attr, + &iio_dev_attr_irq_source.dev_attr.attr, + &iio_dev_attr_write_reg.dev_attr.attr, + &iio_dev_attr_read_reg.dev_attr.attr, + &iio_dev_attr_sixd_wake_mask.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6ds3_accel_attribute_group = { + .attrs = st_lsm6ds3_accel_attributes, +}; + +static const struct iio_info st_lsm6ds3_accel_info = { + .driver_module = THIS_MODULE, + .attrs = &st_lsm6ds3_accel_attribute_group, + .read_raw = &st_lsm6ds3_read_raw, + .write_raw = &st_lsm6ds3_write_raw, +}; + +static struct attribute *st_lsm6ds3_gyro_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_anglvel_scale_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_self_test_available.dev_attr.attr, + &iio_dev_attr_self_test.dev_attr.attr, + &iio_dev_attr_hw_fifo_lenght.dev_attr.attr, + &iio_dev_attr_flush.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6ds3_gyro_attribute_group = { + .attrs = st_lsm6ds3_gyro_attributes, +}; + +static const struct iio_info st_lsm6ds3_gyro_info = { + .driver_module = THIS_MODULE, + .attrs = &st_lsm6ds3_gyro_attribute_group, + .read_raw = &st_lsm6ds3_read_raw, + .write_raw = &st_lsm6ds3_write_raw, +}; + +static struct attribute *st_lsm6ds3_sign_motion_attributes[] = { + NULL, +}; + +static const struct attribute_group st_lsm6ds3_sign_motion_attribute_group = { + .attrs = st_lsm6ds3_sign_motion_attributes, +}; + +static const struct iio_info st_lsm6ds3_sign_motion_info = { + .driver_module = THIS_MODULE, + .attrs = &st_lsm6ds3_sign_motion_attribute_group, +}; + +static struct attribute *st_lsm6ds3_step_c_attributes[] = { + &iio_dev_attr_reset_counter.dev_attr.attr, + &iio_dev_attr_max_delivery_rate.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6ds3_step_c_attribute_group = { + .attrs = st_lsm6ds3_step_c_attributes, +}; + +static const struct iio_info st_lsm6ds3_step_c_info = { + .driver_module = THIS_MODULE, + .attrs = &st_lsm6ds3_step_c_attribute_group, + .read_raw = &st_lsm6ds3_read_raw, +}; + +static struct attribute *st_lsm6ds3_step_d_attributes[] = { + NULL, +}; + +static const struct attribute_group st_lsm6ds3_step_d_attribute_group = { + .attrs = st_lsm6ds3_step_d_attributes, +}; + +static const struct iio_info st_lsm6ds3_step_d_info = { + .driver_module = THIS_MODULE, + .attrs = &st_lsm6ds3_step_d_attribute_group, +}; + +static struct attribute *st_lsm6ds3_tilt_attributes[] = { + NULL, +}; + +static const struct attribute_group st_lsm6ds3_tilt_attribute_group = { + .attrs = st_lsm6ds3_tilt_attributes, +}; + +static const struct iio_info st_lsm6ds3_tilt_info = { + .driver_module = THIS_MODULE, + .attrs = &st_lsm6ds3_tilt_attribute_group, +}; + +#ifdef CONFIG_IIO_TRIGGER +static const struct iio_trigger_ops st_lsm6ds3_trigger_ops = { + .owner = THIS_MODULE, + .set_trigger_state = ST_LSM6DS3_TRIGGER_SET_STATE, +}; +#define ST_LSM6DS3_TRIGGER_OPS (&st_lsm6ds3_trigger_ops) +#else +#define ST_LSM6DS3_TRIGGER_OPS NULL +#endif + +int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq) +{ + u8 wai = 0x00; + int i, n, err; + struct lsm6ds3_sensor_data *sdata; + + mutex_init(&cdata->bank_registers_lock); + mutex_init(&cdata->fifo_lock); + wake_lock_init(&cdata->wlock, WAKE_LOCK_SUSPEND, "st_lsm6ds3_kdriv_lock"); + +#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT + mutex_init(&cdata->passthrough_lock); +#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */ + + cdata->fifo_data = 0; + + err = cdata->tf->read(cdata, ST_LSM6DS3_WAI_ADDRESS, 1, &wai, true); + if (err < 0) { + dev_err(cdata->dev, "failed to read Who-Am-I register.\n"); + return err; + } + if (wai != ST_LSM6DS3_WAI_EXP) { + dev_err(cdata->dev, "Who-Am-I value not valid.\n"); + return -ENODEV; + } + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + cdata->indio_dev[i] = iio_device_alloc(sizeof(*sdata)); + if (cdata->indio_dev[i] == NULL) { + err = -ENOMEM; + goto iio_device_free; + } + sdata = iio_priv(cdata->indio_dev[i]); + sdata->cdata = cdata; + sdata->sindex = i; + + if ((i == ST_INDIO_DEV_ACCEL) || (i == ST_INDIO_DEV_GYRO)) { + sdata->c_odr = st_lsm6ds3_odr_table.odr_avl[0].hz; + sdata->c_gain[0] = + st_lsm6ds3_fs_table[i].fs_avl[0].gain; + } + cdata->indio_dev[i]->modes = INDIO_DIRECT_MODE; + } + + if (irq > 0) { + cdata->irq = irq; + dev_info(cdata->dev, "driver use DRDY int pin 1\n"); + } + + cdata->sixd_mask = SIXD_MASK_VALID_BITS; + + cdata->indio_dev[ST_INDIO_DEV_ACCEL]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DS3_ACCEL_SUFFIX_NAME); + cdata->indio_dev[ST_INDIO_DEV_ACCEL]->info = &st_lsm6ds3_accel_info; + cdata->indio_dev[ST_INDIO_DEV_ACCEL]->channels = st_lsm6ds3_accel_ch; + cdata->indio_dev[ST_INDIO_DEV_ACCEL]->num_channels = + ARRAY_SIZE(st_lsm6ds3_accel_ch); + + cdata->indio_dev[ST_INDIO_DEV_GYRO]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DS3_GYRO_SUFFIX_NAME); + cdata->indio_dev[ST_INDIO_DEV_GYRO]->info = &st_lsm6ds3_gyro_info; + cdata->indio_dev[ST_INDIO_DEV_GYRO]->channels = st_lsm6ds3_gyro_ch; + cdata->indio_dev[ST_INDIO_DEV_GYRO]->num_channels = + ARRAY_SIZE(st_lsm6ds3_gyro_ch); + + cdata->indio_dev[ST_INDIO_DEV_SIGN_MOTION]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DS3_SIGN_MOTION_SUFFIX_NAME); + cdata->indio_dev[ST_INDIO_DEV_SIGN_MOTION]->info = + &st_lsm6ds3_sign_motion_info; + cdata->indio_dev[ST_INDIO_DEV_SIGN_MOTION]->channels = + st_lsm6ds3_sign_motion_ch; + cdata->indio_dev[ST_INDIO_DEV_SIGN_MOTION]->num_channels = + ARRAY_SIZE(st_lsm6ds3_sign_motion_ch); + + cdata->indio_dev[ST_INDIO_DEV_STEP_COUNTER]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DS3_STEP_COUNTER_SUFFIX_NAME); + cdata->indio_dev[ST_INDIO_DEV_STEP_COUNTER]->info = + &st_lsm6ds3_step_c_info; + cdata->indio_dev[ST_INDIO_DEV_STEP_COUNTER]->channels = + st_lsm6ds3_step_c_ch; + cdata->indio_dev[ST_INDIO_DEV_STEP_COUNTER]->num_channels = + ARRAY_SIZE(st_lsm6ds3_step_c_ch); + + cdata->indio_dev[ST_INDIO_DEV_STEP_DETECTOR]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DS3_STEP_DETECTOR_SUFFIX_NAME); + cdata->indio_dev[ST_INDIO_DEV_STEP_DETECTOR]->info = + &st_lsm6ds3_step_d_info; + cdata->indio_dev[ST_INDIO_DEV_STEP_DETECTOR]->channels = + st_lsm6ds3_step_d_ch; + cdata->indio_dev[ST_INDIO_DEV_STEP_DETECTOR]->num_channels = + ARRAY_SIZE(st_lsm6ds3_step_d_ch); + + cdata->indio_dev[ST_INDIO_DEV_TILT]->name = + kasprintf(GFP_KERNEL, "%s_%s", cdata->name, + ST_LSM6DS3_TILT_SUFFIX_NAME); + cdata->indio_dev[ST_INDIO_DEV_TILT]->info = &st_lsm6ds3_tilt_info; + cdata->indio_dev[ST_INDIO_DEV_TILT]->channels = st_lsm6ds3_tilt_ch; + cdata->indio_dev[ST_INDIO_DEV_TILT]->num_channels = + ARRAY_SIZE(st_lsm6ds3_tilt_ch); + + err = st_lsm6ds3_init_sensor(cdata); + if (err < 0) + goto iio_device_free; + + err = st_lsm6ds3_allocate_rings(cdata); + if (err < 0) + goto iio_device_free; + + if (irq > 0) { + err = st_lsm6ds3_allocate_triggers(cdata, + ST_LSM6DS3_TRIGGER_OPS); + if (err < 0) + goto deallocate_ring; + } + + for (n = 0; n < ST_INDIO_DEV_NUM; n++) { + err = iio_device_register(cdata->indio_dev[n]); + if (err) + goto iio_device_unregister_and_trigger_deallocate; + } + + err = st_lsm6ds3_i2c_master_probe(cdata); + if (err < 0) + goto iio_device_unregister_and_trigger_deallocate; + + device_init_wakeup(cdata->dev, true); + + return 0; + +iio_device_unregister_and_trigger_deallocate: + for (n--; n >= 0; n--) + iio_device_unregister(cdata->indio_dev[n]); + + if (irq > 0) + st_lsm6ds3_deallocate_triggers(cdata); +deallocate_ring: + st_lsm6ds3_deallocate_rings(cdata); +iio_device_free: + for (i--; i >= 0; i--) + iio_device_free(cdata->indio_dev[i]); + + return err; +} +EXPORT_SYMBOL(st_lsm6ds3_common_probe); + +void st_lsm6ds3_common_remove(struct lsm6ds3_data *cdata, int irq) +{ + int i; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) + iio_device_unregister(cdata->indio_dev[i]); + + if (irq > 0) + st_lsm6ds3_deallocate_triggers(cdata); + + st_lsm6ds3_deallocate_rings(cdata); + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) + iio_device_free(cdata->indio_dev[i]); + + st_lsm6ds3_i2c_master_exit(cdata); +} +EXPORT_SYMBOL(st_lsm6ds3_common_remove); + +#ifdef CONFIG_PM +int st_lsm6ds3_common_suspend(struct lsm6ds3_data *cdata) +{ +#ifndef CONFIG_ST_LSM6DS3_IIO_SENSORS_WAKEUP + int err, i; + u8 tmp_sensors_enabled; + struct lsm6ds3_sensor_data *sdata; + u8 reg_value; + + tmp_sensors_enabled = cdata->sensors_enabled; + dev_info(cdata->dev, "entering suspend"); + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + sdata = iio_priv(cdata->indio_dev[i]); + if ((i == ST_INDIO_DEV_SIGN_MOTION) || (i == ST_INDIO_DEV_TILT)) + continue; + if(i == ST_INDIO_DEV_ACCEL){ + //stop the fifo + err = st_lsm6ds3_set_drdy_irq(sdata, false); + cdata->int1_save = 1; + continue; //do not disable + } + + + err = st_lsm6ds3_set_enable(sdata, false); + //todo: see if this is good + err = st_lsm6ds3_set_drdy_irq(sdata, false); + if (err < 0) + return err; + } + cdata->sensors_enabled = tmp_sensors_enabled; + +#endif /* CONFIG_ST_LSM6DS3_IIO_SENSORS_WAKEUP */ + + if (cdata->sensors_enabled & ST_LSM6DS3_WAKE_UP_SENSORS) { + if (device_may_wakeup(cdata->dev)) + enable_irq_wake(cdata->irq); + } + err = cdata->tf->read(cdata, + ST_LSM6DS3_MD1_ADDR, 1, ®_value, true); + dev_info(cdata->dev, "before suspend md1: %x, err:%i", reg_value, err); + dev_info(cdata->dev, "manually setting to 0x44"); + reg_value = 0x44; + err = cdata->tf->write(cdata, + ST_LSM6DS3_MD1_ADDR, 1, ®_value, true); + err = cdata->tf->read(cdata, + ST_LSM6DS3_MD1_ADDR, 1, ®_value, true); + dev_info(cdata->dev, "before suspend md1: %x, err:%i", reg_value, err); + + err = cdata->tf->read(cdata, + 0xd, 1, ®_value, true); + dev_info(cdata->dev, "before suspend int1: %x err:%i", reg_value, err); + + err = cdata->tf->read(cdata, + 0x58, 1, ®_value, true); + dev_info(cdata->dev, "before suspend 0x58 tap enable: %x err:%i", reg_value, err); + err = cdata->tf->read(cdata, + 0x10, 1, ®_value, true); + dev_info(cdata->dev, "ctrl1_xl: %x err:%i", reg_value, err); + + err = cdata->tf->read(cdata, + 0xa, 1, ®_value, true); + dev_info(cdata->dev, "before suspend 0xA fifo mode: %x err:%i", reg_value, err); + + err = cdata->tf->read(cdata, + 0x13, 1, ®_value, true); + dev_info(cdata->dev, "before suspend 0x13 ctrl4_c : %x err:%i", reg_value, err); + + err = cdata->tf->read(cdata, + 0x06, 1, ®_value, true); + dev_info(cdata->dev, "before suspend 0x6 fifo_fth : %x err:%i", reg_value, err); + + + + reg_value = 624; //set fifo to 1 second of data (104hz fifo) + err = cdata->tf->write(cdata, + ST_LSM6DS3_FIFO_THR_L_ADDR, 1, ®_value, true); + + cdata->first_irq_from_resume = 1; + return 0; +} +EXPORT_SYMBOL(st_lsm6ds3_common_suspend); + +int st_lsm6ds3_common_resume(struct lsm6ds3_data *cdata) +{ +#ifndef CONFIG_ST_LSM6DS3_IIO_SENSORS_WAKEUP + int err, i; + struct lsm6ds3_sensor_data *sdata; + u8 reg_value; + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + if ((i == ST_INDIO_DEV_SIGN_MOTION) || (i == ST_INDIO_DEV_TILT)) + continue; + + sdata = iio_priv(cdata->indio_dev[i]); + + if(i == ST_INDIO_DEV_ACCEL && cdata->int1_save == 1 ){ + //todo fix how this is set, we're always clearing and setting + err = st_lsm6ds3_set_drdy_irq(sdata, true); + cdata->int1_save = 0; + } + + if ((1 << sdata->sindex) & cdata->sensors_enabled) { + err = st_lsm6ds3_set_enable(sdata, true); + if (err < 0) + return err; + } + } +#endif /* CONFIG_ST_LSM6DS3_IIO_SENSORS_WAKEUP */ + + if (cdata->sensors_enabled & ST_LSM6DS3_WAKE_UP_SENSORS) { + if (device_may_wakeup(cdata->dev)) + disable_irq_wake(cdata->irq); + } + reg_value = 624/8; //set fifo to 1 second of data (104hz fifo) + err = cdata->tf->write(cdata, + ST_LSM6DS3_FIFO_THR_L_ADDR, 1, ®_value, true); + + return 0; +} +EXPORT_SYMBOL(st_lsm6ds3_common_resume); +#endif /* CONFIG_PM */ + +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>"); +MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 core driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c.c new file mode 100644 index 00000000000..436f09e0060 --- /dev/null +++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c.c @@ -0,0 +1,159 @@ +/* + * STMicroelectronics lsm6ds3 i2c driver + * + * Copyright 2014 STMicroelectronics Inc. + * + * Denis Ciocca <denis.ciocca@st.com> + * + * Licensed under the GPL-2. + */ + +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/iio/iio.h> + +#include "st_lsm6ds3.h" + +static int st_lsm6ds3_i2c_read(struct lsm6ds3_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err = 0; + struct i2c_msg msg[2]; + struct i2c_client *client = to_i2c_client(cdata->dev); + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = ®_addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = data; + + if (b_lock) { + mutex_lock(&cdata->bank_registers_lock); + err = i2c_transfer(client->adapter, msg, 2); + mutex_unlock(&cdata->bank_registers_lock); + } else + err = i2c_transfer(client->adapter, msg, 2); + + return err; +} + +static int st_lsm6ds3_i2c_write(struct lsm6ds3_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err = 0; + u8 send[len + 1]; + struct i2c_msg msg; + struct i2c_client *client = to_i2c_client(cdata->dev); + + send[0] = reg_addr; + memcpy(&send[1], data, len * sizeof(u8)); + len++; + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len; + msg.buf = send; + + if (b_lock) { + mutex_lock(&cdata->bank_registers_lock); + err = i2c_transfer(client->adapter, &msg, 1); + mutex_unlock(&cdata->bank_registers_lock); + } else + err = i2c_transfer(client->adapter, &msg, 1); + + return err; +} + +static const struct st_lsm6ds3_transfer_function st_lsm6ds3_tf_i2c = { + .write = st_lsm6ds3_i2c_write, + .read = st_lsm6ds3_i2c_read, +}; + +static int st_lsm6ds3_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + int err; + struct lsm6ds3_data *cdata; + + cdata = kmalloc(sizeof(*cdata), GFP_KERNEL); + if (!cdata) + return -ENOMEM; + + cdata->dev = &client->dev; + cdata->name = client->name; + i2c_set_clientdata(client, cdata); + + cdata->tf = &st_lsm6ds3_tf_i2c; + + err = st_lsm6ds3_common_probe(cdata, client->irq); + if (err < 0) + goto free_data; + + return 0; + +free_data: + kfree(cdata); + return err; +} + +static int st_lsm6ds3_i2c_remove(struct i2c_client *client) +{ + struct lsm6ds3_data *cdata = i2c_get_clientdata(client); + + st_lsm6ds3_common_remove(cdata, client->irq); + kfree(cdata); + + return 0; +} + +#ifdef CONFIG_PM +static int st_lsm6ds3_suspend(struct device *dev) +{ + struct lsm6ds3_data *cdata = i2c_get_clientdata(to_i2c_client(dev)); + + return st_lsm6ds3_common_suspend(cdata); +} + +static int st_lsm6ds3_resume(struct device *dev) +{ + struct lsm6ds3_data *cdata = i2c_get_clientdata(to_i2c_client(dev)); + + return st_lsm6ds3_common_resume(cdata); +} + +static const struct dev_pm_ops st_lsm6ds3_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_lsm6ds3_suspend, st_lsm6ds3_resume) +}; + +#define ST_LSM6DS3_PM_OPS (&st_lsm6ds3_pm_ops) +#else /* CONFIG_PM */ +#define ST_LSM6DS3_PM_OPS NULL +#endif /* CONFIG_PM */ + +static const struct i2c_device_id st_lsm6ds3_id_table[] = { + { LSM6DS3_DEV_NAME }, + { }, +}; +MODULE_DEVICE_TABLE(i2c, st_lsm6ds3_id_table); + +static struct i2c_driver st_lsm6ds3_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st-lsm6ds3-i2c", + .pm = ST_LSM6DS3_PM_OPS, + }, + .probe = st_lsm6ds3_i2c_probe, + .remove = st_lsm6ds3_i2c_remove, + .id_table = st_lsm6ds3_id_table, +}; +module_i2c_driver(st_lsm6ds3_driver); + +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>"); +MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c_master.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c_master.c new file mode 100644 index 00000000000..49e0a08c8b5 --- /dev/null +++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c_master.c @@ -0,0 +1,1387 @@ +/* + * STMicroelectronics lsm6ds3 i2c master driver + * + * Copyright 2014 STMicroelectronics Inc. + * + * Denis Ciocca <denis.ciocca@st.com> + * + * Licensed under the GPL-2. + */ + +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/slab.h> +#include <linux/errno.h> +#include <linux/types.h> +#include <linux/delay.h> +#include <linux/iio/iio.h> +#include <linux/iio/sysfs.h> +#include <linux/iio/trigger.h> +#include <linux/iio/buffer.h> +#include <linux/iio/trigger_consumer.h> +#include <linux/iio/triggered_buffer.h> +#include <asm/unaligned.h> + +#include "st_lsm6ds3.h" + +#define ST_LSM6DS3_EXT0_INDEX 0 +#define ST_LSM6DS3_EXT1_INDEX 1 + +#define ST_LSM6DS3_I2C_MASTER_ODR_LIST_NUM 4 +#define ST_LSM6DS3_EN_BIT 0x01 +#define ST_LSM6DS3_DIS_BIT 0x00 +#define ST_LSM6DS3_HUB_REG1_ADDR 0x2e +#define ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR 0x01 +#define ST_LSM6DS3_FUNC_CFG_REG2_MASK 0x80 +#define ST_LSM6DS3_SLV0_ADDR_ADDR 0x02 +#define ST_LSM6DS3_SLV0_SUBADDR_ADDR 0x03 +#define ST_LSM6DS3_SLV0_CONFIG_ADDR 0x04 +#define ST_LSM6DS3_SLV0_CONFIG_MASK 0x07 +#define ST_LSM6DS3_SLV1_ADDR_ADDR 0x05 +#define ST_LSM6DS3_SLV1_SUBADDR_ADDR 0x06 +#define ST_LSM6DS3_SLV1_CONFIG_ADDR 0x07 +#define ST_LSM6DS3_SLV1_CONFIG_MASK 0x07 +#define ST_LSM6DS3_SLV2_ADDR_ADDR 0x08 +#define ST_LSM6DS3_SLV2_SUBADDR_ADDR 0x09 +#define ST_LSM6DS3_SLV2_CONFIG_ADDR 0x0a +#define ST_LSM6DS3_SLV2_CONFIG_MASK 0x07 +#define ST_LSM6DS3_SLV_AUX_ADDR 0x04 +#define ST_LSM6DS3_SLV_AUX_MASK 0x30 +#define ST_LSM6DS3_SLV_AUX_1 0x00 +#define ST_LSM6DS3_SLV_AUX_2 0x01 +#define ST_LSM6DS3_SLV_AUX_3 0x02 + +/* External sensors configuration */ +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL +static int lis3mdl_initialization(struct lsm6ds3_sensor_data *sdata, + int ext_num); + +#define ST_LSM6DS3_EXT0_ADDR 0x1e +#define ST_LSM6DS3_EXT0_WAI_ADDR 0x0f +#define ST_LSM6DS3_EXT0_WAI_VALUE 0x3d +#define ST_LSM6DS3_EXT0_RESET_ADDR 0x21 +#define ST_LSM6DS3_EXT0_RESET_MASK 0x04 +#define ST_LSM6DS3_EXT0_FULLSCALE_ADDR 0x21 +#define ST_LSM6DS3_EXT0_FULLSCALE_MASK 0x60 +#define ST_LSM6DS3_EXT0_FULLSCALE_VALUE 0x02 +#define ST_LSM6DS3_EXT0_ODR_ADDR 0x20 +#define ST_LSM6DS3_EXT0_ODR_MASK 0x1c +#define ST_LSM6DS3_EXT0_ODR0_HZ 10 +#define ST_LSM6DS3_EXT0_ODR0_VALUE 0x04 +#define ST_LSM6DS3_EXT0_ODR1_HZ 20 +#define ST_LSM6DS3_EXT0_ODR1_VALUE 0x05 +#define ST_LSM6DS3_EXT0_ODR2_HZ 40 +#define ST_LSM6DS3_EXT0_ODR2_VALUE 0x06 +#define ST_LSM6DS3_EXT0_ODR3_HZ 80 +#define ST_LSM6DS3_EXT0_ODR3_VALUE 0x07 +#define ST_LSM6DS3_EXT0_PW_ADDR 0x22 +#define ST_LSM6DS3_EXT0_PW_MASK 0x03 +#define ST_LSM6DS3_EXT0_PW_OFF 0x02 +#define ST_LSM6DS3_EXT0_PW_ON 0x00 +#define ST_LSM6DS3_EXT0_GAIN_VALUE 438 +#define ST_LSM6DS3_EXT0_OUT_X_L_ADDR 0x28 +#define ST_LSM6DS3_EXT0_OUT_Y_L_ADDR 0x2a +#define ST_LSM6DS3_EXT0_OUT_Z_L_ADDR 0x2c +#define ST_LSM6DS3_EXT0_READ_DATA_LEN 6 +#define ST_LSM6DS3_EXT0_BDU_ADDR 0x24 +#define ST_LSM6DS3_EXT0_BDU_MASK 0x40 +#define ST_LSM6DS3_EXT0_STD 3 +#define ST_LSM6DS3_EXT0_BOOT_FUNCTION (&lis3mdl_initialization) +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL */ + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 +static int akm09912_initialization(struct lsm6ds3_sensor_data *sdata, + int ext_num); + +#define ST_LSM6DS3_EXT0_ADDR 0x0c +#define ST_LSM6DS3_EXT0_WAI_ADDR 0x01 +#define ST_LSM6DS3_EXT0_WAI_VALUE 0x04 +#define ST_LSM6DS3_EXT0_RESET_ADDR 0x32 +#define ST_LSM6DS3_EXT0_RESET_MASK 0x01 +#define ST_LSM6DS3_EXT0_FULLSCALE_ADDR 0x00 +#define ST_LSM6DS3_EXT0_FULLSCALE_MASK 0x00 +#define ST_LSM6DS3_EXT0_FULLSCALE_VALUE 0x00 +#define ST_LSM6DS3_EXT0_ODR_ADDR 0x31 +#define ST_LSM6DS3_EXT0_ODR_MASK 0x1f +#define ST_LSM6DS3_EXT0_ODR0_HZ 10 +#define ST_LSM6DS3_EXT0_ODR0_VALUE 0x02 +#define ST_LSM6DS3_EXT0_ODR1_HZ 20 +#define ST_LSM6DS3_EXT0_ODR1_VALUE 0x04 +#define ST_LSM6DS3_EXT0_ODR2_HZ 50 +#define ST_LSM6DS3_EXT0_ODR2_VALUE 0x06 +#define ST_LSM6DS3_EXT0_ODR3_HZ 100 +#define ST_LSM6DS3_EXT0_ODR3_VALUE 0x08 +#define ST_LSM6DS3_EXT0_PW_ADDR ST_LSM6DS3_EXT0_ODR_ADDR +#define ST_LSM6DS3_EXT0_PW_MASK ST_LSM6DS3_EXT0_ODR_MASK +#define ST_LSM6DS3_EXT0_PW_OFF 0x00 +#define ST_LSM6DS3_EXT0_PW_ON ST_LSM6DS3_EXT0_ODR0_VALUE +#define ST_LSM6DS3_EXT0_GAIN_VALUE 1500 +#define ST_LSM6DS3_EXT0_OUT_X_L_ADDR 0x11 +#define ST_LSM6DS3_EXT0_OUT_Y_L_ADDR 0x13 +#define ST_LSM6DS3_EXT0_OUT_Z_L_ADDR 0x15 +#define ST_LSM6DS3_EXT0_READ_DATA_LEN 6 +#define ST_LSM6DS3_EXT0_SENSITIVITY_ADDR 0x60 +#define ST_LSM6DS3_EXT0_SENSITIVITY_LEN 3 +#define ST_LSM6DS3_EXT0_STD 3 +#define ST_LSM6DS3_EXT0_BOOT_FUNCTION (&akm09912_initialization) +#define ST_LSM6DS3_EXT0_DATA_STATUS 0x18 +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */ + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT1_LPS22HB +static int lps22hb_initialization(struct lsm6ds3_sensor_data *sdata, + int ext_num); + +#define ST_LSM6DS3_EXT1_ADDR 0x5d +#define ST_LSM6DS3_EXT1_WAI_ADDR 0x0f +#define ST_LSM6DS3_EXT1_WAI_VALUE 0xb1 +#define ST_LSM6DS3_EXT1_RESET_ADDR 0x11 +#define ST_LSM6DS3_EXT1_RESET_MASK 0x80 +#define ST_LSM6DS3_EXT1_FULLSCALE_ADDR 0x00 +#define ST_LSM6DS3_EXT1_FULLSCALE_MASK 0x00 +#define ST_LSM6DS3_EXT1_FULLSCALE_VALUE 0x00 +#define ST_LSM6DS3_EXT1_ODR_ADDR 0x10 +#define ST_LSM6DS3_EXT1_ODR_MASK 0x70 +#define ST_LSM6DS3_EXT1_ODR0_HZ 1 +#define ST_LSM6DS3_EXT1_ODR0_VALUE 0x01 +#define ST_LSM6DS3_EXT1_ODR1_HZ 10 +#define ST_LSM6DS3_EXT1_ODR1_VALUE 0x02 +#define ST_LSM6DS3_EXT1_ODR2_HZ 25 +#define ST_LSM6DS3_EXT1_ODR2_VALUE 0x03 +#define ST_LSM6DS3_EXT1_ODR3_HZ 50 +#define ST_LSM6DS3_EXT1_ODR3_VALUE 0x04 +#define ST_LSM6DS3_EXT1_PW_ADDR ST_LSM6DS3_EXT1_ODR_ADDR +#define ST_LSM6DS3_EXT1_PW_MASK ST_LSM6DS3_EXT1_ODR_MASK +#define ST_LSM6DS3_EXT1_PW_OFF 0x00 +#define ST_LSM6DS3_EXT1_PW_ON ST_LSM6DS3_EXT1_ODR0_VALUE +#define ST_LSM6DS3_EXT1_GAIN_VALUE 244 +#define ST_LSM6DS3_EXT1_OUT_P_L_ADDR 0x28 +#define ST_LSM6DS3_EXT1_OUT_T_L_ADDR 0x2b +#define ST_LSM6DS3_EXT1_READ_DATA_LEN 5 +#define ST_LSM6DS3_EXT1_BDU_ADDR 0x10 +#define ST_LSM6DS3_EXT1_BDU_MASK 0x02 +#define ST_LSM6DS3_EXT1_STD 1 +#define ST_LSM6DS3_EXT1_BOOT_FUNCTION (&lps22hb_initialization) +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_LPS22HB */ + +/* SENSORS SUFFIX NAMES */ +#define ST_LSM6DS3_EXT0_SUFFIX_NAME "magn" +#define ST_LSM6DS3_EXT1_SUFFIX_NAME "press" + +struct st_lsm6ds3_i2c_master_odr_reg { + unsigned int hz; + u8 value; +}; + +struct st_lsm6ds3_i2c_master_odr_table { + u8 addr; + u8 mask; + struct st_lsm6ds3_i2c_master_odr_reg odr_avl[ST_LSM6DS3_I2C_MASTER_ODR_LIST_NUM]; +}; + +static int st_lsm6ds3_i2c_master_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, int *val, int *val2, long mask); + +static const struct iio_chan_spec st_lsm6ds3_ext0_ch[] = { + ST_LSM6DS3_LSM_CHANNELS(IIO_MAGN, 1, 0, IIO_MOD_X, IIO_LE, + 16, 16, ST_LSM6DS3_EXT0_OUT_X_L_ADDR, 's'), + ST_LSM6DS3_LSM_CHANNELS(IIO_MAGN, 1, 1, IIO_MOD_Y, IIO_LE, + 16, 16, ST_LSM6DS3_EXT0_OUT_Y_L_ADDR, 's'), + ST_LSM6DS3_LSM_CHANNELS(IIO_MAGN, 1, 2, IIO_MOD_Z, IIO_LE, + 16, 16, ST_LSM6DS3_EXT0_OUT_Z_L_ADDR, 's'), + IIO_CHAN_SOFT_TIMESTAMP(3) +}; + +#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED +static const struct iio_chan_spec st_lsm6ds3_ext1_ch[] = { + ST_LSM6DS3_LSM_CHANNELS(IIO_PRESSURE, 0, 0, IIO_NO_MOD, IIO_LE, + 24, 24, ST_LSM6DS3_EXT1_OUT_P_L_ADDR, 'u'), + ST_LSM6DS3_LSM_CHANNELS(IIO_TEMP, 0, 1, IIO_NO_MOD, IIO_LE, + 16, 16, ST_LSM6DS3_EXT1_OUT_T_L_ADDR, 's'), + IIO_CHAN_SOFT_TIMESTAMP(2) +}; +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */ + +static int st_lsm6ds3_i2c_master_set_odr(struct lsm6ds3_sensor_data *sdata, + unsigned int odr); + +static ssize_t st_lsm6ds3_i2c_master_sysfs_sampling_frequency_avail( + struct device *dev, struct device_attribute *attr, char *buf) +{ + return scnprintf(buf, PAGE_SIZE, + "%d %d %d %d %d\n", 26, 52, 104, 208, 416); +} + +static ssize_t st_lsm6ds3_i2c_master_sysfs_get_sampling_frequency( + struct device *dev, struct device_attribute *attr, char *buf) +{ + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", sdata->c_odr); +} + +static ssize_t st_lsm6ds3_i2c_master_sysfs_set_sampling_frequency( + struct device *dev, struct device_attribute *attr, + const char *buf, size_t size) +{ + int err, err2 = -EINVAL; + unsigned int odr; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + err = kstrtoint(buf, 10, &odr); + if (err < 0) + return err; + + mutex_lock(&indio_dev->mlock); + + switch (odr) { + case 26: + case 52: + case 104: + case 208: + case 416: + if (sdata->c_odr != odr) { + + mutex_lock(&sdata->cdata->passthrough_lock); + + disable_irq(sdata->cdata->irq); + st_lsm6ds3_flush_works(); + + err = st_lsm6ds3_enable_passthrough(sdata->cdata, true); + if (err < 0) { + enable_irq(sdata->cdata->irq); + mutex_unlock(&sdata->cdata->passthrough_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + err2 = st_lsm6ds3_i2c_master_set_odr(sdata, odr); + + err = st_lsm6ds3_enable_passthrough(sdata->cdata, + false); + if (err < 0) { + enable_irq(sdata->cdata->irq); + mutex_unlock(&sdata->cdata->passthrough_lock); + mutex_unlock(&indio_dev->mlock); + return err; + } + + enable_irq(sdata->cdata->irq); + mutex_unlock(&sdata->cdata->passthrough_lock); + } + break; + default: + err2 = -EINVAL; + break; + } + + mutex_unlock(&indio_dev->mlock); + + return err2 < 0 ? err2 : size; +} + +static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, + st_lsm6ds3_i2c_master_sysfs_get_sampling_frequency, + st_lsm6ds3_i2c_master_sysfs_set_sampling_frequency); + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL( + st_lsm6ds3_i2c_master_sysfs_sampling_frequency_avail); +static ST_LSM6DS3_FIFO_LENGHT(); +static ST_LSM6DS3_FIFO_FLUSH(); + +static struct attribute *st_lsm6ds3_ext0_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_hw_fifo_lenght.dev_attr.attr, + &iio_dev_attr_flush.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6ds3_ext0_attribute_group = { + .attrs = st_lsm6ds3_ext0_attributes, +}; + +static const struct iio_info st_lsm6ds3_ext0_info = { + .driver_module = THIS_MODULE, + .attrs = &st_lsm6ds3_ext0_attribute_group, + .read_raw = &st_lsm6ds3_i2c_master_read_raw, +}; + +static struct attribute *st_lsm6ds3_ext1_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_hw_fifo_lenght.dev_attr.attr, + &iio_dev_attr_flush.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6ds3_ext1_attribute_group = { + .attrs = st_lsm6ds3_ext1_attributes, +}; + +static const struct iio_info st_lsm6ds3_ext1_info = { + .driver_module = THIS_MODULE, + .attrs = &st_lsm6ds3_ext1_attribute_group, + .read_raw = &st_lsm6ds3_i2c_master_read_raw, +}; + +struct st_lsm6ds3_iio_info_data { + char suffix_name[20]; + struct iio_info *info; + struct iio_chan_spec *channels; + int num_channels; +}; + +struct st_lsm6ds3_reg { + u8 addr; + u8 mask; + u8 def_value; +}; + +struct st_lsm6ds3_power_reg { + u8 addr; + u8 mask; + u8 off_value; + u8 on_value; + bool isodr; +}; + +struct st_lsm6ds3_custom_function { + int (*boot_initialization) (struct lsm6ds3_sensor_data *sdata, + int ext_num); +}; + +static struct st_lsm6ds3_exs_list { + struct st_lsm6ds3_reg wai; + struct st_lsm6ds3_reg reset; + struct st_lsm6ds3_reg fullscale; + struct st_lsm6ds3_i2c_master_odr_table odr; + struct st_lsm6ds3_power_reg power; + u8 fullscale_value; + u8 samples_to_discard; + u8 read_data_len; + u8 num_data_channels; + bool available; + unsigned int gain; + struct i2c_board_info board_info; + struct st_lsm6ds3_iio_info_data data; + struct st_lsm6ds3_custom_function cf; +} st_lsm6ds3_exs_list[] = { + { + .wai = { + .addr = ST_LSM6DS3_EXT0_WAI_ADDR, + .def_value = ST_LSM6DS3_EXT0_WAI_VALUE, + }, + .reset = { + .addr = ST_LSM6DS3_EXT0_RESET_ADDR, + .mask = ST_LSM6DS3_EXT0_RESET_MASK, + }, + .fullscale = { + .addr = ST_LSM6DS3_EXT0_FULLSCALE_ADDR, + .mask = ST_LSM6DS3_EXT0_FULLSCALE_MASK, + .def_value = ST_LSM6DS3_EXT0_FULLSCALE_VALUE, + }, + .odr = { + .addr = ST_LSM6DS3_EXT0_ODR_ADDR, + .mask = ST_LSM6DS3_EXT0_ODR_MASK, + .odr_avl = { + { + .hz = ST_LSM6DS3_EXT0_ODR0_HZ, + .value = ST_LSM6DS3_EXT0_ODR0_VALUE, + }, + { + .hz = ST_LSM6DS3_EXT0_ODR1_HZ, + .value = ST_LSM6DS3_EXT0_ODR1_VALUE, + }, + { + .hz = ST_LSM6DS3_EXT0_ODR2_HZ, + .value = ST_LSM6DS3_EXT0_ODR2_VALUE, + }, + { + .hz = ST_LSM6DS3_EXT0_ODR3_HZ, + .value = ST_LSM6DS3_EXT0_ODR3_VALUE, + }, + }, + }, + .power = { + .addr = ST_LSM6DS3_EXT0_PW_ADDR, + .mask = ST_LSM6DS3_EXT0_PW_MASK, + .off_value = ST_LSM6DS3_EXT0_PW_OFF, + .on_value = ST_LSM6DS3_EXT0_PW_ON, + }, + .samples_to_discard = ST_LSM6DS3_EXT0_STD, + .read_data_len = ST_LSM6DS3_EXT0_READ_DATA_LEN, + .num_data_channels = ARRAY_SIZE(st_lsm6ds3_ext0_ch) - 1, + .available = false, + .gain = ST_LSM6DS3_EXT0_GAIN_VALUE, + .board_info = { .addr = ST_LSM6DS3_EXT0_ADDR, }, + .data = { + .suffix_name = ST_LSM6DS3_EXT0_SUFFIX_NAME, + .info = (struct iio_info *)&st_lsm6ds3_ext0_info, + .channels = (struct iio_chan_spec *)&st_lsm6ds3_ext0_ch, + .num_channels = ARRAY_SIZE(st_lsm6ds3_ext0_ch), + }, + .cf.boot_initialization = ST_LSM6DS3_EXT0_BOOT_FUNCTION, + }, +#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED + { + .wai = { + .addr = ST_LSM6DS3_EXT1_WAI_ADDR, + .def_value = ST_LSM6DS3_EXT1_WAI_VALUE, + }, + .reset = { + .addr = ST_LSM6DS3_EXT1_RESET_ADDR, + .mask = ST_LSM6DS3_EXT1_RESET_MASK, + }, + .fullscale = { + .addr = ST_LSM6DS3_EXT1_FULLSCALE_ADDR, + .mask = ST_LSM6DS3_EXT1_FULLSCALE_MASK, + .def_value = ST_LSM6DS3_EXT1_FULLSCALE_VALUE, + }, + .odr = { + .addr = ST_LSM6DS3_EXT1_ODR_ADDR, + .mask = ST_LSM6DS3_EXT1_ODR_MASK, + .odr_avl = { + { + .hz = ST_LSM6DS3_EXT1_ODR0_HZ, + .value = ST_LSM6DS3_EXT1_ODR0_VALUE, + }, + { + .hz = ST_LSM6DS3_EXT1_ODR1_HZ, + .value = ST_LSM6DS3_EXT1_ODR1_VALUE, + }, + { + .hz = ST_LSM6DS3_EXT1_ODR2_HZ, + .value = ST_LSM6DS3_EXT1_ODR2_VALUE, + }, + { + .hz = ST_LSM6DS3_EXT1_ODR3_HZ, + .value = ST_LSM6DS3_EXT1_ODR3_VALUE, + }, + }, + }, + .power = { + .addr = ST_LSM6DS3_EXT1_PW_ADDR, + .mask = ST_LSM6DS3_EXT1_PW_MASK, + .off_value = ST_LSM6DS3_EXT1_PW_OFF, + .on_value = ST_LSM6DS3_EXT1_PW_ON, + }, + .samples_to_discard = ST_LSM6DS3_EXT1_STD, + .read_data_len = ST_LSM6DS3_EXT1_READ_DATA_LEN, + .num_data_channels = ARRAY_SIZE(st_lsm6ds3_ext1_ch) - 1, + .available = false, + .gain = ST_LSM6DS3_EXT1_GAIN_VALUE, + .board_info = { .addr = ST_LSM6DS3_EXT1_ADDR, }, + .data = { + .suffix_name = ST_LSM6DS3_EXT1_SUFFIX_NAME, + .info = (struct iio_info *)&st_lsm6ds3_ext1_info, + .channels = (struct iio_chan_spec *)&st_lsm6ds3_ext1_ch, + .num_channels = ARRAY_SIZE(st_lsm6ds3_ext1_ch), + }, + .cf.boot_initialization = ST_LSM6DS3_EXT1_BOOT_FUNCTION, + }, +#else /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */ + { + }, +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */ +}; + +static int st_lsm6ds3_i2c_master_read(struct i2c_client *client, + u8 reg_addr, int len, u8 *data) +{ + struct i2c_msg msg[2]; + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = ®_addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = data; + + return i2c_transfer(client->adapter, msg, 2); +} + +static int st_lsm6ds3_i2c_master_write(struct i2c_client *client, + u8 reg_addr, int len, u8 *data) +{ + u8 send[len + 1]; + struct i2c_msg msg; + + send[0] = reg_addr; + memcpy(&send[1], data, len * sizeof(u8)); + len++; + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len; + msg.buf = send; + + return i2c_transfer(client->adapter, &msg, 1); +} + +static int st_lsm6ds3_i2c_master_write_data_with_mask(struct i2c_client *client, + u8 reg_addr, u8 mask, u8 data) +{ + int err; + u8 new_data = 0x00, old_data = 0x00; + + err = st_lsm6ds3_i2c_master_read(client, reg_addr, 1, &old_data); + if (err < 0) + return err; + + new_data = ((old_data & (~mask)) | ((data << __ffs(mask)) & mask)); + + if (new_data == old_data) + return 1; + + return st_lsm6ds3_i2c_master_write(client, reg_addr, 1, &new_data); +} + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL +static int lis3mdl_initialization(struct lsm6ds3_sensor_data *sdata, + int ext_num) +{ + + return st_lsm6ds3_i2c_master_write_data_with_mask( + sdata->cdata->master_client[ext_num], + ST_LSM6DS3_EXT0_BDU_ADDR, + ST_LSM6DS3_EXT0_BDU_MASK, ST_LSM6DS3_EN_BIT); +} +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL */ + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 +static int akm09912_initialization(struct lsm6ds3_sensor_data *sdata, + int ext_num) +{ + int err; + u8 data[ST_LSM6DS3_EXT0_SENSITIVITY_LEN]; + + err = st_lsm6ds3_i2c_master_read(sdata->cdata->master_client[ext_num], + ST_LSM6DS3_EXT0_SENSITIVITY_ADDR, + ST_LSM6DS3_EXT0_SENSITIVITY_LEN, + data); + if (err < 0) + return err; + + sdata->c_gain[0] *= ((((data[0] - 128) * 1000) >> 8) + 1000); + sdata->c_gain[1] *= ((((data[1] - 128) * 1000) >> 8) + 1000); + sdata->c_gain[2] *= ((((data[2] - 128) * 1000) >> 8) + 1000); + + return 0; +} +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */ + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT1_LPS22HB +static int lps22hb_initialization(struct lsm6ds3_sensor_data *sdata, + int ext_num) +{ + + return st_lsm6ds3_i2c_master_write_data_with_mask( + sdata->cdata->master_client[ext_num], + ST_LSM6DS3_EXT1_BDU_ADDR, + ST_LSM6DS3_EXT1_BDU_MASK, ST_LSM6DS3_EN_BIT); +} +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_LPS22HB */ + +static int st_lsm6ds3_i2c_master_set_odr(struct lsm6ds3_sensor_data *sdata, + unsigned int odr) +{ + int err, i, ext_num = sdata->sindex - ST_INDIO_DEV_EXT0; + + for (i = 0; i < ST_LSM6DS3_I2C_MASTER_ODR_LIST_NUM; i++) { + if (st_lsm6ds3_exs_list[ext_num].odr.odr_avl[i].hz >= odr) + break; + } + if (i == ST_LSM6DS3_I2C_MASTER_ODR_LIST_NUM) + i--; + + if (sdata->cdata->sensors_enabled & (1 << sdata->sindex)) { + err = st_lsm6ds3_i2c_master_write_data_with_mask( + sdata->cdata->master_client[ext_num], + st_lsm6ds3_exs_list[ext_num].odr.addr, + st_lsm6ds3_exs_list[ext_num].odr.mask, + st_lsm6ds3_exs_list[ext_num].odr.odr_avl[i].value); + if (err < 0) + return err; + + sdata->cdata->ext_samples_to_discard[ext_num] = + st_lsm6ds3_exs_list[ext_num].samples_to_discard; + + sdata->c_odr = odr; + + if (st_lsm6ds3_exs_list[ext_num].power.isodr) + st_lsm6ds3_exs_list[ext_num].power.on_value = + st_lsm6ds3_exs_list[ext_num].odr.odr_avl[i].value; + + st_lsm6ds3_reconfigure_fifo(sdata->cdata, false); + } else { + sdata->c_odr = odr; + + if (st_lsm6ds3_exs_list[ext_num].power.isodr) + st_lsm6ds3_exs_list[ext_num].power.on_value = + st_lsm6ds3_exs_list[ext_num].odr.odr_avl[i].value; + } + + return 0; +} + +static int st_lsm6ds3_i2c_master_set_enable(struct lsm6ds3_sensor_data *sdata, + bool enable) +{ + u8 reg_value; + int err, ext_num = sdata->sindex - ST_INDIO_DEV_EXT0; + + if (enable) + reg_value = st_lsm6ds3_exs_list[ext_num].power.on_value; + else + reg_value = st_lsm6ds3_exs_list[ext_num].power.off_value; + + err = st_lsm6ds3_i2c_master_write_data_with_mask( + sdata->cdata->master_client[ext_num], + st_lsm6ds3_exs_list[ext_num].power.addr, + st_lsm6ds3_exs_list[ext_num].power.mask, + reg_value); + if (err < 0) + return err; + + sdata->cdata->ext_samples_to_discard[ext_num] = + st_lsm6ds3_exs_list[ext_num].samples_to_discard; + + if (enable) + sdata->cdata->sensors_enabled |= (1 << sdata->sindex); + else + sdata->cdata->sensors_enabled &= ~(1 << sdata->sindex); + + return 0; +} + +static int st_lsm6ds3_i2c_master_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *ch, int *val, int *val2, long mask) +{ + int err; + u8 outdata[(ch->scan_type.storagebits >> 3) + 1]; + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + switch (mask) { + case IIO_CHAN_INFO_RAW: + mutex_lock(&indio_dev->mlock); + + if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + + mutex_lock(&sdata->cdata->passthrough_lock); + + err = st_lsm6ds3_enable_accel_dependency(sdata, true); + if (err < 0) + return err; + + disable_irq(sdata->cdata->irq); + st_lsm6ds3_flush_works(); + + err = st_lsm6ds3_enable_passthrough(sdata->cdata, true); + if (err < 0) + goto read_raw_reset_passthrough; + + err = st_lsm6ds3_i2c_master_set_enable(sdata, true); + if (err < 0) + goto read_raw_reset_passthrough; + + memset(outdata, 0, (ch->scan_type.storagebits >> 3) + 1); + + msleep(200); + + err = st_lsm6ds3_i2c_master_read( + sdata->cdata->master_client[sdata->sindex - + ST_INDIO_DEV_EXT0], ch->address, + ch->scan_type.storagebits >> 3, outdata); + if (err < 0) + goto read_raw_reset_passthrough; + + err = st_lsm6ds3_i2c_master_set_enable(sdata, false); + if (err < 0) + goto read_raw_reset_passthrough; + + err = st_lsm6ds3_enable_passthrough(sdata->cdata, false); + if (err < 0) + goto read_raw_reset_passthrough; + + enable_irq(sdata->cdata->irq); + mutex_unlock(&sdata->cdata->passthrough_lock); + + err = st_lsm6ds3_enable_accel_dependency(sdata, false); + if (err < 0) + return err; + + if ((ch->scan_type.storagebits >> 3) > 2) + *val = (s32)get_unaligned_le32(outdata); + else + *val = (s16)get_unaligned_le16(outdata); + + *val = *val >> ch->scan_type.shift; + + mutex_unlock(&indio_dev->mlock); + + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sdata->c_gain[ch->scan_index]; + + if (ch->type == IIO_TEMP) { + *val = 1; + *val2 = 0; + return IIO_VAL_INT; + } + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 + if (sdata->sindex == ST_INDIO_DEV_EXT0) + return IIO_VAL_INT_PLUS_NANO; +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */ + + return IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } + + return 0; + +read_raw_reset_passthrough: + st_lsm6ds3_enable_passthrough(sdata->cdata, false); + enable_irq(sdata->cdata->irq); + mutex_unlock(&sdata->cdata->passthrough_lock); + st_lsm6ds3_enable_accel_dependency(sdata, false); + return err; +} + +static int st_lsm6ds3_i2c_master_buffer_preenable(struct iio_dev *indio_dev) +{ + int err; + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + mutex_lock(&sdata->cdata->passthrough_lock); + + err = st_lsm6ds3_enable_accel_dependency(sdata, true); + if (err < 0) + return err; + + disable_irq(sdata->cdata->irq); + st_lsm6ds3_flush_works(); + + err = st_lsm6ds3_enable_passthrough(sdata->cdata, true); + if (err < 0) + goto preenable_reset_passthrough; + + err = st_lsm6ds3_i2c_master_set_enable(sdata, true); + if (err < 0) + goto preenable_reset_passthrough; + + err = st_lsm6ds3_enable_passthrough(sdata->cdata, false); + if (err < 0) + goto preenable_reset_passthrough; + + enable_irq(sdata->cdata->irq); + mutex_unlock(&sdata->cdata->passthrough_lock); + + err = st_lsm6ds3_reconfigure_fifo(sdata->cdata, true); + if (err < 0) + return err; + + return iio_sw_buffer_preenable(indio_dev); + +preenable_reset_passthrough: + st_lsm6ds3_enable_passthrough(sdata->cdata, false); + enable_irq(sdata->cdata->irq); + mutex_unlock(&sdata->cdata->passthrough_lock); + + return err; +} + +static int st_lsm6ds3_i2c_master_buffer_postenable(struct iio_dev *indio_dev) +{ + int err; + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + sdata->buffer_data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL); + if (sdata->buffer_data == NULL) + return -ENOMEM; + + err = iio_triggered_buffer_postenable(indio_dev); + if (err < 0) + goto free_buffer_data; + + return 0; + +free_buffer_data: + kfree(sdata->buffer_data); + + return err; +} + +static int st_lsm6ds3_i2c_master_buffer_predisable(struct iio_dev *indio_dev) +{ + int err; + struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev); + + err = iio_triggered_buffer_predisable(indio_dev); + if (err < 0) + return err; + + mutex_lock(&sdata->cdata->passthrough_lock); + + disable_irq(sdata->cdata->irq); + st_lsm6ds3_flush_works(); + + err = st_lsm6ds3_enable_passthrough(sdata->cdata, true); + if (err < 0) + goto predisable_reset_passthrough; + + err = st_lsm6ds3_i2c_master_set_enable(sdata, false); + if (err < 0) + goto predisable_reset_passthrough; + + err = st_lsm6ds3_enable_passthrough(sdata->cdata, false); + if (err < 0) + goto predisable_reset_passthrough; + + enable_irq(sdata->cdata->irq); + mutex_unlock(&sdata->cdata->passthrough_lock); + + err = st_lsm6ds3_enable_accel_dependency(sdata, false); + if (err < 0) + return err; + + err = st_lsm6ds3_reconfigure_fifo(sdata->cdata, true); + if (err < 0) + return err; + + kfree(sdata->buffer_data); + + return 0; + +predisable_reset_passthrough: + st_lsm6ds3_enable_passthrough(sdata->cdata, false); + enable_irq(sdata->cdata->irq); + mutex_unlock(&sdata->cdata->passthrough_lock); + + return err; +} + +static const struct iio_trigger_ops st_lsm6ds3_i2c_master_trigger_ops = { + .owner = THIS_MODULE, + .set_trigger_state = &st_lsm6ds3_trig_set_state, +}; + +int st_lsm6ds3_i2c_master_allocate_trigger(struct lsm6ds3_data *cdata, + int dev_index) +{ + int err; + + cdata->trig[dev_index] = iio_trigger_alloc("%s-trigger", + cdata->indio_dev[dev_index]->name); + if (!cdata->trig[dev_index]) { + dev_err(cdata->dev, "failed to allocate iio trigger.\n"); + return -ENOMEM; + } + + iio_trigger_set_drvdata(cdata->trig[dev_index], + cdata->indio_dev[dev_index]); + cdata->trig[dev_index]->ops = &st_lsm6ds3_i2c_master_trigger_ops; + cdata->trig[dev_index]->dev.parent = cdata->dev; + + err = iio_trigger_register(cdata->trig[dev_index]); + if (err < 0) { + dev_err(cdata->dev, "failed to register iio trigger.\n"); + goto deallocate_trigger; + } + + cdata->indio_dev[dev_index]->trig = cdata->trig[dev_index]; + + return 0; + +deallocate_trigger: + iio_trigger_free(cdata->trig[dev_index]); + return err; +} + +static void st_lsm6ds3_i2c_master_deallocate_trigger(struct lsm6ds3_data *cdata, + int dev_index) +{ + iio_trigger_unregister(cdata->trig[dev_index]); +} + +static const struct iio_buffer_setup_ops st_lsm6ds3_i2c_master_buffer_setup_ops = { + .preenable = &st_lsm6ds3_i2c_master_buffer_preenable, + .postenable = &st_lsm6ds3_i2c_master_buffer_postenable, + .predisable = &st_lsm6ds3_i2c_master_buffer_predisable, +}; + +static inline irqreturn_t st_lsm6ds3_i2c_master_handler_empty(int irq, void *p) +{ + return IRQ_HANDLED; +} + +static int st_lsm6ds3_i2c_master_allocate_buffer(struct lsm6ds3_data *cdata, + int dev_index) +{ + return iio_triggered_buffer_setup(cdata->indio_dev[dev_index], + &st_lsm6ds3_i2c_master_handler_empty, NULL, + &st_lsm6ds3_i2c_master_buffer_setup_ops); +} + +static void st_lsm6ds3_i2c_master_deallocate_buffer(struct lsm6ds3_data *cdata, + int dev_index) +{ + iio_triggered_buffer_cleanup(cdata->indio_dev[dev_index]); +} + +static int st_lsm6ds3_i2c_master_send_sensor_hub_parameters( + struct lsm6ds3_sensor_data *sdata, int ext_num) +{ + int err; + u8 i2c_address_reg, data_start_address_reg, slave_num; + u8 i2c_address, data_start_address, config_reg_addr, config_reg_mask; +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 + u8 i2c_address_reg_akm, data_start_address_reg_akm, temp_reg; + u8 config_reg_addr_akm, config_reg_mask_akm; +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */ + + switch (ext_num) { + case ST_LSM6DS3_EXT0_INDEX: + i2c_address_reg = ST_LSM6DS3_SLV0_ADDR_ADDR; + data_start_address_reg = ST_LSM6DS3_SLV0_SUBADDR_ADDR; + config_reg_addr = ST_LSM6DS3_SLV0_CONFIG_ADDR; + config_reg_mask = ST_LSM6DS3_SLV0_CONFIG_MASK; + break; + case ST_LSM6DS3_EXT1_INDEX: + i2c_address_reg = ST_LSM6DS3_SLV1_ADDR_ADDR; + data_start_address_reg = ST_LSM6DS3_SLV1_SUBADDR_ADDR; + config_reg_addr = ST_LSM6DS3_SLV1_CONFIG_ADDR; + config_reg_mask = ST_LSM6DS3_SLV1_CONFIG_MASK; + break; + default: + return -EINVAL; + } + + i2c_address = (st_lsm6ds3_exs_list[ext_num].board_info.addr << 1) | + ST_LSM6DS3_EN_BIT; + + data_start_address = + st_lsm6ds3_exs_list[ext_num].data.channels[0].address; + + mutex_lock(&sdata->cdata->bank_registers_lock); + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR, + ST_LSM6DS3_FUNC_CFG_REG2_MASK, + ST_LSM6DS3_EN_BIT, false); + if (err < 0) + goto st_lsm6ds3_init_sensor_mutex_unlock; + + err = sdata->cdata->tf->write(sdata->cdata, i2c_address_reg, + 1, &i2c_address, false); + if (err < 0) + goto st_lsm6ds3_init_sensor_mutex_unlock; + + err = sdata->cdata->tf->write(sdata->cdata, + data_start_address_reg, + 1, &data_start_address, false); + if (err < 0) + goto st_lsm6ds3_init_sensor_mutex_unlock; + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + config_reg_addr, config_reg_mask, + st_lsm6ds3_exs_list[ext_num].read_data_len, + false); + if (err < 0) + goto st_lsm6ds3_init_sensor_mutex_unlock; + + if (ext_num == ST_LSM6DS3_EXT0_INDEX) { + if (sdata->cdata->ext1_available) { +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 + slave_num = ST_LSM6DS3_SLV_AUX_3; + i2c_address_reg_akm = ST_LSM6DS3_SLV2_ADDR_ADDR; + data_start_address_reg_akm = ST_LSM6DS3_SLV2_SUBADDR_ADDR; + config_reg_addr_akm = ST_LSM6DS3_SLV2_CONFIG_ADDR; + config_reg_mask_akm = ST_LSM6DS3_SLV2_CONFIG_MASK; +#else /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */ + slave_num = ST_LSM6DS3_SLV_AUX_2; +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */ + } else { +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 + slave_num = ST_LSM6DS3_SLV_AUX_2; + i2c_address_reg_akm = ST_LSM6DS3_SLV1_ADDR_ADDR; + data_start_address_reg_akm = ST_LSM6DS3_SLV1_SUBADDR_ADDR; + config_reg_addr_akm = ST_LSM6DS3_SLV1_CONFIG_ADDR; + config_reg_mask_akm = ST_LSM6DS3_SLV1_CONFIG_MASK; +#else /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */ + slave_num = ST_LSM6DS3_SLV_AUX_1; +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */ + } + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_SLV_AUX_ADDR, + ST_LSM6DS3_SLV_AUX_MASK, + slave_num, false); + if (err < 0) + goto st_lsm6ds3_init_sensor_mutex_unlock; + +#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 + temp_reg = (ST_LSM6DS3_EXT0_ADDR << 1) | ST_LSM6DS3_EN_BIT; + + err = sdata->cdata->tf->write(sdata->cdata, i2c_address_reg_akm, + 1, &temp_reg, false); + if (err < 0) + goto st_lsm6ds3_init_sensor_mutex_unlock; + + temp_reg = ST_LSM6DS3_EXT0_DATA_STATUS; + + err = sdata->cdata->tf->write(sdata->cdata, + data_start_address_reg_akm, + 1, &temp_reg, false); + if (err < 0) + goto st_lsm6ds3_init_sensor_mutex_unlock; + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + config_reg_addr_akm, config_reg_mask_akm, + 1, false); + if (err < 0) + goto st_lsm6ds3_init_sensor_mutex_unlock; +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */ + } + + err = st_lsm6ds3_write_data_with_mask(sdata->cdata, + ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR, + ST_LSM6DS3_FUNC_CFG_REG2_MASK, + ST_LSM6DS3_DIS_BIT, false); + if (err < 0) + goto st_lsm6ds3_init_sensor_mutex_unlock; + + mutex_unlock(&sdata->cdata->bank_registers_lock); + + return 0; + +st_lsm6ds3_init_sensor_mutex_unlock: + mutex_unlock(&sdata->cdata->bank_registers_lock); + return err; +} + +static int st_lsm6ds3_i2c_master_init_sensor(struct lsm6ds3_sensor_data *sdata, + int ext_num, int dev_index) +{ + int err; + + mutex_lock(&sdata->cdata->passthrough_lock); + + err = st_lsm6ds3_enable_passthrough(sdata->cdata, true); + if (err < 0) + goto unlock_passthrough; + + sdata->c_gain[0] = st_lsm6ds3_exs_list[ext_num].gain; + sdata->c_gain[1] = st_lsm6ds3_exs_list[ext_num].gain; + sdata->c_gain[2] = st_lsm6ds3_exs_list[ext_num].gain; + + if ((st_lsm6ds3_exs_list[ext_num].power.addr == + st_lsm6ds3_exs_list[ext_num].odr.addr) && + (st_lsm6ds3_exs_list[ext_num].power.mask == + st_lsm6ds3_exs_list[ext_num].odr.mask)) + st_lsm6ds3_exs_list[ext_num].power.isodr = true; + else + st_lsm6ds3_exs_list[ext_num].power.isodr = false; + + err = st_lsm6ds3_i2c_master_write_data_with_mask( + sdata->cdata->master_client[ext_num], + st_lsm6ds3_exs_list[ext_num].reset.addr, + st_lsm6ds3_exs_list[ext_num].reset.mask, + ST_LSM6DS3_EN_BIT); + if (err < 0) + goto unlock_passthrough; + + usleep_range(3000, 8000); + + if (st_lsm6ds3_exs_list[ext_num].fullscale.addr > 0) { + err = st_lsm6ds3_i2c_master_write_data_with_mask( + sdata->cdata->master_client[ext_num], + st_lsm6ds3_exs_list[ext_num].fullscale.addr, + st_lsm6ds3_exs_list[ext_num].fullscale.mask, + st_lsm6ds3_exs_list[ext_num].fullscale.def_value); + if (err < 0) + goto unlock_passthrough; + } + + if (st_lsm6ds3_exs_list[ext_num].cf.boot_initialization != NULL) { + err = st_lsm6ds3_exs_list[ext_num].cf.boot_initialization(sdata, ext_num); + if (err < 0) + goto unlock_passthrough; + } + + err = st_lsm6ds3_i2c_master_set_enable(sdata, false); + if (err < 0) + goto unlock_passthrough; + + err = st_lsm6ds3_i2c_master_set_odr(sdata, 26); + if (err < 0) + goto unlock_passthrough; + + err = st_lsm6ds3_i2c_master_send_sensor_hub_parameters(sdata, ext_num); + if (err < 0) + goto unlock_passthrough; + + err = st_lsm6ds3_enable_passthrough(sdata->cdata, false); + if (err < 0) + goto unlock_passthrough; + + mutex_unlock(&sdata->cdata->passthrough_lock); + + return 0; + +unlock_passthrough: + mutex_unlock(&sdata->cdata->passthrough_lock); + return err; +} + +static int st_lsm6ds3_i2c_master_allocate_device(struct lsm6ds3_data *cdata, + int ext_num) +{ + int err, dev_index; + struct lsm6ds3_sensor_data *sdata_ext; + + switch (ext_num) { + case ST_LSM6DS3_EXT0_INDEX: + dev_index = ST_INDIO_DEV_EXT0; + break; + case ST_LSM6DS3_EXT1_INDEX: + dev_index = ST_INDIO_DEV_EXT1; + break; + default: + return -EINVAL; + } + + cdata->indio_dev[dev_index] = iio_device_alloc(sizeof(*sdata_ext)); + if (!cdata->indio_dev[dev_index]) + return -ENOMEM; + + sdata_ext = iio_priv(cdata->indio_dev[dev_index]); + sdata_ext->cdata = cdata; + sdata_ext->sindex = dev_index; + + sdata_ext->num_data_channels = + st_lsm6ds3_exs_list[ext_num].num_data_channels; + + cdata->indio_dev[dev_index]->name = kasprintf(GFP_KERNEL, + "%s_%s", cdata->name, + st_lsm6ds3_exs_list[ext_num].data.suffix_name); + + cdata->indio_dev[dev_index]->info = + st_lsm6ds3_exs_list[ext_num].data.info; + cdata->indio_dev[dev_index]->channels = + st_lsm6ds3_exs_list[ext_num].data.channels; + cdata->indio_dev[dev_index]->num_channels = + st_lsm6ds3_exs_list[ext_num].data.num_channels; + + cdata->indio_dev[dev_index]->modes = INDIO_DIRECT_MODE; + + err = st_lsm6ds3_i2c_master_init_sensor(sdata_ext, ext_num, dev_index); + if (err < 0) + goto iio_device_free; + + err = st_lsm6ds3_i2c_master_allocate_buffer(cdata, dev_index); + if (err < 0) + goto iio_device_free; + + err = st_lsm6ds3_i2c_master_allocate_trigger(cdata, dev_index); + if (err < 0) + goto iio_deallocate_buffer; + + err = iio_device_register(cdata->indio_dev[dev_index]); + if (err < 0) + goto iio_deallocate_trigger; + + return 0; + +iio_deallocate_trigger: + st_lsm6ds3_i2c_master_deallocate_trigger(cdata, dev_index); +iio_deallocate_buffer: + st_lsm6ds3_i2c_master_deallocate_buffer(cdata, dev_index); +iio_device_free: + iio_device_free(cdata->indio_dev[dev_index]); + + return err; +} + +static void st_lsm6ds3_i2c_master_deallocate_device(struct lsm6ds3_data *cdata, + int ext_num) +{ + int dev_index; + + switch (ext_num) { + case ST_LSM6DS3_EXT0_INDEX: + dev_index = ST_INDIO_DEV_EXT0; + break; + case ST_LSM6DS3_EXT1_INDEX: + dev_index = ST_INDIO_DEV_EXT1; + break; + default: + return; + } + + iio_device_unregister(cdata->indio_dev[dev_index]); + st_lsm6ds3_i2c_master_deallocate_trigger(cdata, dev_index); + st_lsm6ds3_i2c_master_deallocate_buffer(cdata, dev_index); + iio_device_free(cdata->indio_dev[dev_index]); +} + +int st_lsm6ds3_i2c_master_probe(struct lsm6ds3_data *cdata) +{ + u8 wai; + int err; + struct i2c_client *client = to_i2c_client(cdata->dev); + + cdata->ext0_available = false; + cdata->ext1_available = false; + + cdata->ext0_samples_in_pattern = 0; + cdata->ext1_samples_in_pattern = 0; + + sprintf(st_lsm6ds3_exs_list[ST_LSM6DS3_EXT0_INDEX].board_info.type, + "%s_ext%d", client->name, ST_LSM6DS3_EXT0_INDEX); + + cdata->master_client[ST_LSM6DS3_EXT0_INDEX] = + i2c_new_device(client->adapter, + &st_lsm6ds3_exs_list[ST_LSM6DS3_EXT0_INDEX].board_info); + if (!cdata->master_client[ST_LSM6DS3_EXT0_INDEX]) + return -ENOMEM; + +#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED + sprintf(st_lsm6ds3_exs_list[ST_LSM6DS3_EXT1_INDEX].board_info.type, + "%s_ext%d", client->name, ST_LSM6DS3_EXT1_INDEX); + + cdata->master_client[ST_LSM6DS3_EXT1_INDEX] = + i2c_new_device(client->adapter, + &st_lsm6ds3_exs_list[ST_LSM6DS3_EXT1_INDEX].board_info); + if (!cdata->master_client[ST_LSM6DS3_EXT1_INDEX]) { + err = -ENOMEM; + goto unregister_ext0_i2c_client; + } +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */ + + mutex_lock(&cdata->passthrough_lock); + + err = st_lsm6ds3_enable_passthrough(cdata, true); + if (err < 0) + goto master_probe_passthrough_lock; + + err = st_lsm6ds3_i2c_master_read( + cdata->master_client[ST_LSM6DS3_EXT0_INDEX], + st_lsm6ds3_exs_list[ST_LSM6DS3_EXT0_INDEX].wai.addr, 1, &wai); + if (err < 0) { + dev_err(cdata->dev, "external sensor 0 not available\n"); + i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT0_INDEX]); + } else { + if (wai != st_lsm6ds3_exs_list[ST_LSM6DS3_EXT0_INDEX].wai.def_value) { + dev_err(cdata->dev, "wai value of external sensor 0 mismatch\n"); + i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT0_INDEX]); + } else + cdata->ext0_available = true; + } + +#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED + err = st_lsm6ds3_i2c_master_read( + cdata->master_client[ST_LSM6DS3_EXT1_INDEX], + st_lsm6ds3_exs_list[ST_LSM6DS3_EXT1_INDEX].wai.addr, 1, &wai); + if (err < 0) { + dev_err(cdata->dev, "external sensor 1 not available\n"); + i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT1_INDEX]); + } else { + if (wai != st_lsm6ds3_exs_list[ST_LSM6DS3_EXT1_INDEX].wai.def_value) { + dev_err(cdata->dev, "wai value of external sensor 1 mismatch\n"); + i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT1_INDEX]); + } else + cdata->ext1_available = true; + } +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */ + + err = st_lsm6ds3_enable_passthrough(cdata, false); + if (err < 0) { + if (cdata->ext0_available) + i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT0_INDEX]); + +#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED + if (cdata->ext1_available) + i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT1_INDEX]); +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */ + + mutex_unlock(&cdata->passthrough_lock); + + return err; + } + + mutex_unlock(&cdata->passthrough_lock); + + if (cdata->ext0_available) { + err = st_lsm6ds3_i2c_master_allocate_device(cdata, ST_LSM6DS3_EXT0_INDEX); + if (err < 0) + goto unregister_with_check_i2c_clients; + + } + +#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED + if (cdata->ext1_available) { + err = st_lsm6ds3_i2c_master_allocate_device(cdata, ST_LSM6DS3_EXT1_INDEX); + if (err < 0) + goto deallocate_ext0_device; + + } +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */ + + return 0; + +#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED +deallocate_ext0_device: +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */ + st_lsm6ds3_i2c_master_deallocate_device(cdata, ST_LSM6DS3_EXT0_INDEX); +unregister_with_check_i2c_clients: +#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED + if (cdata->ext1_available) + i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT1_INDEX]); +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */ + if (cdata->ext0_available) + i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT0_INDEX]); + + return err; + +master_probe_passthrough_lock: + mutex_unlock(&cdata->passthrough_lock); +#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED + i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT1_INDEX]); +unregister_ext0_i2c_client: +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */ + i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT0_INDEX]); + + return err; +} +EXPORT_SYMBOL(st_lsm6ds3_i2c_master_probe); + +int st_lsm6ds3_i2c_master_exit(struct lsm6ds3_data *cdata) +{ + if (cdata->ext0_available) { + st_lsm6ds3_i2c_master_deallocate_device(cdata, ST_LSM6DS3_EXT0_INDEX); + i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT0_INDEX]); + } +#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED + if (cdata->ext1_available) { + st_lsm6ds3_i2c_master_deallocate_device(cdata, ST_LSM6DS3_EXT1_INDEX); + i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT1_INDEX]); + } +#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */ + + return 0; +} +EXPORT_SYMBOL(st_lsm6ds3_i2c_master_exit); + +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>"); +MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 i2c master driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_spi.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_spi.c new file mode 100644 index 00000000000..7212ae225f8 --- /dev/null +++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_spi.c @@ -0,0 +1,180 @@ +/* + * STMicroelectronics lsm6ds3 spi driver + * + * Copyright 2014 STMicroelectronics Inc. + * + * Denis Ciocca <denis.ciocca@st.com> + * + * Licensed under the GPL-2. + */ + +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/slab.h> +#include <linux/spi/spi.h> +#include <linux/iio/iio.h> + +#include "st_lsm6ds3.h" + +#define ST_SENSORS_SPI_READ 0x80 + +static int st_lsm6ds3_spi_read(struct lsm6ds3_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err; + + struct spi_transfer xfers[] = { + { + .tx_buf = cdata->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = cdata->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + + if (b_lock) + mutex_lock(&cdata->bank_registers_lock); + + mutex_lock(&cdata->tb.buf_lock); + cdata->tb.tx_buf[0] = reg_addr | ST_SENSORS_SPI_READ; + + err = spi_sync_transfer(to_spi_device(cdata->dev), + xfers, ARRAY_SIZE(xfers)); + if (err) + goto acc_spi_read_error; + + memcpy(data, cdata->tb.rx_buf, len*sizeof(u8)); + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->bank_registers_lock); + + return len; + +acc_spi_read_error: + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->bank_registers_lock); + + return err; +} + +static int st_lsm6ds3_spi_write(struct lsm6ds3_data *cdata, + u8 reg_addr, int len, u8 *data, bool b_lock) +{ + int err; + + struct spi_transfer xfers = { + .tx_buf = cdata->tb.tx_buf, + .bits_per_word = 8, + .len = len + 1, + }; + + if (len >= ST_LSM6DS3_RX_MAX_LENGTH) + return -ENOMEM; + + if (b_lock) + mutex_lock(&cdata->bank_registers_lock); + + mutex_lock(&cdata->tb.buf_lock); + cdata->tb.tx_buf[0] = reg_addr; + + memcpy(&cdata->tb.tx_buf[1], data, len); + + err = spi_sync_transfer(to_spi_device(cdata->dev), &xfers, 1); + mutex_unlock(&cdata->tb.buf_lock); + if (b_lock) + mutex_unlock(&cdata->bank_registers_lock); + + return err; +} + +static const struct st_lsm6ds3_transfer_function st_lsm6ds3_tf_spi = { + .write = st_lsm6ds3_spi_write, + .read = st_lsm6ds3_spi_read, +}; + +static int st_lsm6ds3_spi_probe(struct spi_device *spi) +{ + int err; + struct lsm6ds3_data *cdata; + + cdata = kmalloc(sizeof(*cdata), GFP_KERNEL); + if (!cdata) + return -ENOMEM; + + cdata->dev = &spi->dev; + cdata->name = spi->modalias; + spi_set_drvdata(spi, cdata); + + cdata->tf = &st_lsm6ds3_tf_spi; + + err = st_lsm6ds3_common_probe(cdata, spi->irq); + if (err < 0) + goto free_data; + + return 0; + +free_data: + kfree(cdata); + return err; +} + +static int st_lsm6ds3_spi_remove(struct spi_device *spi) +{ + struct lsm6ds3_data *cdata = spi_get_drvdata(spi); + + st_lsm6ds3_common_remove(cdata, spi->irq); + kfree(cdata); + + return 0; +} + +#ifdef CONFIG_PM +static int st_lsm6ds3_suspend(struct device *dev) +{ + struct lsm6ds3_data *cdata = spi_get_drvdata(to_spi_device(dev)); + + return st_lsm6ds3_common_suspend(cdata); +} + +static int st_lsm6ds3_resume(struct device *dev) +{ + struct lsm6ds3_data *cdata = spi_get_drvdata(to_spi_device(dev)); + + return st_lsm6ds3_common_resume(cdata); +} + +static const struct dev_pm_ops st_lsm6ds3_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_lsm6ds3_suspend, st_lsm6ds3_resume) +}; + +#define ST_LSM6DS3_PM_OPS (&st_lsm6ds3_pm_ops) +#else /* CONFIG_PM */ +#define ST_LSM6DS3_PM_OPS NULL +#endif /* CONFIG_PM */ + +static const struct spi_device_id st_lsm6ds3_id_table[] = { + { LSM6DS3_DEV_NAME }, + { }, +}; +MODULE_DEVICE_TABLE(spi, st_lsm6ds3_id_table); + +static struct spi_driver st_lsm6ds3_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "st-lsm6ds3-spi", + .pm = ST_LSM6DS3_PM_OPS, + }, + .probe = st_lsm6ds3_spi_probe, + .remove = st_lsm6ds3_spi_remove, + .id_table = st_lsm6ds3_id_table, +}; +module_spi_driver(st_lsm6ds3_driver); + +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>"); +MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 spi driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c new file mode 100644 index 00000000000..ccb215c20d0 --- /dev/null +++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c @@ -0,0 +1,250 @@ +/* + * STMicroelectronics lsm6ds3 trigger driver + * + * Copyright 2014 STMicroelectronics Inc. + * + * Denis Ciocca <denis.ciocca@st.com> + * + * Licensed under the GPL-2. + */ + +#define DEBUG //TODO: remove + +#include <linux/kernel.h> +#include <linux/device.h> +#include <linux/module.h> +#include <linux/slab.h> +#include <linux/iio/iio.h> +#include <linux/iio/trigger.h> +#include <linux/interrupt.h> +#include <linux/iio/events.h> +#include <linux/wakelock.h> + +#include "st_lsm6ds3.h" + +#define ST_LSM6DS3_SRC_FUNC_ADDR 0x53 +#define ST_LSM6DS3_FIFO_DATA_AVL_ADDR 0x3b + +#define ST_LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL 0x10 +#define ST_LSM6DS3_SRC_TILT_DATA_AVL 0x20 +#define ST_LSM6DS3_SRC_STEP_COUNTER_DATA_AVL 0x80 +#define ST_LSM6DS3_FIFO_DATA_AVL 0x80 +#define ST_LSM6DS3_FIFO_DATA_OVR 0x40 + +#define ST_LSM6DS3_TAP_SRC_ADDR 0x1c +#define ST_LSM6DS3_TAP_SRC_DETECTED_MASK (1<<6) +#define ST_LSM6DS3_TAP_SRC_SINGLE_TAP_MASK (1<<5) +#define ST_LSM6DS3_TAP_SRC_Z_AXIS_MASK (1<<0) + +#define ST_LSM6DS3_6D_SRC_ADDR 0x1d + + + +static struct workqueue_struct *st_lsm6ds3_wq; + +void st_lsm6ds3_flush_works() +{ + flush_workqueue(st_lsm6ds3_wq); +} + +irqreturn_t st_lsm6ds3_save_timestamp(int irq, void *private) +{ + struct timespec ts; + struct lsm6ds3_data *cdata = private; + + get_monotonic_boottime(&ts); + cdata->timestamp = timespec_to_ns(&ts); + cdata->accel_timestamp = cdata->timestamp; + queue_work(st_lsm6ds3_wq, &cdata->data_work); + + disable_irq_nosync(irq); + + return IRQ_HANDLED; +} + +static void st_lsm6ds3_irq_management(struct work_struct *data_work) +{ + struct lsm6ds3_data *cdata; + u8 d6d_src_reg, tap_src_reg; + u8 src_value = 0x00, src_fifo = 0x00; + u8 d6d_event = 0; + u8 tap_event = 0; + //u8 read_buff[2] = {0}; + + + cdata = container_of((struct work_struct *)data_work, + struct lsm6ds3_data, data_work); + + if(!wake_lock_active(&cdata->wlock)) + wake_lock(&cdata->wlock); + + //cdata->tf->read(cdata, ST_LSM6DS3_TAP_SRC_ADDR, 2, &read_buff[0], true); + //tap_src_reg = read_buff[0]; + //d6d_src_reg = read_buff[1]; + cdata->tf->read(cdata, ST_LSM6DS3_6D_SRC_ADDR, 1, &d6d_src_reg, true); + cdata->tf->read(cdata, ST_LSM6DS3_TAP_SRC_ADDR, 1, &tap_src_reg, true); + cdata->tf->read(cdata, ST_LSM6DS3_SRC_FUNC_ADDR, 1, &src_value, true); + cdata->tf->read(cdata, ST_LSM6DS3_FIFO_DATA_AVL_ADDR, 1, + &src_fifo, true); + + dev_dbg(cdata->dev, "ST irq start :src_value, 6d, tap:%x %x %x",src_value, d6d_src_reg, tap_src_reg); + + if(d6d_src_reg & (1<<6)){ + dev_info(cdata->dev, "D6D IRQ"); + if(cdata->sixd_mask & d6d_src_reg){ + d6d_event = 1; + cdata->last_wakeup_source |= LSM6DS3_WAKEUP_6D; + } + else{ + dev_info(cdata->dev, "ignoring 6d interrupt, wrong axis. mask: 0x%x", cdata->sixd_mask); + } + + } + if(tap_src_reg & ST_LSM6DS3_TAP_SRC_DETECTED_MASK){ + dev_info(cdata->dev, "TAP IRQ"); + if(tap_src_reg & (ST_LSM6DS3_TAP_SRC_SINGLE_TAP_MASK | ST_LSM6DS3_TAP_SRC_Z_AXIS_MASK)){ + tap_event = 1; + cdata->last_wakeup_source |= LSM6DS3_WAKEUP_TAP; + dev_info(cdata->dev, "Valid Tap"); + } + else { + dev_info(cdata->dev, "Ignoring tap"); + } + } + if(cdata->first_irq_from_resume){ + dev_info(cdata->dev, "First IRQ from a RESUME"); + if(!d6d_event && !tap_event){ + dev_info(cdata->dev, "No event from first resume, assuming lost TAP"); + tap_event = 1; + cdata->last_wakeup_source |= LSM6DS3_WAKEUP_TAP; + dev_info(cdata->dev, "Valid Tap from sleep"); + } + } + + if (src_fifo & ST_LSM6DS3_FIFO_DATA_AVL) { + dev_dbg(cdata->dev, "Fifo data available"); + st_lsm6ds3_read_fifo(cdata, true); + } +//significant motion event processing + if(d6d_event || tap_event){ + dev_info(cdata->dev, "Sending sig mot event"); + wake_lock_timeout(&cdata->wlock,msecs_to_jiffies(500)); + if (cdata->sign_motion_event_ready) { + iio_push_event(cdata->indio_dev[ + ST_INDIO_DEV_SIGN_MOTION], + IIO_UNMOD_EVENT_CODE(IIO_SIGN_MOTION, + 0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_EITHER), + cdata->accel_timestamp); + + cdata->sign_motion_event_ready = false; + } + } + + if (src_value & ST_LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL) { + iio_push_event(cdata->indio_dev[ST_INDIO_DEV_STEP_DETECTOR], + IIO_UNMOD_EVENT_CODE(IIO_STEP_DETECTOR, + 0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_EITHER), + cdata->timestamp); + + if (cdata->sign_motion_event_ready) { + dev_info(cdata->dev, "significant motion irq, pushing event (disable this?)"); + iio_push_event(cdata->indio_dev[ + ST_INDIO_DEV_SIGN_MOTION], + IIO_UNMOD_EVENT_CODE(IIO_SIGN_MOTION, + 0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_EITHER), + cdata->timestamp); + + cdata->sign_motion_event_ready = false; + } + } + + if (src_value & ST_LSM6DS3_SRC_STEP_COUNTER_DATA_AVL) { + iio_trigger_poll_chained( + cdata->trig[ST_INDIO_DEV_STEP_COUNTER], 0); + } + + if (src_value & ST_LSM6DS3_SRC_TILT_DATA_AVL) { + iio_push_event(cdata->indio_dev[ST_INDIO_DEV_TILT], + IIO_UNMOD_EVENT_CODE(IIO_TILT, + 0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_EITHER), + cdata->timestamp); + } + + enable_irq(cdata->irq); + if(wake_lock_active(&cdata->wlock)) + wake_unlock(&cdata->wlock); + + cdata->first_irq_from_resume = 0; + return; +} + +int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata, + const struct iio_trigger_ops *trigger_ops) +{ + int err, i, n; + + if (!st_lsm6ds3_wq) + st_lsm6ds3_wq = create_workqueue(cdata->name); + + if (!st_lsm6ds3_wq) + return -EINVAL; + + INIT_WORK(&cdata->data_work, st_lsm6ds3_irq_management); + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) { + cdata->trig[i] = iio_trigger_alloc("%s-trigger", + cdata->indio_dev[i]->name); + if (!cdata->trig[i]) { + dev_err(cdata->dev, + "failed to allocate iio trigger.\n"); + err = -ENOMEM; + goto deallocate_trigger; + } + iio_trigger_set_drvdata(cdata->trig[i], cdata->indio_dev[i]); + cdata->trig[i]->ops = trigger_ops; + cdata->trig[i]->dev.parent = cdata->dev; + } + + err = request_threaded_irq(cdata->irq, st_lsm6ds3_save_timestamp, NULL, + IRQF_TRIGGER_HIGH, cdata->name, cdata); + if (err) + goto deallocate_trigger; + + for (n = 0; n < ST_INDIO_DEV_NUM; n++) { + err = iio_trigger_register(cdata->trig[n]); + if (err < 0) { + dev_err(cdata->dev, + "failed to register iio trigger.\n"); + goto free_irq; + } + cdata->indio_dev[n]->trig = cdata->trig[n]; + } + + return 0; + +free_irq: + free_irq(cdata->irq, cdata); + for (n--; n >= 0; n--) + iio_trigger_unregister(cdata->trig[n]); +deallocate_trigger: + for (i--; i >= 0; i--) + iio_trigger_free(cdata->trig[i]); + + return err; +} +EXPORT_SYMBOL(st_lsm6ds3_allocate_triggers); + +void st_lsm6ds3_deallocate_triggers(struct lsm6ds3_data *cdata) +{ + int i; + + free_irq(cdata->irq, cdata); + + for (i = 0; i < ST_INDIO_DEV_NUM; i++) + iio_trigger_unregister(cdata->trig[i]); +} +EXPORT_SYMBOL(st_lsm6ds3_deallocate_triggers); + +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>"); +MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 trigger driver"); +MODULE_LICENSE("GPL v2"); |