From e0549df64ed888e4f908d81990c55edb6b2c2f7d Mon Sep 17 00:00:00 2001 From: Linus Walleij Date: Tue, 25 Oct 2016 16:15:53 +0200 Subject: [PATCH 01/40] iio: gyro: Add MPU-3050 device tree bindings This adds device tree bindings for the MPU-3050 gyroscope. Since it is the first set of bindings for a gyroscope, the folder for it is also created. Cc: devicetree@vger.kernel.org Cc: Peter Rosin Acked-by: Rob Herring Signed-off-by: Linus Walleij Signed-off-by: Jonathan Cameron --- .../iio/gyroscope/invensense,mpu3050.txt | 46 +++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 Documentation/devicetree/bindings/iio/gyroscope/invensense,mpu3050.txt diff --git a/Documentation/devicetree/bindings/iio/gyroscope/invensense,mpu3050.txt b/Documentation/devicetree/bindings/iio/gyroscope/invensense,mpu3050.txt new file mode 100644 index 000000000000..b0d3b59966bc --- /dev/null +++ b/Documentation/devicetree/bindings/iio/gyroscope/invensense,mpu3050.txt @@ -0,0 +1,46 @@ +Invensense MPU-3050 Gyroscope device tree bindings + +Required properties: + - compatible : should be "invensense,mpu3050" + - reg : the I2C address of the sensor + +Optional properties: + - interrupt-parent : should be the phandle for the interrupt controller + - interrupts : interrupt mapping for the trigger interrupt from the + internal oscillator. The following IRQ modes are supported: + IRQ_TYPE_EDGE_RISING, IRQ_TYPE_EDGE_FALLING, IRQ_TYPE_LEVEL_HIGH and + IRQ_TYPE_LEVEL_LOW. The driver should detect and configure the hardware + for the desired interrupt type. + - vdd-supply : supply regulator for the main power voltage. + - vlogic-supply : supply regulator for the signal voltage. + - mount-matrix : see iio/mount-matrix.txt + +Optional subnodes: + - The MPU-3050 will pass through and forward the I2C signals from the + incoming I2C bus, alternatively drive traffic to a slave device (usually + an accelerometer) on its own initiative. Therefore is supports a subnode + i2c gate node. For details see: i2c/i2c-gate.txt + +Example: + +mpu3050@68 { + compatible = "invensense,mpu3050"; + reg = <0x68>; + interrupt-parent = <&foo>; + interrupts = <12 IRQ_TYPE_EDGE_FALLING>; + vdd-supply = <&bar>; + vlogic-supply = <&baz>; + + /* External I2C interface */ + i2c-gate { + #address-cells = <1>; + #size-cells = <0>; + + fnord@18 { + compatible = "fnord"; + reg = <0x18>; + interrupt-parent = <&foo>; + interrupts = <13 IRQ_TYPE_EDGE_FALLING>; + }; + }; +}; From 3904b28efb2c780c23dcddfb87e07fe0230661e5 Mon Sep 17 00:00:00 2001 From: Linus Walleij Date: Tue, 25 Oct 2016 16:15:54 +0200 Subject: [PATCH 02/40] iio: gyro: Add driver for the MPU-3050 gyroscope This adds a new driver for the Invensense MPU-3050 gyroscope. This driver is based on information from the rough input driver in drivers/input/misc/mpu3050.c and the scratch misc driver posted by Nathan Royer in 2011. Some years have passed but this is finally a fully-fledged driver for this gyroscope. It was developed and tested on the Qualcomm APQ8060 Dragonboard. The driver supports both raw and buffered input. It also supports the internal trigger mechanism by registering a trigger that can fire in response to the internal sample engine of the component. In addition to reading out the gyroscope sensor values, the driver also supports reading the temperature from the sensor. The driver currently only supports I2C but the MPU-3050 can also be used from SPI, so the I2C portions are split in their own file and we just use regmap to access all registers, so it will be trivial to plug in SPI support if/when someone has a system requiring this. To conserve power, the driver utilizes the runtime PM framework and will put the sensor in off mode and disable the regulators when unused, after a timeout of 10 seconds. The fullscale can be set for the sensor to 250, 500, 1000 or 2000 deg/s. This corresponds to scale values of rougly 0.000122, 0.000275, 0.000512 or 0.001068. By writing such values (or close to these) into "in_anglevel_scale", the corresponding fullscale can be chosen. It will default to 2000 deg/s (~35 rad/s). The gyro component can have DC offsets on all axes. These can be compensated using the standard sysfs ABI property "in_anglevel_[xyz]_calibbias". This is in positive/negative values of the raw values, so a suitable calibration bias can be determined by userspace by reading the "in_anglevel_[xyz]_raw" for a few iterations while holding the sensor still, create an average integer, and writing the negative inverse of that into "in_anglevel_[xyz]_calibbias". After this the hardware will automatically subtract the bias, also when using buffered readings. Since the MPU-3050 has an outgoing I2C port it needs to act as an I2C mux. This means that the device is switching I2C traffic to devices beyond it. On my system this is the only way to reach the accelerometer. The "sensor fusion" ability of the MPU-3050 to directly talk to the device on the outgoing I2C port is currently not used by the driver, but it has code to allow I2C traffic to pass through so that the Linux kernel can reach the device on the other side with a kernel driver. Example usage with the native trigger: $ generic_buffer -a -c10 -n mpu3050 iio device number being used is 0 iio trigger number being used is 0 No channels are enabled, enabling all channels Enabling: in_anglvel_z_en Enabling: in_timestamp_en Enabling: in_anglvel_y_en Enabling: in_temp_en Enabling: in_anglvel_x_en /sys/bus/iio/devices/iio:device0 mpu3050-dev0 29607.142578 -0.117493 0.074768 0.012817 180788797150 29639.285156 -0.117493 0.076904 0.013885 180888982335 29696.427734 -0.116425 0.076904 0.012817 180989178039 29742.857422 -0.117493 0.076904 0.012817 181089377742 29764.285156 -0.116425 0.077972 0.012817 181189574187 29860.714844 -0.115356 0.076904 0.012817 181289772705 29864.285156 -0.117493 0.076904 0.012817 181389971520 29910.714844 -0.115356 0.076904 0.013885 181490170483 29917.857422 -0.116425 0.076904 0.011749 181590369742 29975.000000 -0.116425 0.076904 0.012817 181690567075 Disabling: in_anglvel_z_en Disabling: in_timestamp_en Disabling: in_anglvel_y_en Disabling: in_temp_en Disabling: in_anglvel_x_en The first column is the temperature in millidegrees, then the x,y,z axes in succession followed by the timestamp. Also tested successfully using the HRTimer trigger. Cc: Nick Vaccaro Cc: Ge Gao Cc: Anna Si Cc: Dmitry Torokhov Cc: Crestez Dan Leonard Cc: Daniel Baluta Cc: Gregor Boirie Cc: Peter Rosin Cc: Peter Meerwald-Stadler Signed-off-by: Linus Walleij Signed-off-by: Jonathan Cameron --- MAINTAINERS | 7 + drivers/iio/gyro/Kconfig | 17 + drivers/iio/gyro/Makefile | 5 + drivers/iio/gyro/mpu3050-core.c | 1307 +++++++++++++++++++++++++++++++ drivers/iio/gyro/mpu3050-i2c.c | 124 +++ drivers/iio/gyro/mpu3050.h | 96 +++ 6 files changed, 1556 insertions(+) create mode 100644 drivers/iio/gyro/mpu3050-core.c create mode 100644 drivers/iio/gyro/mpu3050-i2c.c create mode 100644 drivers/iio/gyro/mpu3050.h diff --git a/MAINTAINERS b/MAINTAINERS index 7084d8e8592f..5e218aaa4c7a 100644 --- a/MAINTAINERS +++ b/MAINTAINERS @@ -6496,6 +6496,13 @@ S: Maintained F: arch/x86/include/asm/pmc_core.h F: drivers/platform/x86/intel_pmc_core* +INVENSENSE MPU-3050 GYROSCOPE DRIVER +M: Linus Walleij +L: linux-iio@vger.kernel.org +S: Maintained +F: drivers/iio/gyro/mpu3050* +F: Documentation/devicetree/bindings/iio/gyroscope/inv,mpu3050.txt + IOC3 ETHERNET DRIVER M: Ralf Baechle L: linux-mips@linux-mips.org diff --git a/drivers/iio/gyro/Kconfig b/drivers/iio/gyro/Kconfig index 205a84420ae9..107b5efd4178 100644 --- a/drivers/iio/gyro/Kconfig +++ b/drivers/iio/gyro/Kconfig @@ -84,6 +84,23 @@ config HID_SENSOR_GYRO_3D Say yes here to build support for the HID SENSOR Gyroscope 3D. +config MPU3050 + tristate + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + select REGMAP + +config MPU3050_I2C + tristate "Invensense MPU3050 devices on I2C" + depends on !(INPUT_MPU3050=y || INPUT_MPU3050=m) + select MPU3050 + select REGMAP_I2C + select I2C_MUX + help + This driver supports the Invensense MPU3050 gyroscope over I2C. + This driver can be built as a module. The module will be called + inv-mpu3050-i2c. + config IIO_ST_GYRO_3AXIS tristate "STMicroelectronics gyroscopes 3-Axis Driver" depends on (I2C || SPI_MASTER) && SYSFS diff --git a/drivers/iio/gyro/Makefile b/drivers/iio/gyro/Makefile index f866a4be0667..f0e149a606b0 100644 --- a/drivers/iio/gyro/Makefile +++ b/drivers/iio/gyro/Makefile @@ -14,6 +14,11 @@ obj-$(CONFIG_BMG160_SPI) += bmg160_spi.o obj-$(CONFIG_HID_SENSOR_GYRO_3D) += hid-sensor-gyro-3d.o +# Currently this is rolled into one module, split it if +# we ever create a separate SPI interface for MPU-3050 +obj-$(CONFIG_MPU3050) += mpu3050.o +mpu3050-objs := mpu3050-core.o mpu3050-i2c.o + itg3200-y := itg3200_core.o itg3200-$(CONFIG_IIO_BUFFER) += itg3200_buffer.o obj-$(CONFIG_ITG3200) += itg3200.o diff --git a/drivers/iio/gyro/mpu3050-core.c b/drivers/iio/gyro/mpu3050-core.c new file mode 100644 index 000000000000..ed681c70a7b4 --- /dev/null +++ b/drivers/iio/gyro/mpu3050-core.c @@ -0,0 +1,1307 @@ +/* + * MPU3050 gyroscope driver + * + * Copyright (C) 2016 Linaro Ltd. + * Author: Linus Walleij + * + * Based on the input subsystem driver, Copyright (C) 2011 Wistron Co.Ltd + * Joseph Lai and trimmed down by + * Alan Cox in turn based on bma023.c. + * Device behaviour based on a misc driver posted by Nathan Royer in 2011. + * + * TODO: add support for setting up the low pass 3dB frequency. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mpu3050.h" + +#define MPU3050_CHIP_ID 0x69 + +/* + * Register map: anything suffixed *_H is a big-endian high byte and always + * followed by the corresponding low byte (*_L) even though these are not + * explicitly included in the register definitions. + */ +#define MPU3050_CHIP_ID_REG 0x00 +#define MPU3050_PRODUCT_ID_REG 0x01 +#define MPU3050_XG_OFFS_TC 0x05 +#define MPU3050_YG_OFFS_TC 0x08 +#define MPU3050_ZG_OFFS_TC 0x0B +#define MPU3050_X_OFFS_USR_H 0x0C +#define MPU3050_Y_OFFS_USR_H 0x0E +#define MPU3050_Z_OFFS_USR_H 0x10 +#define MPU3050_FIFO_EN 0x12 +#define MPU3050_AUX_VDDIO 0x13 +#define MPU3050_SLV_ADDR 0x14 +#define MPU3050_SMPLRT_DIV 0x15 +#define MPU3050_DLPF_FS_SYNC 0x16 +#define MPU3050_INT_CFG 0x17 +#define MPU3050_AUX_ADDR 0x18 +#define MPU3050_INT_STATUS 0x1A +#define MPU3050_TEMP_H 0x1B +#define MPU3050_XOUT_H 0x1D +#define MPU3050_YOUT_H 0x1F +#define MPU3050_ZOUT_H 0x21 +#define MPU3050_DMP_CFG1 0x35 +#define MPU3050_DMP_CFG2 0x36 +#define MPU3050_BANK_SEL 0x37 +#define MPU3050_MEM_START_ADDR 0x38 +#define MPU3050_MEM_R_W 0x39 +#define MPU3050_FIFO_COUNT_H 0x3A +#define MPU3050_FIFO_R 0x3C +#define MPU3050_USR_CTRL 0x3D +#define MPU3050_PWR_MGM 0x3E + +/* MPU memory bank read options */ +#define MPU3050_MEM_PRFTCH BIT(5) +#define MPU3050_MEM_USER_BANK BIT(4) +/* Bits 8-11 select memory bank */ +#define MPU3050_MEM_RAM_BANK_0 0 +#define MPU3050_MEM_RAM_BANK_1 1 +#define MPU3050_MEM_RAM_BANK_2 2 +#define MPU3050_MEM_RAM_BANK_3 3 +#define MPU3050_MEM_OTP_BANK_0 4 + +#define MPU3050_AXIS_REGS(axis) (MPU3050_XOUT_H + (axis * 2)) + +/* Register bits */ + +/* FIFO Enable */ +#define MPU3050_FIFO_EN_FOOTER BIT(0) +#define MPU3050_FIFO_EN_AUX_ZOUT BIT(1) +#define MPU3050_FIFO_EN_AUX_YOUT BIT(2) +#define MPU3050_FIFO_EN_AUX_XOUT BIT(3) +#define MPU3050_FIFO_EN_GYRO_ZOUT BIT(4) +#define MPU3050_FIFO_EN_GYRO_YOUT BIT(5) +#define MPU3050_FIFO_EN_GYRO_XOUT BIT(6) +#define MPU3050_FIFO_EN_TEMP_OUT BIT(7) + +/* + * Digital Low Pass filter (DLPF) + * Full Scale (FS) + * and Synchronization + */ +#define MPU3050_EXT_SYNC_NONE 0x00 +#define MPU3050_EXT_SYNC_TEMP 0x20 +#define MPU3050_EXT_SYNC_GYROX 0x40 +#define MPU3050_EXT_SYNC_GYROY 0x60 +#define MPU3050_EXT_SYNC_GYROZ 0x80 +#define MPU3050_EXT_SYNC_ACCELX 0xA0 +#define MPU3050_EXT_SYNC_ACCELY 0xC0 +#define MPU3050_EXT_SYNC_ACCELZ 0xE0 +#define MPU3050_EXT_SYNC_MASK 0xE0 +#define MPU3050_EXT_SYNC_SHIFT 5 + +#define MPU3050_FS_250DPS 0x00 +#define MPU3050_FS_500DPS 0x08 +#define MPU3050_FS_1000DPS 0x10 +#define MPU3050_FS_2000DPS 0x18 +#define MPU3050_FS_MASK 0x18 +#define MPU3050_FS_SHIFT 3 + +#define MPU3050_DLPF_CFG_256HZ_NOLPF2 0x00 +#define MPU3050_DLPF_CFG_188HZ 0x01 +#define MPU3050_DLPF_CFG_98HZ 0x02 +#define MPU3050_DLPF_CFG_42HZ 0x03 +#define MPU3050_DLPF_CFG_20HZ 0x04 +#define MPU3050_DLPF_CFG_10HZ 0x05 +#define MPU3050_DLPF_CFG_5HZ 0x06 +#define MPU3050_DLPF_CFG_2100HZ_NOLPF 0x07 +#define MPU3050_DLPF_CFG_MASK 0x07 +#define MPU3050_DLPF_CFG_SHIFT 0 + +/* Interrupt config */ +#define MPU3050_INT_RAW_RDY_EN BIT(0) +#define MPU3050_INT_DMP_DONE_EN BIT(1) +#define MPU3050_INT_MPU_RDY_EN BIT(2) +#define MPU3050_INT_ANYRD_2CLEAR BIT(4) +#define MPU3050_INT_LATCH_EN BIT(5) +#define MPU3050_INT_OPEN BIT(6) +#define MPU3050_INT_ACTL BIT(7) +/* Interrupt status */ +#define MPU3050_INT_STATUS_RAW_RDY BIT(0) +#define MPU3050_INT_STATUS_DMP_DONE BIT(1) +#define MPU3050_INT_STATUS_MPU_RDY BIT(2) +#define MPU3050_INT_STATUS_FIFO_OVFLW BIT(7) +/* USR_CTRL */ +#define MPU3050_USR_CTRL_FIFO_EN BIT(6) +#define MPU3050_USR_CTRL_AUX_IF_EN BIT(5) +#define MPU3050_USR_CTRL_AUX_IF_RST BIT(3) +#define MPU3050_USR_CTRL_FIFO_RST BIT(1) +#define MPU3050_USR_CTRL_GYRO_RST BIT(0) +/* PWR_MGM */ +#define MPU3050_PWR_MGM_PLL_X 0x01 +#define MPU3050_PWR_MGM_PLL_Y 0x02 +#define MPU3050_PWR_MGM_PLL_Z 0x03 +#define MPU3050_PWR_MGM_CLKSEL_MASK 0x07 +#define MPU3050_PWR_MGM_STBY_ZG BIT(3) +#define MPU3050_PWR_MGM_STBY_YG BIT(4) +#define MPU3050_PWR_MGM_STBY_XG BIT(5) +#define MPU3050_PWR_MGM_SLEEP BIT(6) +#define MPU3050_PWR_MGM_RESET BIT(7) +#define MPU3050_PWR_MGM_MASK 0xff + +/* + * Fullscale precision is (for finest precision) +/- 250 deg/s, so the full + * scale is actually 500 deg/s. All 16 bits are then used to cover this scale, + * in two's complement. + */ +static unsigned int mpu3050_fs_precision[] = { + IIO_DEGREE_TO_RAD(250), + IIO_DEGREE_TO_RAD(500), + IIO_DEGREE_TO_RAD(1000), + IIO_DEGREE_TO_RAD(2000) +}; + +/* + * Regulator names + */ +static const char mpu3050_reg_vdd[] = "vdd"; +static const char mpu3050_reg_vlogic[] = "vlogic"; + +static unsigned int mpu3050_get_freq(struct mpu3050 *mpu3050) +{ + unsigned int freq; + + if (mpu3050->lpf == MPU3050_DLPF_CFG_256HZ_NOLPF2) + freq = 8000; + else + freq = 1000; + freq /= (mpu3050->divisor + 1); + + return freq; +} + +static int mpu3050_start_sampling(struct mpu3050 *mpu3050) +{ + __be16 raw_val[3]; + int ret; + int i; + + /* Reset */ + ret = regmap_update_bits(mpu3050->map, MPU3050_PWR_MGM, + MPU3050_PWR_MGM_RESET, MPU3050_PWR_MGM_RESET); + if (ret) + return ret; + + /* Turn on the Z-axis PLL */ + ret = regmap_update_bits(mpu3050->map, MPU3050_PWR_MGM, + MPU3050_PWR_MGM_CLKSEL_MASK, + MPU3050_PWR_MGM_PLL_Z); + if (ret) + return ret; + + /* Write calibration offset registers */ + for (i = 0; i < 3; i++) + raw_val[i] = cpu_to_be16(mpu3050->calibration[i]); + + ret = regmap_bulk_write(mpu3050->map, MPU3050_X_OFFS_USR_H, raw_val, + sizeof(raw_val)); + if (ret) + return ret; + + /* Set low pass filter (sample rate), sync and full scale */ + ret = regmap_write(mpu3050->map, MPU3050_DLPF_FS_SYNC, + MPU3050_EXT_SYNC_NONE << MPU3050_EXT_SYNC_SHIFT | + mpu3050->fullscale << MPU3050_FS_SHIFT | + mpu3050->lpf << MPU3050_DLPF_CFG_SHIFT); + if (ret) + return ret; + + /* Set up sampling frequency */ + ret = regmap_write(mpu3050->map, MPU3050_SMPLRT_DIV, mpu3050->divisor); + if (ret) + return ret; + + /* + * Max 50 ms start-up time after setting DLPF_FS_SYNC + * according to the data sheet, then wait for the next sample + * at this frequency T = 1000/f ms. + */ + msleep(50 + 1000 / mpu3050_get_freq(mpu3050)); + + return 0; +} + +static int mpu3050_set_8khz_samplerate(struct mpu3050 *mpu3050) +{ + int ret; + u8 divisor; + enum mpu3050_lpf lpf; + + lpf = mpu3050->lpf; + divisor = mpu3050->divisor; + + mpu3050->lpf = LPF_256_HZ_NOLPF; /* 8 kHz base frequency */ + mpu3050->divisor = 0; /* Divide by 1 */ + ret = mpu3050_start_sampling(mpu3050); + + mpu3050->lpf = lpf; + mpu3050->divisor = divisor; + + return ret; +} + +static int mpu3050_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, + long mask) +{ + struct mpu3050 *mpu3050 = iio_priv(indio_dev); + int ret; + __be16 raw_val; + + switch (mask) { + case IIO_CHAN_INFO_OFFSET: + switch (chan->type) { + case IIO_TEMP: + /* The temperature scaling is (x+23000)/280 Celsius */ + *val = 23000; + return IIO_VAL_INT; + default: + return -EINVAL; + } + case IIO_CHAN_INFO_CALIBBIAS: + switch (chan->type) { + case IIO_ANGL_VEL: + *val = mpu3050->calibration[chan->scan_index-1]; + return IIO_VAL_INT; + default: + return -EINVAL; + } + case IIO_CHAN_INFO_SAMP_FREQ: + *val = mpu3050_get_freq(mpu3050); + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + switch (chan->type) { + case IIO_TEMP: + /* Millidegrees, see about temperature scaling above */ + *val = 1000; + *val2 = 280; + return IIO_VAL_FRACTIONAL; + case IIO_ANGL_VEL: + /* + * Convert to the corresponding full scale in + * radians. All 16 bits are used with sign to + * span the available scale: to account for the one + * missing value if we multiply by 1/S16_MAX, instead + * multiply with 2/U16_MAX. + */ + *val = mpu3050_fs_precision[mpu3050->fullscale] * 2; + *val2 = U16_MAX; + return IIO_VAL_FRACTIONAL; + default: + return -EINVAL; + } + case IIO_CHAN_INFO_RAW: + /* Resume device */ + pm_runtime_get_sync(mpu3050->dev); + mutex_lock(&mpu3050->lock); + + ret = mpu3050_set_8khz_samplerate(mpu3050); + if (ret) + goto out_read_raw_unlock; + + switch (chan->type) { + case IIO_TEMP: + ret = regmap_bulk_read(mpu3050->map, MPU3050_TEMP_H, + &raw_val, sizeof(raw_val)); + if (ret) { + dev_err(mpu3050->dev, + "error reading temperature\n"); + goto out_read_raw_unlock; + } + + *val = be16_to_cpu(raw_val); + ret = IIO_VAL_INT; + + goto out_read_raw_unlock; + case IIO_ANGL_VEL: + ret = regmap_bulk_read(mpu3050->map, + MPU3050_AXIS_REGS(chan->scan_index-1), + &raw_val, + sizeof(raw_val)); + if (ret) { + dev_err(mpu3050->dev, + "error reading axis data\n"); + goto out_read_raw_unlock; + } + + *val = be16_to_cpu(raw_val); + ret = IIO_VAL_INT; + + goto out_read_raw_unlock; + default: + ret = -EINVAL; + goto out_read_raw_unlock; + } + default: + break; + } + + return -EINVAL; + +out_read_raw_unlock: + mutex_unlock(&mpu3050->lock); + pm_runtime_mark_last_busy(mpu3050->dev); + pm_runtime_put_autosuspend(mpu3050->dev); + + return ret; +} + +static int mpu3050_write_raw(struct iio_dev *indio_dev, + const struct iio_chan_spec *chan, + int val, int val2, long mask) +{ + struct mpu3050 *mpu3050 = iio_priv(indio_dev); + /* + * Couldn't figure out a way to precalculate these at compile time. + */ + unsigned int fs250 = + DIV_ROUND_CLOSEST(mpu3050_fs_precision[0] * 1000000 * 2, + U16_MAX); + unsigned int fs500 = + DIV_ROUND_CLOSEST(mpu3050_fs_precision[1] * 1000000 * 2, + U16_MAX); + unsigned int fs1000 = + DIV_ROUND_CLOSEST(mpu3050_fs_precision[2] * 1000000 * 2, + U16_MAX); + unsigned int fs2000 = + DIV_ROUND_CLOSEST(mpu3050_fs_precision[3] * 1000000 * 2, + U16_MAX); + + switch (mask) { + case IIO_CHAN_INFO_CALIBBIAS: + if (chan->type != IIO_ANGL_VEL) + return -EINVAL; + mpu3050->calibration[chan->scan_index-1] = val; + return 0; + case IIO_CHAN_INFO_SAMP_FREQ: + /* + * The max samplerate is 8000 Hz, the minimum + * 1000 / 256 ~= 4 Hz + */ + if (val < 4 || val > 8000) + return -EINVAL; + + /* + * Above 1000 Hz we must turn off the digital low pass filter + * so we get a base frequency of 8kHz to the divider + */ + if (val > 1000) { + mpu3050->lpf = LPF_256_HZ_NOLPF; + mpu3050->divisor = DIV_ROUND_CLOSEST(8000, val) - 1; + return 0; + } + + mpu3050->lpf = LPF_188_HZ; + mpu3050->divisor = DIV_ROUND_CLOSEST(1000, val) - 1; + return 0; + case IIO_CHAN_INFO_SCALE: + if (chan->type != IIO_ANGL_VEL) + return -EINVAL; + /* + * We support +/-250, +/-500, +/-1000 and +/2000 deg/s + * which means we need to round to the closest radians + * which will be roughly +/-4.3, +/-8.7, +/-17.5, +/-35 + * rad/s. The scale is then for the 16 bits used to cover + * it 2/(2^16) of that. + */ + + /* Just too large, set the max range */ + if (val != 0) { + mpu3050->fullscale = FS_2000_DPS; + return 0; + } + + /* + * Now we're dealing with fractions below zero in millirad/s + * do some integer interpolation and match with the closest + * fullscale in the table. + */ + if (val2 <= fs250 || + val2 < ((fs500 + fs250) / 2)) + mpu3050->fullscale = FS_250_DPS; + else if (val2 <= fs500 || + val2 < ((fs1000 + fs500) / 2)) + mpu3050->fullscale = FS_500_DPS; + else if (val2 <= fs1000 || + val2 < ((fs2000 + fs1000) / 2)) + mpu3050->fullscale = FS_1000_DPS; + else + /* Catch-all */ + mpu3050->fullscale = FS_2000_DPS; + return 0; + default: + break; + } + + return -EINVAL; +} + +static irqreturn_t mpu3050_trigger_handler(int irq, void *p) +{ + const struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + struct mpu3050 *mpu3050 = iio_priv(indio_dev); + int ret; + /* + * Temperature 1*16 bits + * Three axes 3*16 bits + * Timestamp 64 bits (4*16 bits) + * Sum total 8*16 bits + */ + __be16 hw_values[8]; + s64 timestamp; + unsigned int datums_from_fifo = 0; + + /* + * If we're using the hardware trigger, get the precise timestamp from + * the top half of the threaded IRQ handler. Otherwise get the + * timestamp here so it will be close in time to the actual values + * read from the registers. + */ + if (iio_trigger_using_own(indio_dev)) + timestamp = mpu3050->hw_timestamp; + else + timestamp = iio_get_time_ns(indio_dev); + + mutex_lock(&mpu3050->lock); + + /* Using the hardware IRQ trigger? Check the buffer then. */ + if (mpu3050->hw_irq_trigger) { + __be16 raw_fifocnt; + u16 fifocnt; + /* X, Y, Z + temperature */ + unsigned int bytes_per_datum = 8; + bool fifo_overflow = false; + + ret = regmap_bulk_read(mpu3050->map, + MPU3050_FIFO_COUNT_H, + &raw_fifocnt, + sizeof(raw_fifocnt)); + if (ret) + goto out_trigger_unlock; + fifocnt = be16_to_cpu(raw_fifocnt); + + if (fifocnt == 512) { + dev_info(mpu3050->dev, + "FIFO overflow! Emptying and resetting FIFO\n"); + fifo_overflow = true; + /* Reset and enable the FIFO */ + ret = regmap_update_bits(mpu3050->map, + MPU3050_USR_CTRL, + MPU3050_USR_CTRL_FIFO_EN | + MPU3050_USR_CTRL_FIFO_RST, + MPU3050_USR_CTRL_FIFO_EN | + MPU3050_USR_CTRL_FIFO_RST); + if (ret) { + dev_info(mpu3050->dev, "error resetting FIFO\n"); + goto out_trigger_unlock; + } + mpu3050->pending_fifo_footer = false; + } + + if (fifocnt) + dev_dbg(mpu3050->dev, + "%d bytes in the FIFO\n", + fifocnt); + + while (!fifo_overflow && fifocnt > bytes_per_datum) { + unsigned int toread; + unsigned int offset; + __be16 fifo_values[5]; + + /* + * If there is a FIFO footer in the pipe, first clear + * that out. This follows the complex algorithm in the + * datasheet that states that you may never leave the + * FIFO empty after the first reading: you have to + * always leave two footer bytes in it. The footer is + * in practice just two zero bytes. + */ + if (mpu3050->pending_fifo_footer) { + toread = bytes_per_datum + 2; + offset = 0; + } else { + toread = bytes_per_datum; + offset = 1; + /* Put in some dummy value */ + fifo_values[0] = 0xAAAA; + } + + ret = regmap_bulk_read(mpu3050->map, + MPU3050_FIFO_R, + &fifo_values[offset], + toread); + + dev_dbg(mpu3050->dev, + "%04x %04x %04x %04x %04x\n", + fifo_values[0], + fifo_values[1], + fifo_values[2], + fifo_values[3], + fifo_values[4]); + + /* Index past the footer (fifo_values[0]) and push */ + iio_push_to_buffers_with_timestamp(indio_dev, + &fifo_values[1], + timestamp); + + fifocnt -= toread; + datums_from_fifo++; + mpu3050->pending_fifo_footer = true; + + /* + * If we're emptying the FIFO, just make sure to + * check if something new appeared. + */ + if (fifocnt < bytes_per_datum) { + ret = regmap_bulk_read(mpu3050->map, + MPU3050_FIFO_COUNT_H, + &raw_fifocnt, + sizeof(raw_fifocnt)); + if (ret) + goto out_trigger_unlock; + fifocnt = be16_to_cpu(raw_fifocnt); + } + + if (fifocnt < bytes_per_datum) + dev_dbg(mpu3050->dev, + "%d bytes left in the FIFO\n", + fifocnt); + + /* + * At this point, the timestamp that triggered the + * hardware interrupt is no longer valid for what + * we are reading (the interrupt likely fired for + * the value on the top of the FIFO), so set the + * timestamp to zero and let userspace deal with it. + */ + timestamp = 0; + } + } + + /* + * If we picked some datums from the FIFO that's enough, else + * fall through and just read from the current value registers. + * This happens in two cases: + * + * - We are using some other trigger (external, like an HRTimer) + * than the sensor's own sample generator. In this case the + * sensor is just set to the max sampling frequency and we give + * the trigger a copy of the latest value every time we get here. + * + * - The hardware trigger is active but unused and we actually use + * another trigger which calls here with a frequency higher + * than what the device provides data. We will then just read + * duplicate values directly from the hardware registers. + */ + if (datums_from_fifo) { + dev_dbg(mpu3050->dev, + "read %d datums from the FIFO\n", + datums_from_fifo); + goto out_trigger_unlock; + } + + ret = regmap_bulk_read(mpu3050->map, MPU3050_TEMP_H, &hw_values, + sizeof(hw_values)); + if (ret) { + dev_err(mpu3050->dev, + "error reading axis data\n"); + goto out_trigger_unlock; + } + + iio_push_to_buffers_with_timestamp(indio_dev, hw_values, timestamp); + +out_trigger_unlock: + mutex_unlock(&mpu3050->lock); + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; +} + +static int mpu3050_buffer_preenable(struct iio_dev *indio_dev) +{ + struct mpu3050 *mpu3050 = iio_priv(indio_dev); + + pm_runtime_get_sync(mpu3050->dev); + + /* Unless we have OUR trigger active, run at full speed */ + if (!mpu3050->hw_irq_trigger) + return mpu3050_set_8khz_samplerate(mpu3050); + + return 0; +} + +static int mpu3050_buffer_postdisable(struct iio_dev *indio_dev) +{ + struct mpu3050 *mpu3050 = iio_priv(indio_dev); + + pm_runtime_mark_last_busy(mpu3050->dev); + pm_runtime_put_autosuspend(mpu3050->dev); + + return 0; +} + +static const struct iio_buffer_setup_ops mpu3050_buffer_setup_ops = { + .preenable = mpu3050_buffer_preenable, + .postenable = iio_triggered_buffer_postenable, + .predisable = iio_triggered_buffer_predisable, + .postdisable = mpu3050_buffer_postdisable, +}; + +static const struct iio_mount_matrix * +mpu3050_get_mount_matrix(const struct iio_dev *indio_dev, + const struct iio_chan_spec *chan) +{ + struct mpu3050 *mpu3050 = iio_priv(indio_dev); + + return &mpu3050->orientation; +} + +static const struct iio_chan_spec_ext_info mpu3050_ext_info[] = { + IIO_MOUNT_MATRIX(IIO_SHARED_BY_TYPE, mpu3050_get_mount_matrix), + { }, +}; + +#define MPU3050_AXIS_CHANNEL(axis, index) \ + { \ + .type = IIO_ANGL_VEL, \ + .modified = 1, \ + .channel2 = IIO_MOD_##axis, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_CALIBBIAS), \ + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),\ + .ext_info = mpu3050_ext_info, \ + .scan_index = index, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_BE, \ + }, \ + } + +static const struct iio_chan_spec mpu3050_channels[] = { + { + .type = IIO_TEMP, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | + BIT(IIO_CHAN_INFO_SCALE) | + BIT(IIO_CHAN_INFO_OFFSET), + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), + .scan_index = 0, + .scan_type = { + .sign = 's', + .realbits = 16, + .storagebits = 16, + .endianness = IIO_BE, + }, + }, + MPU3050_AXIS_CHANNEL(X, 1), + MPU3050_AXIS_CHANNEL(Y, 2), + MPU3050_AXIS_CHANNEL(Z, 3), + IIO_CHAN_SOFT_TIMESTAMP(4), +}; + +/* Four channels apart from timestamp, scan mask = 0x0f */ +static const unsigned long mpu3050_scan_masks[] = { 0xf, 0 }; + +/* + * These are just the hardcoded factors resulting from the more elaborate + * calculations done with fractions in the scale raw get/set functions. + */ +static IIO_CONST_ATTR(anglevel_scale_available, + "0.000122070 " + "0.000274658 " + "0.000518798 " + "0.001068115"); + +static struct attribute *mpu3050_attributes[] = { + &iio_const_attr_anglevel_scale_available.dev_attr.attr, + NULL, +}; + +static const struct attribute_group mpu3050_attribute_group = { + .attrs = mpu3050_attributes, +}; + +static const struct iio_info mpu3050_info = { + .driver_module = THIS_MODULE, + .read_raw = mpu3050_read_raw, + .write_raw = mpu3050_write_raw, + .attrs = &mpu3050_attribute_group, + .driver_module = THIS_MODULE, +}; + +/** + * mpu3050_read_mem() - read MPU-3050 internal memory + * @mpu3050: device to read from + * @bank: target bank + * @addr: target address + * @len: number of bytes + * @buf: the buffer to store the read bytes in + */ +static int mpu3050_read_mem(struct mpu3050 *mpu3050, + u8 bank, + u8 addr, + u8 len, + u8 *buf) +{ + int ret; + + ret = regmap_write(mpu3050->map, + MPU3050_BANK_SEL, + bank); + if (ret) + return ret; + + ret = regmap_write(mpu3050->map, + MPU3050_MEM_START_ADDR, + addr); + if (ret) + return ret; + + return regmap_bulk_read(mpu3050->map, + MPU3050_MEM_R_W, + buf, + len); +} + +static int mpu3050_hw_init(struct mpu3050 *mpu3050) +{ + int ret; + u8 otp[8]; + + /* Reset */ + ret = regmap_update_bits(mpu3050->map, + MPU3050_PWR_MGM, + MPU3050_PWR_MGM_RESET, + MPU3050_PWR_MGM_RESET); + if (ret) + return ret; + + /* Turn on the PLL */ + ret = regmap_update_bits(mpu3050->map, + MPU3050_PWR_MGM, + MPU3050_PWR_MGM_CLKSEL_MASK, + MPU3050_PWR_MGM_PLL_Z); + if (ret) + return ret; + + /* Disable IRQs */ + ret = regmap_write(mpu3050->map, + MPU3050_INT_CFG, + 0); + if (ret) + return ret; + + /* Read out the 8 bytes of OTP (one-time-programmable) memory */ + ret = mpu3050_read_mem(mpu3050, + (MPU3050_MEM_PRFTCH | + MPU3050_MEM_USER_BANK | + MPU3050_MEM_OTP_BANK_0), + 0, + sizeof(otp), + otp); + if (ret) + return ret; + + /* This is device-unique data so it goes into the entropy pool */ + add_device_randomness(otp, sizeof(otp)); + + dev_info(mpu3050->dev, + "die ID: %04X, wafer ID: %02X, A lot ID: %04X, " + "W lot ID: %03X, WP ID: %01X, rev ID: %02X\n", + /* Die ID, bits 0-12 */ + (otp[1] << 8 | otp[0]) & 0x1fff, + /* Wafer ID, bits 13-17 */ + ((otp[2] << 8 | otp[1]) & 0x03e0) >> 5, + /* A lot ID, bits 18-33 */ + ((otp[4] << 16 | otp[3] << 8 | otp[2]) & 0x3fffc) >> 2, + /* W lot ID, bits 34-45 */ + ((otp[5] << 8 | otp[4]) & 0x3ffc) >> 2, + /* WP ID, bits 47-49 */ + ((otp[6] << 8 | otp[5]) & 0x0380) >> 7, + /* rev ID, bits 50-55 */ + otp[6] >> 2); + + return 0; +} + +static int mpu3050_power_up(struct mpu3050 *mpu3050) +{ + int ret; + + ret = regulator_bulk_enable(ARRAY_SIZE(mpu3050->regs), mpu3050->regs); + if (ret) { + dev_err(mpu3050->dev, "cannot enable regulators\n"); + return ret; + } + /* + * 20-100 ms start-up time for register read/write according to + * the datasheet, be on the safe side and wait 200 ms. + */ + msleep(200); + + /* Take device out of sleep mode */ + ret = regmap_update_bits(mpu3050->map, MPU3050_PWR_MGM, + MPU3050_PWR_MGM_SLEEP, 0); + if (ret) { + dev_err(mpu3050->dev, "error setting power mode\n"); + return ret; + } + msleep(10); + + return 0; +} + +static int mpu3050_power_down(struct mpu3050 *mpu3050) +{ + int ret; + + /* + * Put MPU-3050 into sleep mode before cutting regulators. + * This is important, because we may not be the sole user + * of the regulator so the power may stay on after this, and + * then we would be wasting power unless we go to sleep mode + * first. + */ + ret = regmap_update_bits(mpu3050->map, MPU3050_PWR_MGM, + MPU3050_PWR_MGM_SLEEP, MPU3050_PWR_MGM_SLEEP); + if (ret) + dev_err(mpu3050->dev, "error putting to sleep\n"); + + ret = regulator_bulk_disable(ARRAY_SIZE(mpu3050->regs), mpu3050->regs); + if (ret) + dev_err(mpu3050->dev, "error disabling regulators\n"); + + return 0; +} + +static irqreturn_t mpu3050_irq_handler(int irq, void *p) +{ + struct iio_trigger *trig = p; + struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig); + struct mpu3050 *mpu3050 = iio_priv(indio_dev); + + if (!mpu3050->hw_irq_trigger) + return IRQ_NONE; + + /* Get the time stamp as close in time as possible */ + mpu3050->hw_timestamp = iio_get_time_ns(indio_dev); + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t mpu3050_irq_thread(int irq, void *p) +{ + struct iio_trigger *trig = p; + struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig); + struct mpu3050 *mpu3050 = iio_priv(indio_dev); + unsigned int val; + int ret; + + /* ACK IRQ and check if it was from us */ + ret = regmap_read(mpu3050->map, MPU3050_INT_STATUS, &val); + if (ret) { + dev_err(mpu3050->dev, "error reading IRQ status\n"); + return IRQ_HANDLED; + } + if (!(val & MPU3050_INT_STATUS_RAW_RDY)) + return IRQ_NONE; + + iio_trigger_poll_chained(p); + + return IRQ_HANDLED; +} + +/** + * mpu3050_drdy_trigger_set_state() - set data ready interrupt state + * @trig: trigger instance + * @enable: true if trigger should be enabled, false to disable + */ +static int mpu3050_drdy_trigger_set_state(struct iio_trigger *trig, + bool enable) +{ + struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig); + struct mpu3050 *mpu3050 = iio_priv(indio_dev); + unsigned int val; + int ret; + + /* Disabling trigger: disable interrupt and return */ + if (!enable) { + /* Disable all interrupts */ + ret = regmap_write(mpu3050->map, + MPU3050_INT_CFG, + 0); + if (ret) + dev_err(mpu3050->dev, "error disabling IRQ\n"); + + /* Clear IRQ flag */ + ret = regmap_read(mpu3050->map, MPU3050_INT_STATUS, &val); + if (ret) + dev_err(mpu3050->dev, "error clearing IRQ status\n"); + + /* Disable all things in the FIFO and reset it */ + ret = regmap_write(mpu3050->map, MPU3050_FIFO_EN, 0); + if (ret) + dev_err(mpu3050->dev, "error disabling FIFO\n"); + + ret = regmap_write(mpu3050->map, MPU3050_USR_CTRL, + MPU3050_USR_CTRL_FIFO_RST); + if (ret) + dev_err(mpu3050->dev, "error resetting FIFO\n"); + + pm_runtime_mark_last_busy(mpu3050->dev); + pm_runtime_put_autosuspend(mpu3050->dev); + mpu3050->hw_irq_trigger = false; + + return 0; + } else { + /* Else we're enabling the trigger from this point */ + pm_runtime_get_sync(mpu3050->dev); + mpu3050->hw_irq_trigger = true; + + /* Disable all things in the FIFO */ + ret = regmap_write(mpu3050->map, MPU3050_FIFO_EN, 0); + if (ret) + return ret; + + /* Reset and enable the FIFO */ + ret = regmap_update_bits(mpu3050->map, MPU3050_USR_CTRL, + MPU3050_USR_CTRL_FIFO_EN | + MPU3050_USR_CTRL_FIFO_RST, + MPU3050_USR_CTRL_FIFO_EN | + MPU3050_USR_CTRL_FIFO_RST); + if (ret) + return ret; + + mpu3050->pending_fifo_footer = false; + + /* Turn on the FIFO for temp+X+Y+Z */ + ret = regmap_write(mpu3050->map, MPU3050_FIFO_EN, + MPU3050_FIFO_EN_TEMP_OUT | + MPU3050_FIFO_EN_GYRO_XOUT | + MPU3050_FIFO_EN_GYRO_YOUT | + MPU3050_FIFO_EN_GYRO_ZOUT | + MPU3050_FIFO_EN_FOOTER); + if (ret) + return ret; + + /* Configure the sample engine */ + ret = mpu3050_start_sampling(mpu3050); + if (ret) + return ret; + + /* Clear IRQ flag */ + ret = regmap_read(mpu3050->map, MPU3050_INT_STATUS, &val); + if (ret) + dev_err(mpu3050->dev, "error clearing IRQ status\n"); + + /* Give us interrupts whenever there is new data ready */ + val = MPU3050_INT_RAW_RDY_EN; + + if (mpu3050->irq_actl) + val |= MPU3050_INT_ACTL; + if (mpu3050->irq_latch) + val |= MPU3050_INT_LATCH_EN; + if (mpu3050->irq_opendrain) + val |= MPU3050_INT_OPEN; + + ret = regmap_write(mpu3050->map, MPU3050_INT_CFG, val); + if (ret) + return ret; + } + + return 0; +} + +static const struct iio_trigger_ops mpu3050_trigger_ops = { + .owner = THIS_MODULE, + .set_trigger_state = mpu3050_drdy_trigger_set_state, +}; + +static int mpu3050_trigger_probe(struct iio_dev *indio_dev, int irq) +{ + struct mpu3050 *mpu3050 = iio_priv(indio_dev); + unsigned long irq_trig; + int ret; + + mpu3050->trig = devm_iio_trigger_alloc(&indio_dev->dev, + "%s-dev%d", + indio_dev->name, + indio_dev->id); + if (!mpu3050->trig) + return -ENOMEM; + + /* Check if IRQ is open drain */ + if (of_property_read_bool(mpu3050->dev->of_node, "drive-open-drain")) + mpu3050->irq_opendrain = true; + + irq_trig = irqd_get_trigger_type(irq_get_irq_data(irq)); + /* + * Configure the interrupt generator hardware to supply whatever + * the interrupt is configured for, edges low/high level low/high, + * we can provide it all. + */ + switch (irq_trig) { + case IRQF_TRIGGER_RISING: + dev_info(&indio_dev->dev, + "pulse interrupts on the rising edge\n"); + if (mpu3050->irq_opendrain) { + dev_info(&indio_dev->dev, + "rising edge incompatible with open drain\n"); + mpu3050->irq_opendrain = false; + } + break; + case IRQF_TRIGGER_FALLING: + mpu3050->irq_actl = true; + dev_info(&indio_dev->dev, + "pulse interrupts on the falling edge\n"); + break; + case IRQF_TRIGGER_HIGH: + mpu3050->irq_latch = true; + dev_info(&indio_dev->dev, + "interrupts active high level\n"); + if (mpu3050->irq_opendrain) { + dev_info(&indio_dev->dev, + "active high incompatible with open drain\n"); + mpu3050->irq_opendrain = false; + } + /* + * With level IRQs, we mask the IRQ until it is processed, + * but with edge IRQs (pulses) we can queue several interrupts + * in the top half. + */ + irq_trig |= IRQF_ONESHOT; + break; + case IRQF_TRIGGER_LOW: + mpu3050->irq_latch = true; + mpu3050->irq_actl = true; + irq_trig |= IRQF_ONESHOT; + dev_info(&indio_dev->dev, + "interrupts active low level\n"); + break; + default: + /* This is the most preferred mode, if possible */ + dev_err(&indio_dev->dev, + "unsupported IRQ trigger specified (%lx), enforce " + "rising edge\n", irq_trig); + irq_trig = IRQF_TRIGGER_RISING; + break; + } + + /* An open drain line can be shared with several devices */ + if (mpu3050->irq_opendrain) + irq_trig |= IRQF_SHARED; + + ret = request_threaded_irq(irq, + mpu3050_irq_handler, + mpu3050_irq_thread, + irq_trig, + mpu3050->trig->name, + mpu3050->trig); + if (ret) { + dev_err(mpu3050->dev, + "can't get IRQ %d, error %d\n", irq, ret); + return ret; + } + + mpu3050->irq = irq; + mpu3050->trig->dev.parent = mpu3050->dev; + mpu3050->trig->ops = &mpu3050_trigger_ops; + iio_trigger_set_drvdata(mpu3050->trig, indio_dev); + + ret = iio_trigger_register(mpu3050->trig); + if (ret) + return ret; + + indio_dev->trig = iio_trigger_get(mpu3050->trig); + + return 0; +} + +int mpu3050_common_probe(struct device *dev, + struct regmap *map, + int irq, + const char *name) +{ + struct iio_dev *indio_dev; + struct mpu3050 *mpu3050; + unsigned int val; + int ret; + + indio_dev = devm_iio_device_alloc(dev, sizeof(*mpu3050)); + if (!indio_dev) + return -ENOMEM; + mpu3050 = iio_priv(indio_dev); + + mpu3050->dev = dev; + mpu3050->map = map; + mutex_init(&mpu3050->lock); + /* Default fullscale: 2000 degrees per second */ + mpu3050->fullscale = FS_2000_DPS; + /* 1 kHz, divide by 100, default frequency = 10 Hz */ + mpu3050->lpf = MPU3050_DLPF_CFG_188HZ; + mpu3050->divisor = 99; + + /* Read the mounting matrix, if present */ + ret = of_iio_read_mount_matrix(dev, "mount-matrix", + &mpu3050->orientation); + if (ret) + return ret; + + /* Fetch and turn on regulators */ + mpu3050->regs[0].supply = mpu3050_reg_vdd; + mpu3050->regs[1].supply = mpu3050_reg_vlogic; + ret = devm_regulator_bulk_get(dev, ARRAY_SIZE(mpu3050->regs), + mpu3050->regs); + if (ret) { + dev_err(dev, "Cannot get regulators\n"); + return ret; + } + + ret = mpu3050_power_up(mpu3050); + if (ret) + return ret; + + ret = regmap_read(map, MPU3050_CHIP_ID_REG, &val); + if (ret) { + dev_err(dev, "could not read device ID\n"); + ret = -ENODEV; + + goto err_power_down; + } + + if (val != MPU3050_CHIP_ID) { + dev_err(dev, "unsupported chip id %02x\n", (u8)val); + ret = -ENODEV; + goto err_power_down; + } + + ret = regmap_read(map, MPU3050_PRODUCT_ID_REG, &val); + if (ret) { + dev_err(dev, "could not read device ID\n"); + ret = -ENODEV; + + goto err_power_down; + } + dev_info(dev, "found MPU-3050 part no: %d, version: %d\n", + ((val >> 4) & 0xf), (val & 0xf)); + + ret = mpu3050_hw_init(mpu3050); + if (ret) + goto err_power_down; + + indio_dev->dev.parent = dev; + indio_dev->channels = mpu3050_channels; + indio_dev->num_channels = ARRAY_SIZE(mpu3050_channels); + indio_dev->info = &mpu3050_info; + indio_dev->available_scan_masks = mpu3050_scan_masks; + indio_dev->modes = INDIO_DIRECT_MODE; + indio_dev->name = name; + + ret = iio_triggered_buffer_setup(indio_dev, iio_pollfunc_store_time, + mpu3050_trigger_handler, + &mpu3050_buffer_setup_ops); + if (ret) { + dev_err(dev, "triggered buffer setup failed\n"); + goto err_power_down; + } + + ret = iio_device_register(indio_dev); + if (ret) { + dev_err(dev, "device register failed\n"); + goto err_cleanup_buffer; + } + + dev_set_drvdata(dev, indio_dev); + + /* Check if we have an assigned IRQ to use as trigger */ + if (irq) { + ret = mpu3050_trigger_probe(indio_dev, irq); + if (ret) + dev_err(dev, "failed to register trigger\n"); + } + + /* Enable runtime PM */ + pm_runtime_get_noresume(dev); + pm_runtime_set_active(dev); + pm_runtime_enable(dev); + /* + * Set autosuspend to two orders of magnitude larger than the + * start-up time. 100ms start-up time means 10000ms autosuspend, + * i.e. 10 seconds. + */ + pm_runtime_set_autosuspend_delay(dev, 10000); + pm_runtime_use_autosuspend(dev); + pm_runtime_put(dev); + + return 0; + +err_cleanup_buffer: + iio_triggered_buffer_cleanup(indio_dev); +err_power_down: + mpu3050_power_down(mpu3050); + + return ret; +} +EXPORT_SYMBOL(mpu3050_common_probe); + +int mpu3050_common_remove(struct device *dev) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct mpu3050 *mpu3050 = iio_priv(indio_dev); + + pm_runtime_get_sync(dev); + pm_runtime_put_noidle(dev); + pm_runtime_disable(dev); + iio_triggered_buffer_cleanup(indio_dev); + if (mpu3050->irq) + free_irq(mpu3050->irq, mpu3050); + iio_device_unregister(indio_dev); + mpu3050_power_down(mpu3050); + + return 0; +} +EXPORT_SYMBOL(mpu3050_common_remove); + +#ifdef CONFIG_PM +static int mpu3050_runtime_suspend(struct device *dev) +{ + return mpu3050_power_down(iio_priv(dev_get_drvdata(dev))); +} + +static int mpu3050_runtime_resume(struct device *dev) +{ + return mpu3050_power_up(iio_priv(dev_get_drvdata(dev))); +} +#endif /* CONFIG_PM */ + +const struct dev_pm_ops mpu3050_dev_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend, + pm_runtime_force_resume) + SET_RUNTIME_PM_OPS(mpu3050_runtime_suspend, + mpu3050_runtime_resume, NULL) +}; +EXPORT_SYMBOL(mpu3050_dev_pm_ops); + +MODULE_AUTHOR("Linus Walleij"); +MODULE_DESCRIPTION("MPU3050 gyroscope driver"); +MODULE_LICENSE("GPL"); diff --git a/drivers/iio/gyro/mpu3050-i2c.c b/drivers/iio/gyro/mpu3050-i2c.c new file mode 100644 index 000000000000..06007200bf49 --- /dev/null +++ b/drivers/iio/gyro/mpu3050-i2c.c @@ -0,0 +1,124 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "mpu3050.h" + +static const struct regmap_config mpu3050_i2c_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int mpu3050_i2c_bypass_select(struct i2c_mux_core *mux, u32 chan_id) +{ + struct mpu3050 *mpu3050 = i2c_mux_priv(mux); + + /* Just power up the device, that is all that is needed */ + pm_runtime_get_sync(mpu3050->dev); + return 0; +} + +static int mpu3050_i2c_bypass_deselect(struct i2c_mux_core *mux, u32 chan_id) +{ + struct mpu3050 *mpu3050 = i2c_mux_priv(mux); + + pm_runtime_mark_last_busy(mpu3050->dev); + pm_runtime_put_autosuspend(mpu3050->dev); + return 0; +} + +static int mpu3050_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct regmap *regmap; + const char *name; + struct mpu3050 *mpu3050; + int ret; + + if (!i2c_check_functionality(client->adapter, + I2C_FUNC_SMBUS_I2C_BLOCK)) + return -EOPNOTSUPP; + + if (id) + name = id->name; + else + return -ENODEV; + + regmap = devm_regmap_init_i2c(client, &mpu3050_i2c_regmap_config); + if (IS_ERR(regmap)) { + dev_err(&client->dev, "Failed to register i2c regmap %d\n", + (int)PTR_ERR(regmap)); + return PTR_ERR(regmap); + } + + ret = mpu3050_common_probe(&client->dev, regmap, client->irq, name); + if (ret) + return ret; + + /* The main driver is up, now register the I2C mux */ + mpu3050 = iio_priv(dev_get_drvdata(&client->dev)); + mpu3050->i2cmux = i2c_mux_alloc(client->adapter, &client->dev, + 1, 0, I2C_MUX_LOCKED | I2C_MUX_GATE, + mpu3050_i2c_bypass_select, + mpu3050_i2c_bypass_deselect); + /* Just fail the mux, there is no point in killing the driver */ + if (!mpu3050->i2cmux) + dev_err(&client->dev, "failed to allocate I2C mux\n"); + else { + mpu3050->i2cmux->priv = mpu3050; + ret = i2c_mux_add_adapter(mpu3050->i2cmux, 0, 0, 0); + if (ret) + dev_err(&client->dev, "failed to add I2C mux\n"); + } + + return 0; +} + +static int mpu3050_i2c_remove(struct i2c_client *client) +{ + struct iio_dev *indio_dev = dev_get_drvdata(&client->dev); + struct mpu3050 *mpu3050 = iio_priv(indio_dev); + + if (mpu3050->i2cmux) + i2c_mux_del_adapters(mpu3050->i2cmux); + + return mpu3050_common_remove(&client->dev); +} + +/* + * device id table is used to identify what device can be + * supported by this driver + */ +static const struct i2c_device_id mpu3050_i2c_id[] = { + { "mpu3050" }, + {} +}; +MODULE_DEVICE_TABLE(i2c, mpu3050_i2c_id); + +static const struct of_device_id mpu3050_i2c_of_match[] = { + { .compatible = "invensense,mpu3050", .data = "mpu3050" }, + /* Deprecated vendor ID from the Input driver */ + { .compatible = "invn,mpu3050", .data = "mpu3050" }, + { }, +}; +MODULE_DEVICE_TABLE(of, mpu3050_i2c_of_match); + +static struct i2c_driver mpu3050_i2c_driver = { + .probe = mpu3050_i2c_probe, + .remove = mpu3050_i2c_remove, + .id_table = mpu3050_i2c_id, + .driver = { + .of_match_table = mpu3050_i2c_of_match, + .name = "mpu3050-i2c", + .pm = &mpu3050_dev_pm_ops, + }, +}; +module_i2c_driver(mpu3050_i2c_driver); + +MODULE_AUTHOR("Linus Walleij"); +MODULE_DESCRIPTION("Invensense MPU3050 gyroscope driver"); +MODULE_LICENSE("GPL"); diff --git a/drivers/iio/gyro/mpu3050.h b/drivers/iio/gyro/mpu3050.h new file mode 100644 index 000000000000..bef87a714dc5 --- /dev/null +++ b/drivers/iio/gyro/mpu3050.h @@ -0,0 +1,96 @@ +#include +#include +#include +#include +#include + +/** + * enum mpu3050_fullscale - indicates the full range of the sensor in deg/sec + */ +enum mpu3050_fullscale { + FS_250_DPS = 0, + FS_500_DPS, + FS_1000_DPS, + FS_2000_DPS, +}; + +/** + * enum mpu3050_lpf - indicates the low pass filter width + */ +enum mpu3050_lpf { + /* This implicity sets sample frequency to 8 kHz */ + LPF_256_HZ_NOLPF = 0, + /* All others sets the sample frequency to 1 kHz */ + LPF_188_HZ, + LPF_98_HZ, + LPF_42_HZ, + LPF_20_HZ, + LPF_10_HZ, + LPF_5_HZ, + LPF_2100_HZ_NOLPF, +}; + +enum mpu3050_axis { + AXIS_X = 0, + AXIS_Y, + AXIS_Z, + AXIS_MAX, +}; + +/** + * struct mpu3050 - instance state container for the device + * @dev: parent device for this instance + * @orientation: mounting matrix, flipped axis etc + * @map: regmap to reach the registers + * @lock: serialization lock to marshal all requests + * @irq: the IRQ used for this device + * @regs: the regulators to power this device + * @fullscale: the current fullscale setting for the device + * @lpf: digital low pass filter setting for the device + * @divisor: base frequency divider: divides 8 or 1 kHz + * @calibration: the three signed 16-bit calibration settings that + * get written into the offset registers for each axis to compensate + * for DC offsets + * @trig: trigger for the MPU-3050 interrupt, if present + * @hw_irq_trigger: hardware interrupt trigger is in use + * @irq_actl: interrupt is active low + * @irq_latch: latched IRQ, this means that it is a level IRQ + * @irq_opendrain: the interrupt line shall be configured open drain + * @pending_fifo_footer: tells us if there is a pending footer in the FIFO + * that we have to read out first when handling the FIFO + * @hw_timestamp: latest hardware timestamp from the trigger IRQ, when in + * use + * @i2cmux: an I2C mux reflecting the fact that this sensor is a hub with + * a pass-through I2C interface coming out of it: this device needs to be + * powered up in order to reach devices on the other side of this mux + */ +struct mpu3050 { + struct device *dev; + struct iio_mount_matrix orientation; + struct regmap *map; + struct mutex lock; + int irq; + struct regulator_bulk_data regs[2]; + enum mpu3050_fullscale fullscale; + enum mpu3050_lpf lpf; + u8 divisor; + s16 calibration[3]; + struct iio_trigger *trig; + bool hw_irq_trigger; + bool irq_actl; + bool irq_latch; + bool irq_opendrain; + bool pending_fifo_footer; + s64 hw_timestamp; + struct i2c_mux_core *i2cmux; +}; + +/* Probe called from different transports */ +int mpu3050_common_probe(struct device *dev, + struct regmap *map, + int irq, + const char *name); +int mpu3050_common_remove(struct device *dev); + +/* PM ops */ +extern const struct dev_pm_ops mpu3050_dev_pm_ops; From 3089ec2c104ccfb7295d6c2a25d5b505cc59669b Mon Sep 17 00:00:00 2001 From: Arnd Bergmann Date: Tue, 25 Oct 2016 17:56:23 +0200 Subject: [PATCH 03/40] staging: iio: cdc/ad7746: fix missing return value MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit As found by "gcc -Wmaybe-uninitialized", the latest change to the driver lacked an initalization for the return code in one of the added cases: drivers/staging/iio/cdc/ad7746.c: In function ‘ad7746_read_raw’: drivers/staging/iio/cdc/ad7746.c:655:2: error: ‘ret’ may be used uninitialized in this function [-Werror=maybe-uninitialized] This sets it to IIO_VAL_INT, which I think is what we want here. Fixes: 2296c0623eb7 ("staging: iio: cdc: ad7746: implement IIO_CHAN_INFO_SAMP_FREQ") Signed-off-by: Arnd Bergmann Signed-off-by: Jonathan Cameron --- drivers/staging/iio/cdc/ad7746.c | 1 + 1 file changed, 1 insertion(+) diff --git a/drivers/staging/iio/cdc/ad7746.c b/drivers/staging/iio/cdc/ad7746.c index f41251ceeacd..a5828f9aa437 100644 --- a/drivers/staging/iio/cdc/ad7746.c +++ b/drivers/staging/iio/cdc/ad7746.c @@ -642,6 +642,7 @@ static int ad7746_read_raw(struct iio_dev *indio_dev, case IIO_VOLTAGE: *val = ad7746_vt_filter_rate_table[ (chip->config >> 6) & 0x3][0]; + ret = IIO_VAL_INT; break; default: ret = -EINVAL; From 32cb7d27e65df9daa7cee8f1fdf7b259f214bee2 Mon Sep 17 00:00:00 2001 From: Arnd Bergmann Date: Tue, 25 Oct 2016 17:55:04 +0200 Subject: [PATCH 04/40] iio: maxim_thermocouple: detect invalid storage size in read() As found by gcc -Wmaybe-uninitialized, having a storage_bytes value other than 2 or 4 will result in undefined behavior: drivers/iio/temperature/maxim_thermocouple.c: In function 'maxim_thermocouple_read': drivers/iio/temperature/maxim_thermocouple.c:141:5: error: 'ret' may be used uninitialized in this function [-Werror=maybe-uninitialized] This probably cannot happen, but returning -EINVAL here is appropriate and makes gcc happy and the code more robust. Fixes: 231147ee77f3 ("iio: maxim_thermocouple: Align 16 bit big endian value of raw reads") Signed-off-by: Arnd Bergmann Signed-off-by: Jonathan Cameron --- drivers/iio/temperature/maxim_thermocouple.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/drivers/iio/temperature/maxim_thermocouple.c b/drivers/iio/temperature/maxim_thermocouple.c index 39dd2026ccc9..d19a4dea4c3c 100644 --- a/drivers/iio/temperature/maxim_thermocouple.c +++ b/drivers/iio/temperature/maxim_thermocouple.c @@ -137,6 +137,8 @@ static int maxim_thermocouple_read(struct maxim_thermocouple_data *data, case 4: *val = be32_to_cpu(buf); break; + default: + ret = -EINVAL; } /* check to be sure this is a valid reading */ From 974e6f02e27e1b46c6c5e600e70ced25079f73eb Mon Sep 17 00:00:00 2001 From: Enric Balletbo i Serra Date: Mon, 1 Aug 2016 11:54:35 +0200 Subject: [PATCH 05/40] iio: cros_ec_sensors_core: Add common functions for the ChromeOS EC Sensor Hub. Add the core functions to be able to support the sensors attached behind the ChromeOS Embedded Controller and used by other IIO cros-ec sensor drivers. The cros_ec_sensor_core driver matches with current driver in ChromeOS 4.4 tree, so it includes all the fixes at the moment. The support for this driver was made by Gwendal Grignou. The original patch and all the fixes has been squashed and rebased on top of mainline. Signed-off-by: Gwendal Grignou Signed-off-by: Guenter Roeck [eballetbo: split, squash and rebase on top of mainline the patches found in ChromeOS tree] Signed-off-by: Enric Balletbo i Serra Signed-off-by: Jonathan Cameron --- .../ABI/testing/sysfs-bus-iio-cros-ec | 18 + drivers/iio/common/Kconfig | 1 + drivers/iio/common/Makefile | 1 + drivers/iio/common/cros_ec_sensors/Kconfig | 14 + drivers/iio/common/cros_ec_sensors/Makefile | 5 + .../cros_ec_sensors/cros_ec_sensors_core.c | 450 ++++++++++++++++++ .../cros_ec_sensors/cros_ec_sensors_core.h | 175 +++++++ include/linux/mfd/cros_ec.h | 9 + include/linux/mfd/cros_ec_commands.h | 99 +++- 9 files changed, 767 insertions(+), 5 deletions(-) create mode 100644 Documentation/ABI/testing/sysfs-bus-iio-cros-ec create mode 100644 drivers/iio/common/cros_ec_sensors/Kconfig create mode 100644 drivers/iio/common/cros_ec_sensors/Makefile create mode 100644 drivers/iio/common/cros_ec_sensors/cros_ec_sensors_core.c create mode 100644 drivers/iio/common/cros_ec_sensors/cros_ec_sensors_core.h diff --git a/Documentation/ABI/testing/sysfs-bus-iio-cros-ec b/Documentation/ABI/testing/sysfs-bus-iio-cros-ec new file mode 100644 index 000000000000..297b9720f024 --- /dev/null +++ b/Documentation/ABI/testing/sysfs-bus-iio-cros-ec @@ -0,0 +1,18 @@ +What: /sys/bus/iio/devices/iio:deviceX/calibrate +Date: July 2015 +KernelVersion: 4.7 +Contact: linux-iio@vger.kernel.org +Description: + Writing '1' will perform a FOC (Fast Online Calibration). The + corresponding calibration offsets can be read from *_calibbias + entries. + +What: /sys/bus/iio/devices/iio:deviceX/location +Date: July 2015 +KernelVersion: 4.7 +Contact: linux-iio@vger.kernel.org +Description: + This attribute returns a string with the physical location where + the motion sensor is placed. For example, in a laptop a motion + sensor can be located on the base or on the lid. Current valid + values are 'base' and 'lid'. diff --git a/drivers/iio/common/Kconfig b/drivers/iio/common/Kconfig index 26a6026de614..e108996a9627 100644 --- a/drivers/iio/common/Kconfig +++ b/drivers/iio/common/Kconfig @@ -2,6 +2,7 @@ # IIO common modules # +source "drivers/iio/common/cros_ec_sensors/Kconfig" source "drivers/iio/common/hid-sensors/Kconfig" source "drivers/iio/common/ms_sensors/Kconfig" source "drivers/iio/common/ssp_sensors/Kconfig" diff --git a/drivers/iio/common/Makefile b/drivers/iio/common/Makefile index 585da6a1b188..6fa760e1bdd5 100644 --- a/drivers/iio/common/Makefile +++ b/drivers/iio/common/Makefile @@ -7,6 +7,7 @@ # # When adding new entries keep the list in alphabetical order +obj-y += cros_ec_sensors/ obj-y += hid-sensors/ obj-y += ms_sensors/ obj-y += ssp_sensors/ diff --git a/drivers/iio/common/cros_ec_sensors/Kconfig b/drivers/iio/common/cros_ec_sensors/Kconfig new file mode 100644 index 000000000000..24743be15a5b --- /dev/null +++ b/drivers/iio/common/cros_ec_sensors/Kconfig @@ -0,0 +1,14 @@ +# +# Chrome OS Embedded Controller managed sensors library +# +config IIO_CROS_EC_SENSORS_CORE + tristate "ChromeOS EC Sensors Core" + depends on SYSFS && MFD_CROS_EC + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + help + Base module for the ChromeOS EC Sensors module. + Contains core functions used by other IIO CrosEC sensor + drivers. + Define common attributes and sysfs interrupt handler. + diff --git a/drivers/iio/common/cros_ec_sensors/Makefile b/drivers/iio/common/cros_ec_sensors/Makefile new file mode 100644 index 000000000000..95b690139bfd --- /dev/null +++ b/drivers/iio/common/cros_ec_sensors/Makefile @@ -0,0 +1,5 @@ +# +# Makefile for sensors seen through the ChromeOS EC sensor hub. +# + +obj-$(CONFIG_IIO_CROS_EC_SENSORS_CORE) += cros_ec_sensors_core.o diff --git a/drivers/iio/common/cros_ec_sensors/cros_ec_sensors_core.c b/drivers/iio/common/cros_ec_sensors/cros_ec_sensors_core.c new file mode 100644 index 000000000000..a3be7991355e --- /dev/null +++ b/drivers/iio/common/cros_ec_sensors/cros_ec_sensors_core.c @@ -0,0 +1,450 @@ +/* + * cros_ec_sensors_core - Common function for Chrome OS EC sensor driver. + * + * Copyright (C) 2016 Google, Inc + * + * This software is licensed under the terms of the GNU General Public + * License version 2, as published by the Free Software Foundation, and + * may be copied, distributed, and modified under those terms. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cros_ec_sensors_core.h" + +static char *cros_ec_loc[] = { + [MOTIONSENSE_LOC_BASE] = "base", + [MOTIONSENSE_LOC_LID] = "lid", + [MOTIONSENSE_LOC_MAX] = "unknown", +}; + +int cros_ec_sensors_core_init(struct platform_device *pdev, + struct iio_dev *indio_dev, + bool physical_device) +{ + struct device *dev = &pdev->dev; + struct cros_ec_sensors_core_state *state = iio_priv(indio_dev); + struct cros_ec_dev *ec = dev_get_drvdata(pdev->dev.parent); + struct cros_ec_sensor_platform *sensor_platform = dev_get_platdata(dev); + + platform_set_drvdata(pdev, indio_dev); + + state->ec = ec->ec_dev; + state->msg = devm_kzalloc(&pdev->dev, + max((u16)sizeof(struct ec_params_motion_sense), + state->ec->max_response), GFP_KERNEL); + if (!state->msg) + return -ENOMEM; + + state->resp = (struct ec_response_motion_sense *)state->msg->data; + + mutex_init(&state->cmd_lock); + + /* Set up the host command structure. */ + state->msg->version = 2; + state->msg->command = EC_CMD_MOTION_SENSE_CMD + ec->cmd_offset; + state->msg->outsize = sizeof(struct ec_params_motion_sense); + + indio_dev->dev.parent = &pdev->dev; + indio_dev->name = pdev->name; + + if (physical_device) { + indio_dev->modes = INDIO_DIRECT_MODE; + + state->param.cmd = MOTIONSENSE_CMD_INFO; + state->param.info.sensor_num = sensor_platform->sensor_num; + if (cros_ec_motion_send_host_cmd(state, 0)) { + dev_warn(dev, "Can not access sensor info\n"); + return -EIO; + } + state->type = state->resp->info.type; + state->loc = state->resp->info.location; + } + + return 0; +} +EXPORT_SYMBOL_GPL(cros_ec_sensors_core_init); + +int cros_ec_motion_send_host_cmd(struct cros_ec_sensors_core_state *state, + u16 opt_length) +{ + int ret; + + if (opt_length) + state->msg->insize = min(opt_length, state->ec->max_response); + else + state->msg->insize = state->ec->max_response; + + memcpy(state->msg->data, &state->param, sizeof(state->param)); + + ret = cros_ec_cmd_xfer_status(state->ec, state->msg); + if (ret < 0) + return -EIO; + + if (ret && + state->resp != (struct ec_response_motion_sense *)state->msg->data) + memcpy(state->resp, state->msg->data, ret); + + return 0; +} +EXPORT_SYMBOL_GPL(cros_ec_motion_send_host_cmd); + +static ssize_t cros_ec_sensors_calibrate(struct iio_dev *indio_dev, + uintptr_t private, const struct iio_chan_spec *chan, + const char *buf, size_t len) +{ + struct cros_ec_sensors_core_state *st = iio_priv(indio_dev); + int ret, i; + bool calibrate; + + ret = strtobool(buf, &calibrate); + if (ret < 0) + return ret; + if (!calibrate) + return -EINVAL; + + mutex_lock(&st->cmd_lock); + st->param.cmd = MOTIONSENSE_CMD_PERFORM_CALIB; + ret = cros_ec_motion_send_host_cmd(st, 0); + if (ret != 0) { + dev_warn(&indio_dev->dev, "Unable to calibrate sensor\n"); + } else { + /* Save values */ + for (i = CROS_EC_SENSOR_X; i < CROS_EC_SENSOR_MAX_AXIS; i++) + st->calib[i] = st->resp->perform_calib.offset[i]; + } + mutex_unlock(&st->cmd_lock); + + return ret ? ret : len; +} + +static ssize_t cros_ec_sensors_loc(struct iio_dev *indio_dev, + uintptr_t private, const struct iio_chan_spec *chan, + char *buf) +{ + struct cros_ec_sensors_core_state *st = iio_priv(indio_dev); + + return snprintf(buf, PAGE_SIZE, "%s\n", cros_ec_loc[st->loc]); +} + +const struct iio_chan_spec_ext_info cros_ec_sensors_ext_info[] = { + { + .name = "calibrate", + .shared = IIO_SHARED_BY_ALL, + .write = cros_ec_sensors_calibrate + }, + { + .name = "location", + .shared = IIO_SHARED_BY_ALL, + .read = cros_ec_sensors_loc + }, + { }, +}; +EXPORT_SYMBOL_GPL(cros_ec_sensors_ext_info); + +/** + * cros_ec_sensors_idx_to_reg - convert index into offset in shared memory + * @st: pointer to state information for device + * @idx: sensor index (should be element of enum sensor_index) + * + * Return: address to read at + */ +static unsigned int cros_ec_sensors_idx_to_reg( + struct cros_ec_sensors_core_state *st, + unsigned int idx) +{ + /* + * When using LPC interface, only space for 2 Accel and one Gyro. + * First halfword of MOTIONSENSE_TYPE_ACCEL is used by angle. + */ + if (st->type == MOTIONSENSE_TYPE_ACCEL) + return EC_MEMMAP_ACC_DATA + sizeof(u16) * + (1 + idx + st->param.info.sensor_num * + CROS_EC_SENSOR_MAX_AXIS); + + return EC_MEMMAP_GYRO_DATA + sizeof(u16) * idx; +} + +static int cros_ec_sensors_cmd_read_u8(struct cros_ec_device *ec, + unsigned int offset, u8 *dest) +{ + return ec->cmd_readmem(ec, offset, 1, dest); +} + +static int cros_ec_sensors_cmd_read_u16(struct cros_ec_device *ec, + unsigned int offset, u16 *dest) +{ + __le16 tmp; + int ret = ec->cmd_readmem(ec, offset, 2, &tmp); + + if (ret >= 0) + *dest = le16_to_cpu(tmp); + + return ret; +} + +/** + * cros_ec_sensors_read_until_not_busy() - read until is not busy + * + * @st: pointer to state information for device + * + * Read from EC status byte until it reads not busy. + * Return: 8-bit status if ok, -errno on failure. + */ +static int cros_ec_sensors_read_until_not_busy( + struct cros_ec_sensors_core_state *st) +{ + struct cros_ec_device *ec = st->ec; + u8 status; + int ret, attempts = 0; + + ret = cros_ec_sensors_cmd_read_u8(ec, EC_MEMMAP_ACC_STATUS, &status); + if (ret < 0) + return ret; + + while (status & EC_MEMMAP_ACC_STATUS_BUSY_BIT) { + /* Give up after enough attempts, return error. */ + if (attempts++ >= 50) + return -EIO; + + /* Small delay every so often. */ + if (attempts % 5 == 0) + msleep(25); + + ret = cros_ec_sensors_cmd_read_u8(ec, EC_MEMMAP_ACC_STATUS, + &status); + if (ret < 0) + return ret; + } + + return status; +} + +/** + * read_ec_sensors_data_unsafe() - read acceleration data from EC shared memory + * @indio_dev: pointer to IIO device + * @scan_mask: bitmap of the sensor indices to scan + * @data: location to store data + * + * This is the unsafe function for reading the EC data. It does not guarantee + * that the EC will not modify the data as it is being read in. + * + * Return: 0 on success, -errno on failure. + */ +static int cros_ec_sensors_read_data_unsafe(struct iio_dev *indio_dev, + unsigned long scan_mask, s16 *data) +{ + struct cros_ec_sensors_core_state *st = iio_priv(indio_dev); + struct cros_ec_device *ec = st->ec; + unsigned int i; + int ret; + + /* Read all sensors enabled in scan_mask. Each value is 2 bytes. */ + for_each_set_bit(i, &scan_mask, indio_dev->masklength) { + ret = cros_ec_sensors_cmd_read_u16(ec, + cros_ec_sensors_idx_to_reg(st, i), + data); + if (ret < 0) + return ret; + + data++; + } + + return 0; +} + +int cros_ec_sensors_read_lpc(struct iio_dev *indio_dev, + unsigned long scan_mask, s16 *data) +{ + struct cros_ec_sensors_core_state *st = iio_priv(indio_dev); + struct cros_ec_device *ec = st->ec; + u8 samp_id = 0xff, status = 0; + int ret, attempts = 0; + + /* + * Continually read all data from EC until the status byte after + * all reads reflects that the EC is not busy and the sample id + * matches the sample id from before all reads. This guarantees + * that data read in was not modified by the EC while reading. + */ + while ((status & (EC_MEMMAP_ACC_STATUS_BUSY_BIT | + EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK)) != samp_id) { + /* If we have tried to read too many times, return error. */ + if (attempts++ >= 5) + return -EIO; + + /* Read status byte until EC is not busy. */ + status = cros_ec_sensors_read_until_not_busy(st); + if (status < 0) + return status; + + /* + * Store the current sample id so that we can compare to the + * sample id after reading the data. + */ + samp_id = status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK; + + /* Read all EC data, format it, and store it into data. */ + ret = cros_ec_sensors_read_data_unsafe(indio_dev, scan_mask, + data); + if (ret < 0) + return ret; + + /* Read status byte. */ + ret = cros_ec_sensors_cmd_read_u8(ec, EC_MEMMAP_ACC_STATUS, + &status); + if (ret < 0) + return ret; + } + + return 0; +} +EXPORT_SYMBOL_GPL(cros_ec_sensors_read_lpc); + +int cros_ec_sensors_read_cmd(struct iio_dev *indio_dev, + unsigned long scan_mask, s16 *data) +{ + struct cros_ec_sensors_core_state *st = iio_priv(indio_dev); + int ret; + unsigned int i; + + /* Read all sensor data through a command. */ + st->param.cmd = MOTIONSENSE_CMD_DATA; + ret = cros_ec_motion_send_host_cmd(st, sizeof(st->resp->data)); + if (ret != 0) { + dev_warn(&indio_dev->dev, "Unable to read sensor data\n"); + return ret; + } + + for_each_set_bit(i, &scan_mask, indio_dev->masklength) { + *data = st->resp->data.data[i]; + data++; + } + + return 0; +} +EXPORT_SYMBOL_GPL(cros_ec_sensors_read_cmd); + +irqreturn_t cros_ec_sensors_capture(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + struct cros_ec_sensors_core_state *st = iio_priv(indio_dev); + int ret; + + mutex_lock(&st->cmd_lock); + + /* Clear capture data. */ + memset(st->samples, 0, indio_dev->scan_bytes); + + /* Read data based on which channels are enabled in scan mask. */ + ret = st->read_ec_sensors_data(indio_dev, + *(indio_dev->active_scan_mask), + (s16 *)st->samples); + if (ret < 0) + goto done; + + iio_push_to_buffers_with_timestamp(indio_dev, st->samples, + iio_get_time_ns(indio_dev)); + +done: + /* + * Tell the core we are done with this trigger and ready for the + * next one. + */ + iio_trigger_notify_done(indio_dev->trig); + + mutex_unlock(&st->cmd_lock); + + return IRQ_HANDLED; +} +EXPORT_SYMBOL_GPL(cros_ec_sensors_capture); + +int cros_ec_sensors_core_read(struct cros_ec_sensors_core_state *st, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask) +{ + int ret = IIO_VAL_INT; + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: + st->param.cmd = MOTIONSENSE_CMD_EC_RATE; + st->param.ec_rate.data = + EC_MOTION_SENSE_NO_VALUE; + + if (cros_ec_motion_send_host_cmd(st, 0)) + ret = -EIO; + else + *val = st->resp->ec_rate.ret; + break; + case IIO_CHAN_INFO_FREQUENCY: + st->param.cmd = MOTIONSENSE_CMD_SENSOR_ODR; + st->param.sensor_odr.data = + EC_MOTION_SENSE_NO_VALUE; + + if (cros_ec_motion_send_host_cmd(st, 0)) + ret = -EIO; + else + *val = st->resp->sensor_odr.ret; + break; + default: + break; + } + + return ret; +} +EXPORT_SYMBOL_GPL(cros_ec_sensors_core_read); + +int cros_ec_sensors_core_write(struct cros_ec_sensors_core_state *st, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + int ret = 0; + + switch (mask) { + case IIO_CHAN_INFO_FREQUENCY: + st->param.cmd = MOTIONSENSE_CMD_SENSOR_ODR; + st->param.sensor_odr.data = val; + + /* Always roundup, so caller gets at least what it asks for. */ + st->param.sensor_odr.roundup = 1; + + if (cros_ec_motion_send_host_cmd(st, 0)) + ret = -EIO; + break; + case IIO_CHAN_INFO_SAMP_FREQ: + st->param.cmd = MOTIONSENSE_CMD_EC_RATE; + st->param.ec_rate.data = val; + + if (cros_ec_motion_send_host_cmd(st, 0)) + ret = -EIO; + else + st->curr_sampl_freq = val; + break; + default: + ret = -EINVAL; + break; + } + return ret; +} +EXPORT_SYMBOL_GPL(cros_ec_sensors_core_write); + +MODULE_DESCRIPTION("ChromeOS EC sensor hub core functions"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/common/cros_ec_sensors/cros_ec_sensors_core.h b/drivers/iio/common/cros_ec_sensors/cros_ec_sensors_core.h new file mode 100644 index 000000000000..8bc2ca3c2e2e --- /dev/null +++ b/drivers/iio/common/cros_ec_sensors/cros_ec_sensors_core.h @@ -0,0 +1,175 @@ +/* + * ChromeOS EC sensor hub + * + * Copyright (C) 2016 Google, Inc + * + * This software is licensed under the terms of the GNU General Public + * License version 2, as published by the Free Software Foundation, and + * may be copied, distributed, and modified under those terms. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#ifndef __CROS_EC_SENSORS_CORE_H +#define __CROS_EC_SENSORS_CORE_H + +#include + +enum { + CROS_EC_SENSOR_X, + CROS_EC_SENSOR_Y, + CROS_EC_SENSOR_Z, + CROS_EC_SENSOR_MAX_AXIS, +}; + +/* EC returns sensor values using signed 16 bit registers */ +#define CROS_EC_SENSOR_BITS 16 + +/* + * 4 16 bit channels are allowed. + * Good enough for current sensors, they use up to 3 16 bit vectors. + */ +#define CROS_EC_SAMPLE_SIZE (sizeof(s64) * 2) + +/* Minimum sampling period to use when device is suspending */ +#define CROS_EC_MIN_SUSPEND_SAMPLING_FREQUENCY 1000 /* 1 second */ + +/** + * struct cros_ec_sensors_core_state - state data for EC sensors IIO driver + * @ec: cros EC device structure + * @cmd_lock: lock used to prevent simultaneous access to the + * commands. + * @msg: cros EC command structure + * @param: motion sensor parameters structure + * @resp: motion sensor response structure + * @type: type of motion sensor + * @loc: location where the motion sensor is placed + * @calib: calibration parameters. Note that trigger + * captured data will always provide the calibrated + * data + * @samples: static array to hold data from a single capture. + * For each channel we need 2 bytes, except for + * the timestamp. The timestamp is always last and + * is always 8-byte aligned. + * @read_ec_sensors_data: function used for accessing sensors values + * @cuur_sampl_freq: current sampling period + */ +struct cros_ec_sensors_core_state { + struct cros_ec_device *ec; + struct mutex cmd_lock; + + struct cros_ec_command *msg; + struct ec_params_motion_sense param; + struct ec_response_motion_sense *resp; + + enum motionsensor_type type; + enum motionsensor_location loc; + + s16 calib[CROS_EC_SENSOR_MAX_AXIS]; + + u8 samples[CROS_EC_SAMPLE_SIZE]; + + int (*read_ec_sensors_data)(struct iio_dev *indio_dev, + unsigned long scan_mask, s16 *data); + + int curr_sampl_freq; +}; + +/** + * cros_ec_sensors_read_lpc() - retrieve data from EC shared memory + * @indio_dev: pointer to IIO device + * @scan_mask: bitmap of the sensor indices to scan + * @data: location to store data + * + * This is the safe function for reading the EC data. It guarantees that the + * data sampled was not modified by the EC while being read. + * + * Return: 0 on success, -errno on failure. + */ +int cros_ec_sensors_read_lpc(struct iio_dev *indio_dev, unsigned long scan_mask, + s16 *data); + +/** + * cros_ec_sensors_read_cmd() - retrieve data using the EC command protocol + * @indio_dev: pointer to IIO device + * @scan_mask: bitmap of the sensor indices to scan + * @data: location to store data + * + * Return: 0 on success, -errno on failure. + */ +int cros_ec_sensors_read_cmd(struct iio_dev *indio_dev, unsigned long scan_mask, + s16 *data); + +/** + * cros_ec_sensors_core_init() - basic initialization of the core structure + * @pdev: platform device created for the sensors + * @indio_dev: iio device structure of the device + * @physical_device: true if the device refers to a physical device + * + * Return: 0 on success, -errno on failure. + */ +int cros_ec_sensors_core_init(struct platform_device *pdev, + struct iio_dev *indio_dev, bool physical_device); + +/** + * cros_ec_sensors_capture() - the trigger handler function + * @irq: the interrupt number. + * @p: a pointer to the poll function. + * + * On a trigger event occurring, if the pollfunc is attached then this + * handler is called as a threaded interrupt (and hence may sleep). It + * is responsible for grabbing data from the device and pushing it into + * the associated buffer. + * + * Return: IRQ_HANDLED + */ +irqreturn_t cros_ec_sensors_capture(int irq, void *p); + +/** + * cros_ec_motion_send_host_cmd() - send motion sense host command + * @st: pointer to state information for device + * @opt_length: optional length to reduce the response size, useful on the data + * path. Otherwise, the maximal allowed response size is used + * + * When called, the sub-command is assumed to be set in param->cmd. + * + * Return: 0 on success, -errno on failure. + */ +int cros_ec_motion_send_host_cmd(struct cros_ec_sensors_core_state *st, + u16 opt_length); + +/** + * cros_ec_sensors_core_read() - function to request a value from the sensor + * @st: pointer to state information for device + * @chan: channel specification structure table + * @val: will contain one element making up the returned value + * @val2: will contain another element making up the returned value + * @mask: specifies which values to be requested + * + * Return: the type of value returned by the device + */ +int cros_ec_sensors_core_read(struct cros_ec_sensors_core_state *st, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask); + +/** + * cros_ec_sensors_core_write() - function to write a value to the sensor + * @st: pointer to state information for device + * @chan: channel specification structure table + * @val: first part of value to write + * @val2: second part of value to write + * @mask: specifies which values to write + * + * Return: the type of value returned by the device + */ +int cros_ec_sensors_core_write(struct cros_ec_sensors_core_state *st, + struct iio_chan_spec const *chan, + int val, int val2, long mask); + +/* List of extended channel specification for all sensors */ +extern const struct iio_chan_spec_ext_info cros_ec_sensors_ext_info[]; + +#endif /* __CROS_EC_SENSORS_CORE_H */ diff --git a/include/linux/mfd/cros_ec.h b/include/linux/mfd/cros_ec.h index 76f7ef4d3a0d..1f85b7aff097 100644 --- a/include/linux/mfd/cros_ec.h +++ b/include/linux/mfd/cros_ec.h @@ -148,6 +148,15 @@ struct cros_ec_device { int event_size; }; +/** + * struct cros_ec_sensor_platform - ChromeOS EC sensor platform information + * + * @sensor_num: Id of the sensor, as reported by the EC. + */ +struct cros_ec_sensor_platform { + u8 sensor_num; +}; + /* struct cros_ec_platform - ChromeOS EC platform information * * @ec_name: name of EC device (e.g. 'cros-ec', 'cros-pd', ...) diff --git a/include/linux/mfd/cros_ec_commands.h b/include/linux/mfd/cros_ec_commands.h index 76728ff37d01..8826e0f64b0e 100644 --- a/include/linux/mfd/cros_ec_commands.h +++ b/include/linux/mfd/cros_ec_commands.h @@ -1315,6 +1315,24 @@ enum motionsense_command { */ MOTIONSENSE_CMD_KB_WAKE_ANGLE = 5, + /* + * Returns a single sensor data. + */ + MOTIONSENSE_CMD_DATA = 6, + + /* + * Perform low level calibration.. On sensors that support it, ask to + * do offset calibration. + */ + MOTIONSENSE_CMD_PERFORM_CALIB = 10, + + /* + * Sensor Offset command is a setter/getter command for the offset used + * for calibration. The offsets can be calculated by the host, or via + * PERFORM_CALIB command. + */ + MOTIONSENSE_CMD_SENSOR_OFFSET = 11, + /* Number of motionsense sub-commands. */ MOTIONSENSE_NUM_CMDS }; @@ -1335,12 +1353,18 @@ enum motionsensor_id { enum motionsensor_type { MOTIONSENSE_TYPE_ACCEL = 0, MOTIONSENSE_TYPE_GYRO = 1, + MOTIONSENSE_TYPE_MAG = 2, + MOTIONSENSE_TYPE_PROX = 3, + MOTIONSENSE_TYPE_LIGHT = 4, + MOTIONSENSE_TYPE_ACTIVITY = 5, + MOTIONSENSE_TYPE_MAX }; /* List of motion sensor locations. */ enum motionsensor_location { MOTIONSENSE_LOC_BASE = 0, MOTIONSENSE_LOC_LID = 1, + MOTIONSENSE_LOC_MAX, }; /* List of motion sensor chips. */ @@ -1361,6 +1385,31 @@ enum motionsensor_chip { */ #define EC_MOTION_SENSE_NO_VALUE -1 +#define EC_MOTION_SENSE_INVALID_CALIB_TEMP 0x8000 + +/* Set Calibration information */ +#define MOTION_SENSE_SET_OFFSET 1 + +struct ec_response_motion_sensor_data { + /* Flags for each sensor. */ + uint8_t flags; + /* Sensor number the data comes from */ + uint8_t sensor_num; + /* Each sensor is up to 3-axis. */ + union { + int16_t data[3]; + struct { + uint16_t rsvd; + uint32_t timestamp; + } __packed; + struct { + uint8_t activity; /* motionsensor_activity */ + uint8_t state; + int16_t add_info[2]; + }; + }; +} __packed; + struct ec_params_motion_sense { uint8_t cmd; union { @@ -1378,9 +1427,37 @@ struct ec_params_motion_sense { int16_t data; } ec_rate, kb_wake_angle; + /* Used for MOTIONSENSE_CMD_SENSOR_OFFSET */ + struct { + uint8_t sensor_num; + + /* + * bit 0: If set (MOTION_SENSE_SET_OFFSET), set + * the calibration information in the EC. + * If unset, just retrieve calibration information. + */ + uint16_t flags; + + /* + * Temperature at calibration, in units of 0.01 C + * 0x8000: invalid / unknown. + * 0x0: 0C + * 0x7fff: +327.67C + */ + int16_t temp; + + /* + * Offset for calibration. + * Unit: + * Accelerometer: 1/1024 g + * Gyro: 1/1024 deg/s + * Compass: 1/16 uT + */ + int16_t offset[3]; + } __packed sensor_offset; + /* Used for MOTIONSENSE_CMD_INFO. */ struct { - /* Should be element of enum motionsensor_id. */ uint8_t sensor_num; } info; @@ -1410,11 +1487,14 @@ struct ec_response_motion_sense { /* Flags representing the motion sensor module. */ uint8_t module_flags; - /* Flags for each sensor in enum motionsensor_id. */ - uint8_t sensor_flags[EC_MOTION_SENSOR_COUNT]; + /* Number of sensors managed directly by the EC. */ + uint8_t sensor_count; - /* Array of all sensor data. Each sensor is 3-axis. */ - int16_t data[3*EC_MOTION_SENSOR_COUNT]; + /* + * Sensor data is truncated if response_max is too small + * for holding all the data. + */ + struct ec_response_motion_sensor_data sensor[0]; } dump; /* Used for MOTIONSENSE_CMD_INFO. */ @@ -1429,6 +1509,9 @@ struct ec_response_motion_sense { uint8_t chip; } info; + /* Used for MOTIONSENSE_CMD_DATA */ + struct ec_response_motion_sensor_data data; + /* * Used for MOTIONSENSE_CMD_EC_RATE, MOTIONSENSE_CMD_SENSOR_ODR, * MOTIONSENSE_CMD_SENSOR_RANGE, and @@ -1438,6 +1521,12 @@ struct ec_response_motion_sense { /* Current value of the parameter queried. */ int32_t ret; } ec_rate, sensor_odr, sensor_range, kb_wake_angle; + + /* Used for MOTIONSENSE_CMD_SENSOR_OFFSET */ + struct { + int16_t temp; + int16_t offset[3]; + } sensor_offset, perform_calib; }; } __packed; From c14dca07a31dac8bd91aa818df62fb3bf1d846c5 Mon Sep 17 00:00:00 2001 From: Enric Balletbo i Serra Date: Mon, 1 Aug 2016 11:54:36 +0200 Subject: [PATCH 06/40] iio: cros_ec_sensors: add ChromeOS EC Contiguous Sensors driver Handle 3d contiguous sensors like Accelerometers, Gyroscope and Magnetometer that are presented by the ChromeOS EC Sensor hub. Signed-off-by: Guenter Roeck Signed-off-by: Enric Balletbo i Serra Signed-off-by: Jonathan Cameron --- drivers/iio/common/cros_ec_sensors/Kconfig | 9 + drivers/iio/common/cros_ec_sensors/Makefile | 1 + .../common/cros_ec_sensors/cros_ec_sensors.c | 322 ++++++++++++++++++ 3 files changed, 332 insertions(+) create mode 100644 drivers/iio/common/cros_ec_sensors/cros_ec_sensors.c diff --git a/drivers/iio/common/cros_ec_sensors/Kconfig b/drivers/iio/common/cros_ec_sensors/Kconfig index 24743be15a5b..3349c9d2cf7a 100644 --- a/drivers/iio/common/cros_ec_sensors/Kconfig +++ b/drivers/iio/common/cros_ec_sensors/Kconfig @@ -12,3 +12,12 @@ config IIO_CROS_EC_SENSORS_CORE drivers. Define common attributes and sysfs interrupt handler. +config IIO_CROS_EC_SENSORS + tristate "ChromeOS EC Contiguous Sensors" + select IIO_CROS_EC_SENSORS_CORE + select MFD_CROSS_EC + help + Module to handle 3d contiguous sensors like + Accelerometers, Gyroscope and Magnetometer that are + presented by the ChromeOS EC Sensor hub. + Creates an IIO device for each functions. diff --git a/drivers/iio/common/cros_ec_sensors/Makefile b/drivers/iio/common/cros_ec_sensors/Makefile index 95b690139bfd..ec716ff2a775 100644 --- a/drivers/iio/common/cros_ec_sensors/Makefile +++ b/drivers/iio/common/cros_ec_sensors/Makefile @@ -3,3 +3,4 @@ # obj-$(CONFIG_IIO_CROS_EC_SENSORS_CORE) += cros_ec_sensors_core.o +obj-$(CONFIG_IIO_CROS_EC_SENSORS) += cros_ec_sensors.o diff --git a/drivers/iio/common/cros_ec_sensors/cros_ec_sensors.c b/drivers/iio/common/cros_ec_sensors/cros_ec_sensors.c new file mode 100644 index 000000000000..48edeba35b83 --- /dev/null +++ b/drivers/iio/common/cros_ec_sensors/cros_ec_sensors.c @@ -0,0 +1,322 @@ +/* + * cros_ec_sensors - Driver for Chrome OS Embedded Controller sensors. + * + * Copyright (C) 2016 Google, 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. + * + * This driver uses the cros-ec interface to communicate with the Chrome OS + * EC about sensors data. Data access is presented through iio sysfs. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cros_ec_sensors_core.h" + +#define CROS_EC_SENSORS_MAX_CHANNELS 4 + +/* State data for ec_sensors iio driver. */ +struct cros_ec_sensors_state { + /* Shared by all sensors */ + struct cros_ec_sensors_core_state core; + + struct iio_chan_spec channels[CROS_EC_SENSORS_MAX_CHANNELS]; +}; + +static int cros_ec_sensors_read(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask) +{ + struct cros_ec_sensors_state *st = iio_priv(indio_dev); + s16 data = 0; + s64 val64; + int i; + int ret = IIO_VAL_INT; + int idx = chan->scan_index; + + mutex_lock(&st->core.cmd_lock); + + switch (mask) { + case IIO_CHAN_INFO_RAW: + ret = st->core.read_ec_sensors_data(indio_dev, 1 << idx, &data); + if (ret < 0) + break; + + *val = data; + break; + case IIO_CHAN_INFO_CALIBBIAS: + st->core.param.cmd = MOTIONSENSE_CMD_SENSOR_OFFSET; + st->core.param.sensor_offset.flags = 0; + + ret = cros_ec_motion_send_host_cmd(&st->core, 0); + if (ret < 0) + break; + + /* Save values */ + for (i = CROS_EC_SENSOR_X; i < CROS_EC_SENSOR_MAX_AXIS; i++) + st->core.calib[i] = + st->core.resp->sensor_offset.offset[i]; + + *val = st->core.calib[idx]; + break; + case IIO_CHAN_INFO_SCALE: + st->core.param.cmd = MOTIONSENSE_CMD_SENSOR_RANGE; + st->core.param.sensor_range.data = EC_MOTION_SENSE_NO_VALUE; + + ret = cros_ec_motion_send_host_cmd(&st->core, 0); + if (ret < 0) + break; + + val64 = st->core.resp->sensor_range.ret; + switch (st->core.type) { + case MOTIONSENSE_TYPE_ACCEL: + /* + * EC returns data in g, iio exepects m/s^2. + * Do not use IIO_G_TO_M_S_2 to avoid precision loss. + */ + *val = div_s64(val64 * 980665, 10); + *val2 = 10000 << (CROS_EC_SENSOR_BITS - 1); + ret = IIO_VAL_FRACTIONAL; + break; + case MOTIONSENSE_TYPE_GYRO: + /* + * EC returns data in dps, iio expects rad/s. + * Do not use IIO_DEGREE_TO_RAD to avoid precision + * loss. Round to the nearest integer. + */ + *val = div_s64(val64 * 314159 + 9000000ULL, 1000); + *val2 = 18000 << (CROS_EC_SENSOR_BITS - 1); + ret = IIO_VAL_FRACTIONAL; + break; + case MOTIONSENSE_TYPE_MAG: + /* + * EC returns data in 16LSB / uT, + * iio expects Gauss + */ + *val = val64; + *val2 = 100 << (CROS_EC_SENSOR_BITS - 1); + ret = IIO_VAL_FRACTIONAL; + break; + default: + ret = -EINVAL; + } + break; + default: + ret = cros_ec_sensors_core_read(&st->core, chan, val, val2, + mask); + break; + } + mutex_unlock(&st->core.cmd_lock); + + return ret; +} + +static int cros_ec_sensors_write(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct cros_ec_sensors_state *st = iio_priv(indio_dev); + int i; + int ret = 0; + int idx = chan->scan_index; + + mutex_lock(&st->core.cmd_lock); + + switch (mask) { + case IIO_CHAN_INFO_CALIBBIAS: + st->core.calib[idx] = val; + + /* Send to EC for each axis, even if not complete */ + st->core.param.cmd = MOTIONSENSE_CMD_SENSOR_OFFSET; + st->core.param.sensor_offset.flags = + MOTION_SENSE_SET_OFFSET; + for (i = CROS_EC_SENSOR_X; i < CROS_EC_SENSOR_MAX_AXIS; i++) + st->core.param.sensor_offset.offset[i] = + st->core.calib[i]; + st->core.param.sensor_offset.temp = + EC_MOTION_SENSE_INVALID_CALIB_TEMP; + + ret = cros_ec_motion_send_host_cmd(&st->core, 0); + break; + case IIO_CHAN_INFO_SCALE: + if (st->core.type == MOTIONSENSE_TYPE_MAG) { + ret = -EINVAL; + break; + } + st->core.param.cmd = MOTIONSENSE_CMD_SENSOR_RANGE; + st->core.param.sensor_range.data = val; + + /* Always roundup, so caller gets at least what it asks for. */ + st->core.param.sensor_range.roundup = 1; + + ret = cros_ec_motion_send_host_cmd(&st->core, 0); + break; + default: + ret = cros_ec_sensors_core_write( + &st->core, chan, val, val2, mask); + break; + } + + mutex_unlock(&st->core.cmd_lock); + + return ret; +} + +static const struct iio_info ec_sensors_info = { + .read_raw = &cros_ec_sensors_read, + .write_raw = &cros_ec_sensors_write, + .driver_module = THIS_MODULE, +}; + +static int cros_ec_sensors_probe(struct platform_device *pdev) +{ + struct device *dev = &pdev->dev; + struct cros_ec_dev *ec_dev = dev_get_drvdata(dev->parent); + struct cros_ec_device *ec_device; + struct iio_dev *indio_dev; + struct cros_ec_sensors_state *state; + struct iio_chan_spec *channel; + int ret, i; + + if (!ec_dev || !ec_dev->ec_dev) { + dev_warn(&pdev->dev, "No CROS EC device found.\n"); + return -EINVAL; + } + ec_device = ec_dev->ec_dev; + + indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(*state)); + if (!indio_dev) + return -ENOMEM; + + ret = cros_ec_sensors_core_init(pdev, indio_dev, true); + if (ret) + return ret; + + indio_dev->info = &ec_sensors_info; + state = iio_priv(indio_dev); + for (channel = state->channels, i = CROS_EC_SENSOR_X; + i < CROS_EC_SENSOR_MAX_AXIS; i++, channel++) { + /* Common part */ + channel->info_mask_separate = + BIT(IIO_CHAN_INFO_RAW) | + BIT(IIO_CHAN_INFO_CALIBBIAS); + channel->info_mask_shared_by_all = + BIT(IIO_CHAN_INFO_SCALE) | + BIT(IIO_CHAN_INFO_FREQUENCY) | + BIT(IIO_CHAN_INFO_SAMP_FREQ); + channel->scan_type.realbits = CROS_EC_SENSOR_BITS; + channel->scan_type.storagebits = CROS_EC_SENSOR_BITS; + channel->scan_index = i; + channel->ext_info = cros_ec_sensors_ext_info; + channel->modified = 1; + channel->channel2 = IIO_MOD_X + i; + channel->scan_type.sign = 's'; + + /* Sensor specific */ + switch (state->core.type) { + case MOTIONSENSE_TYPE_ACCEL: + channel->type = IIO_ACCEL; + break; + case MOTIONSENSE_TYPE_GYRO: + channel->type = IIO_ANGL_VEL; + break; + case MOTIONSENSE_TYPE_MAG: + channel->type = IIO_MAGN; + break; + default: + dev_err(&pdev->dev, "Unknown motion sensor\n"); + return -EINVAL; + } + } + + /* Timestamp */ + channel->type = IIO_TIMESTAMP; + channel->channel = -1; + channel->scan_index = CROS_EC_SENSOR_MAX_AXIS; + channel->scan_type.sign = 's'; + channel->scan_type.realbits = 64; + channel->scan_type.storagebits = 64; + + indio_dev->channels = state->channels; + indio_dev->num_channels = CROS_EC_SENSORS_MAX_CHANNELS; + + /* There is only enough room for accel and gyro in the io space */ + if ((state->core.ec->cmd_readmem != NULL) && + (state->core.type != MOTIONSENSE_TYPE_MAG)) + state->core.read_ec_sensors_data = cros_ec_sensors_read_lpc; + else + state->core.read_ec_sensors_data = cros_ec_sensors_read_cmd; + + ret = iio_triggered_buffer_setup(indio_dev, NULL, + cros_ec_sensors_capture, NULL); + if (ret) + return ret; + + ret = iio_device_register(indio_dev); + if (ret) + goto error_uninit_buffer; + + return 0; + +error_uninit_buffer: + iio_triggered_buffer_cleanup(indio_dev); + + return ret; +} + +static int cros_ec_sensors_remove(struct platform_device *pdev) +{ + struct iio_dev *indio_dev = platform_get_drvdata(pdev); + + iio_device_unregister(indio_dev); + iio_triggered_buffer_cleanup(indio_dev); + + return 0; +} + +static const struct platform_device_id cros_ec_sensors_ids[] = { + { + .name = "cros-ec-accel", + }, + { + .name = "cros-ec-gyro", + }, + { + .name = "cros-ec-mag", + }, + { /* sentinel */ } +}; +MODULE_DEVICE_TABLE(platform, cros_ec_sensors_ids); + +static struct platform_driver cros_ec_sensors_platform_driver = { + .driver = { + .name = "cros-ec-sensors", + }, + .probe = cros_ec_sensors_probe, + .remove = cros_ec_sensors_remove, + .id_table = cros_ec_sensors_ids, +}; +module_platform_driver(cros_ec_sensors_platform_driver); + +MODULE_DESCRIPTION("ChromeOS EC 3-axis sensors driver"); +MODULE_LICENSE("GPL v2"); From e4244ebddae27e9200146bba897f12a3950ce722 Mon Sep 17 00:00:00 2001 From: Vincent Palatin Date: Mon, 1 Aug 2016 11:54:37 +0200 Subject: [PATCH 07/40] platform/chrome: Introduce a new function to check EC features. Use the EC_CMD_GET_FEATURES message to check the supported features for each MCU. Signed-off-by: Vincent Palatin [tomeu: adapted to changes in mainline] Signed-off-by: Tomeu Vizoso [enric: remove references to USB PD feature and do it more generic] Signed-off-by: Enric Balletbo i Serra Reviewed-by: Guenter Roeck For the MFD changes: Acked-by: Lee Jones Signed-off-by: Jonathan Cameron --- drivers/platform/chrome/cros_ec_dev.c | 37 ++++++++++++ include/linux/mfd/cros_ec.h | 1 + include/linux/mfd/cros_ec_commands.h | 84 +++++++++++++++++++++++++++ 3 files changed, 122 insertions(+) diff --git a/drivers/platform/chrome/cros_ec_dev.c b/drivers/platform/chrome/cros_ec_dev.c index 8abd80dbcbed..7eb53078b03d 100644 --- a/drivers/platform/chrome/cros_ec_dev.c +++ b/drivers/platform/chrome/cros_ec_dev.c @@ -87,6 +87,41 @@ static int ec_get_version(struct cros_ec_dev *ec, char *str, int maxlen) return ret; } +static int cros_ec_check_features(struct cros_ec_dev *ec, int feature) +{ + struct cros_ec_command *msg; + int ret; + + if (ec->features[0] == -1U && ec->features[1] == -1U) { + /* features bitmap not read yet */ + + msg = kmalloc(sizeof(*msg) + sizeof(ec->features), GFP_KERNEL); + if (!msg) + return -ENOMEM; + + msg->version = 0; + msg->command = EC_CMD_GET_FEATURES + ec->cmd_offset; + msg->insize = sizeof(ec->features); + msg->outsize = 0; + + ret = cros_ec_cmd_xfer(ec->ec_dev, msg); + if (ret < 0 || msg->result != EC_RES_SUCCESS) { + dev_warn(ec->dev, "cannot get EC features: %d/%d\n", + ret, msg->result); + memset(ec->features, 0, sizeof(ec->features)); + } + + memcpy(ec->features, msg->data, sizeof(ec->features)); + + dev_dbg(ec->dev, "EC features %08x %08x\n", + ec->features[0], ec->features[1]); + + kfree(msg); + } + + return ec->features[feature / 32] & EC_FEATURE_MASK_0(feature); +} + /* Device file ops */ static int ec_device_open(struct inode *inode, struct file *filp) { @@ -245,6 +280,8 @@ static int ec_device_probe(struct platform_device *pdev) ec->ec_dev = dev_get_drvdata(dev->parent); ec->dev = dev; ec->cmd_offset = ec_platform->cmd_offset; + ec->features[0] = -1U; /* Not cached yet */ + ec->features[1] = -1U; /* Not cached yet */ device_initialize(&ec->class_dev); cdev_init(&ec->cdev, &fops); diff --git a/include/linux/mfd/cros_ec.h b/include/linux/mfd/cros_ec.h index 1f85b7aff097..f62043a75f43 100644 --- a/include/linux/mfd/cros_ec.h +++ b/include/linux/mfd/cros_ec.h @@ -184,6 +184,7 @@ struct cros_ec_dev { struct cros_ec_device *ec_dev; struct device *dev; u16 cmd_offset; + u32 features[2]; }; /** diff --git a/include/linux/mfd/cros_ec_commands.h b/include/linux/mfd/cros_ec_commands.h index 8826e0f64b0e..1683003603f3 100644 --- a/include/linux/mfd/cros_ec_commands.h +++ b/include/linux/mfd/cros_ec_commands.h @@ -713,6 +713,90 @@ struct ec_response_get_set_value { /* More than one command can use these structs to get/set paramters. */ #define EC_CMD_GSV_PAUSE_IN_S5 0x0c +/*****************************************************************************/ +/* List the features supported by the firmware */ +#define EC_CMD_GET_FEATURES 0x0d + +/* Supported features */ +enum ec_feature_code { + /* + * This image contains a limited set of features. Another image + * in RW partition may support more features. + */ + EC_FEATURE_LIMITED = 0, + /* + * Commands for probing/reading/writing/erasing the flash in the + * EC are present. + */ + EC_FEATURE_FLASH = 1, + /* + * Can control the fan speed directly. + */ + EC_FEATURE_PWM_FAN = 2, + /* + * Can control the intensity of the keyboard backlight. + */ + EC_FEATURE_PWM_KEYB = 3, + /* + * Support Google lightbar, introduced on Pixel. + */ + EC_FEATURE_LIGHTBAR = 4, + /* Control of LEDs */ + EC_FEATURE_LED = 5, + /* Exposes an interface to control gyro and sensors. + * The host goes through the EC to access these sensors. + * In addition, the EC may provide composite sensors, like lid angle. + */ + EC_FEATURE_MOTION_SENSE = 6, + /* The keyboard is controlled by the EC */ + EC_FEATURE_KEYB = 7, + /* The AP can use part of the EC flash as persistent storage. */ + EC_FEATURE_PSTORE = 8, + /* The EC monitors BIOS port 80h, and can return POST codes. */ + EC_FEATURE_PORT80 = 9, + /* + * Thermal management: include TMP specific commands. + * Higher level than direct fan control. + */ + EC_FEATURE_THERMAL = 10, + /* Can switch the screen backlight on/off */ + EC_FEATURE_BKLIGHT_SWITCH = 11, + /* Can switch the wifi module on/off */ + EC_FEATURE_WIFI_SWITCH = 12, + /* Monitor host events, through for example SMI or SCI */ + EC_FEATURE_HOST_EVENTS = 13, + /* The EC exposes GPIO commands to control/monitor connected devices. */ + EC_FEATURE_GPIO = 14, + /* The EC can send i2c messages to downstream devices. */ + EC_FEATURE_I2C = 15, + /* Command to control charger are included */ + EC_FEATURE_CHARGER = 16, + /* Simple battery support. */ + EC_FEATURE_BATTERY = 17, + /* + * Support Smart battery protocol + * (Common Smart Battery System Interface Specification) + */ + EC_FEATURE_SMART_BATTERY = 18, + /* EC can dectect when the host hangs. */ + EC_FEATURE_HANG_DETECT = 19, + /* Report power information, for pit only */ + EC_FEATURE_PMU = 20, + /* Another Cros EC device is present downstream of this one */ + EC_FEATURE_SUB_MCU = 21, + /* Support USB Power delivery (PD) commands */ + EC_FEATURE_USB_PD = 22, + /* Control USB multiplexer, for audio through USB port for instance. */ + EC_FEATURE_USB_MUX = 23, + /* Motion Sensor code has an internal software FIFO */ + EC_FEATURE_MOTION_SENSE_FIFO = 24, +}; + +#define EC_FEATURE_MASK_0(event_code) (1UL << (event_code % 32)) +#define EC_FEATURE_MASK_1(event_code) (1UL << (event_code - 32)) +struct ec_response_get_features { + uint32_t flags[2]; +} __packed; /*****************************************************************************/ /* Flash commands */ From 5668bfdd90cd7b330aa25a5ff853b55dc224d13d Mon Sep 17 00:00:00 2001 From: Enric Balletbo i Serra Date: Mon, 1 Aug 2016 11:54:38 +0200 Subject: [PATCH 08/40] platform/chrome: cros_ec_dev - Register cros-ec sensors Check whether the ChromeOS Embedded Controller is a sensor hub and in such case issue a command to get the number of sensors and register them all. Signed-off-by: Gwendal Grignou Signed-off-by: Enric Balletbo i Serra Reviewed-by: Guenter Roeck Acked-by: Olof Johansson Signed-off-by: Jonathan Cameron --- .../common/cros_ec_sensors/cros_ec_sensors.c | 4 +- drivers/platform/chrome/cros_ec_dev.c | 122 ++++++++++++++++++ 2 files changed, 124 insertions(+), 2 deletions(-) diff --git a/drivers/iio/common/cros_ec_sensors/cros_ec_sensors.c b/drivers/iio/common/cros_ec_sensors/cros_ec_sensors.c index 48edeba35b83..d6c372bb433b 100644 --- a/drivers/iio/common/cros_ec_sensors/cros_ec_sensors.c +++ b/drivers/iio/common/cros_ec_sensors/cros_ec_sensors.c @@ -51,7 +51,7 @@ static int cros_ec_sensors_read(struct iio_dev *indio_dev, s16 data = 0; s64 val64; int i; - int ret = IIO_VAL_INT; + int ret; int idx = chan->scan_index; mutex_lock(&st->core.cmd_lock); @@ -137,7 +137,7 @@ static int cros_ec_sensors_write(struct iio_dev *indio_dev, { struct cros_ec_sensors_state *st = iio_priv(indio_dev); int i; - int ret = 0; + int ret; int idx = chan->scan_index; mutex_lock(&st->core.cmd_lock); diff --git a/drivers/platform/chrome/cros_ec_dev.c b/drivers/platform/chrome/cros_ec_dev.c index 7eb53078b03d..47268ecedc4d 100644 --- a/drivers/platform/chrome/cros_ec_dev.c +++ b/drivers/platform/chrome/cros_ec_dev.c @@ -18,6 +18,7 @@ */ #include +#include #include #include #include @@ -265,6 +266,123 @@ static void __remove(struct device *dev) kfree(ec); } +static void cros_ec_sensors_register(struct cros_ec_dev *ec) +{ + /* + * Issue a command to get the number of sensor reported. + * Build an array of sensors driver and register them all. + */ + int ret, i, id, sensor_num; + struct mfd_cell *sensor_cells; + struct cros_ec_sensor_platform *sensor_platforms; + int sensor_type[MOTIONSENSE_TYPE_MAX]; + struct ec_params_motion_sense *params; + struct ec_response_motion_sense *resp; + struct cros_ec_command *msg; + + msg = kzalloc(sizeof(struct cros_ec_command) + + max(sizeof(*params), sizeof(*resp)), GFP_KERNEL); + if (msg == NULL) + return; + + msg->version = 2; + msg->command = EC_CMD_MOTION_SENSE_CMD + ec->cmd_offset; + msg->outsize = sizeof(*params); + msg->insize = sizeof(*resp); + + params = (struct ec_params_motion_sense *)msg->data; + params->cmd = MOTIONSENSE_CMD_DUMP; + + ret = cros_ec_cmd_xfer(ec->ec_dev, msg); + if (ret < 0 || msg->result != EC_RES_SUCCESS) { + dev_warn(ec->dev, "cannot get EC sensor information: %d/%d\n", + ret, msg->result); + goto error; + } + + resp = (struct ec_response_motion_sense *)msg->data; + sensor_num = resp->dump.sensor_count; + /* Allocate 2 extra sensors in case lid angle or FIFO are needed */ + sensor_cells = kzalloc(sizeof(struct mfd_cell) * (sensor_num + 2), + GFP_KERNEL); + if (sensor_cells == NULL) + goto error; + + sensor_platforms = kzalloc(sizeof(struct cros_ec_sensor_platform) * + (sensor_num + 1), GFP_KERNEL); + if (sensor_platforms == NULL) + goto error_platforms; + + memset(sensor_type, 0, sizeof(sensor_type)); + id = 0; + for (i = 0; i < sensor_num; i++) { + params->cmd = MOTIONSENSE_CMD_INFO; + params->info.sensor_num = i; + ret = cros_ec_cmd_xfer(ec->ec_dev, msg); + if (ret < 0 || msg->result != EC_RES_SUCCESS) { + dev_warn(ec->dev, "no info for EC sensor %d : %d/%d\n", + i, ret, msg->result); + continue; + } + switch (resp->info.type) { + case MOTIONSENSE_TYPE_ACCEL: + sensor_cells[id].name = "cros-ec-accel"; + break; + case MOTIONSENSE_TYPE_GYRO: + sensor_cells[id].name = "cros-ec-gyro"; + break; + case MOTIONSENSE_TYPE_MAG: + sensor_cells[id].name = "cros-ec-mag"; + break; + case MOTIONSENSE_TYPE_PROX: + sensor_cells[id].name = "cros-ec-prox"; + break; + case MOTIONSENSE_TYPE_LIGHT: + sensor_cells[id].name = "cros-ec-light"; + break; + case MOTIONSENSE_TYPE_ACTIVITY: + sensor_cells[id].name = "cros-ec-activity"; + break; + default: + dev_warn(ec->dev, "unknown type %d\n", resp->info.type); + continue; + } + sensor_platforms[id].sensor_num = i; + sensor_cells[id].id = sensor_type[resp->info.type]; + sensor_cells[id].platform_data = &sensor_platforms[id]; + sensor_cells[id].pdata_size = + sizeof(struct cros_ec_sensor_platform); + + sensor_type[resp->info.type]++; + id++; + } + if (sensor_type[MOTIONSENSE_TYPE_ACCEL] >= 2) { + sensor_platforms[id].sensor_num = sensor_num; + + sensor_cells[id].name = "cros-ec-angle"; + sensor_cells[id].id = 0; + sensor_cells[id].platform_data = &sensor_platforms[id]; + sensor_cells[id].pdata_size = + sizeof(struct cros_ec_sensor_platform); + id++; + } + if (cros_ec_check_features(ec, EC_FEATURE_MOTION_SENSE_FIFO)) { + sensor_cells[id].name = "cros-ec-ring"; + id++; + } + + ret = mfd_add_devices(ec->dev, 0, sensor_cells, id, + NULL, 0, NULL); + if (ret) + dev_err(ec->dev, "failed to add EC sensors\n"); + + kfree(sensor_platforms); +error_platforms: + kfree(sensor_cells); +error: + kfree(msg); +} + static int ec_device_probe(struct platform_device *pdev) { int retval = -ENOMEM; @@ -319,6 +437,10 @@ static int ec_device_probe(struct platform_device *pdev) goto dev_reg_failed; } + /* check whether this EC is a sensor hub. */ + if (cros_ec_check_features(ec, EC_FEATURE_MOTION_SENSE)) + cros_ec_sensors_register(ec); + return 0; dev_reg_failed: From b4d2192e6fa81fab2000277fd399023d3a23ac96 Mon Sep 17 00:00:00 2001 From: Jonathan Cameron Date: Sun, 30 Oct 2016 12:08:42 +0000 Subject: [PATCH 09/40] iio:cros_ec_sensors: Swap from a select to a depends in Kconfig Would have merged this into the original patch as a fixup but I've already pushed that out as an immutable branch for others to use so it'll have to be a separate patch. The original select had a typo as well. Trying to do this via a select was opening a can of worms due to a tree of other elements that would also have needed selecting. A simple depends seems much mroe straight forward and appropriate in this case. Signed-off-by: Jonathan Cameron Cc: Lee Jones Cc: Enric Balletbo i Serra --- drivers/iio/common/cros_ec_sensors/Kconfig | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/drivers/iio/common/cros_ec_sensors/Kconfig b/drivers/iio/common/cros_ec_sensors/Kconfig index 3349c9d2cf7a..135f6825903f 100644 --- a/drivers/iio/common/cros_ec_sensors/Kconfig +++ b/drivers/iio/common/cros_ec_sensors/Kconfig @@ -14,8 +14,7 @@ config IIO_CROS_EC_SENSORS_CORE config IIO_CROS_EC_SENSORS tristate "ChromeOS EC Contiguous Sensors" - select IIO_CROS_EC_SENSORS_CORE - select MFD_CROSS_EC + depends on IIO_CROS_EC_SENSORS_CORE help Module to handle 3d contiguous sensors like Accelerometers, Gyroscope and Magnetometer that are From d1c67534123d088289fa847809ba1a6fa11b1932 Mon Sep 17 00:00:00 2001 From: Wei Yongjun Date: Sat, 29 Oct 2016 16:11:20 +0000 Subject: [PATCH 10/40] iio: humidity: remove duplicated include from hts221_buffer.c Remove duplicated include. Signed-off-by: Wei Yongjun Signed-off-by: Jonathan Cameron --- drivers/iio/humidity/hts221_buffer.c | 1 - 1 file changed, 1 deletion(-) diff --git a/drivers/iio/humidity/hts221_buffer.c b/drivers/iio/humidity/hts221_buffer.c index 76b2e8130b6f..72ddcdac21a2 100644 --- a/drivers/iio/humidity/hts221_buffer.c +++ b/drivers/iio/humidity/hts221_buffer.c @@ -15,7 +15,6 @@ #include #include -#include #include #include #include From b0d801985418e0890adf14398373b9c4470911d1 Mon Sep 17 00:00:00 2001 From: Paul Kocialkowski Date: Tue, 25 Oct 2016 21:20:10 +0200 Subject: [PATCH 11/40] iio: si7020: Add devicetree support and trivial bindings This adds devicetree support for the si7020 iio driver. Since it works well without requiring any additional property, its compatible string is added to the trivial i2c devices bindings list. Signed-off-by: Paul Kocialkowski Acked-by: Rob Herring Signed-off-by: Jonathan Cameron --- .../devicetree/bindings/i2c/trivial-devices.txt | 1 + drivers/iio/humidity/si7020.c | 11 ++++++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/Documentation/devicetree/bindings/i2c/trivial-devices.txt b/Documentation/devicetree/bindings/i2c/trivial-devices.txt index 6e4ba816094b..03349ad5abfa 100644 --- a/Documentation/devicetree/bindings/i2c/trivial-devices.txt +++ b/Documentation/devicetree/bindings/i2c/trivial-devices.txt @@ -152,6 +152,7 @@ ricoh,rv5c387a I2C bus SERIAL INTERFACE REAL-TIME CLOCK IC samsung,24ad0xd1 S524AD0XF1 (128K/256K-bit Serial EEPROM for Low Power) sgx,vz89x SGX Sensortech VZ89X Sensors sii,s35390a 2-wire CMOS real-time clock +silabs,si7020 Relative Humidity and Temperature Sensors skyworks,sky81452 Skyworks SKY81452: Six-Channel White LED Driver with Touch Panel Bias Supply st,24c256 i2c serial eeprom (24cxx) st,m41t00 Serial real-time clock (RTC) diff --git a/drivers/iio/humidity/si7020.c b/drivers/iio/humidity/si7020.c index ffc2ccf6374e..345a7656c5ef 100644 --- a/drivers/iio/humidity/si7020.c +++ b/drivers/iio/humidity/si7020.c @@ -154,8 +154,17 @@ static const struct i2c_device_id si7020_id[] = { }; MODULE_DEVICE_TABLE(i2c, si7020_id); +static const struct of_device_id si7020_dt_ids[] = { + { .compatible = "silabs,si7020" }, + { } +}; +MODULE_DEVICE_TABLE(of, si7020_dt_ids); + static struct i2c_driver si7020_driver = { - .driver.name = "si7020", + .driver = { + .name = "si7020", + .of_match_table = of_match_ptr(si7020_dt_ids), + }, .probe = si7020_probe, .id_table = si7020_id, }; From dcdb0a78cab39efbfa00f005fd2691a07335f41e Mon Sep 17 00:00:00 2001 From: Lorenzo Bianconi Date: Tue, 25 Oct 2016 23:09:03 +0200 Subject: [PATCH 12/40] iio: accel: st_accel: add support to lng2dm add support to STMicroelectronics LNG2DM accelerometer to st_accel framework Signed-off-by: Lorenzo Bianconi Signed-off-by: Jonathan Cameron --- drivers/iio/accel/Kconfig | 3 +- drivers/iio/accel/st_accel.h | 1 + drivers/iio/accel/st_accel_core.c | 73 +++++++++++++++++++++++++++++++ drivers/iio/accel/st_accel_i2c.c | 5 +++ drivers/iio/accel/st_accel_spi.c | 1 + 5 files changed, 82 insertions(+), 1 deletion(-) diff --git a/drivers/iio/accel/Kconfig b/drivers/iio/accel/Kconfig index c6cc2c0909a1..c68bdb649005 100644 --- a/drivers/iio/accel/Kconfig +++ b/drivers/iio/accel/Kconfig @@ -127,7 +127,8 @@ config IIO_ST_ACCEL_3AXIS help Say yes here to build support for STMicroelectronics accelerometers: LSM303DLH, LSM303DLHC, LIS3DH, LSM330D, LSM330DL, LSM330DLC, - LIS331DLH, LSM303DL, LSM303DLM, LSM330, LIS2DH12, H3LIS331DL. + LIS331DLH, LSM303DL, LSM303DLM, LSM330, LIS2DH12, H3LIS331DL, + LNG2DM This driver can also be built as a module. If so, these modules will be created: diff --git a/drivers/iio/accel/st_accel.h b/drivers/iio/accel/st_accel.h index f8dfdb690563..7c231687109a 100644 --- a/drivers/iio/accel/st_accel.h +++ b/drivers/iio/accel/st_accel.h @@ -30,6 +30,7 @@ #define LSM303AGR_ACCEL_DEV_NAME "lsm303agr_accel" #define LIS2DH12_ACCEL_DEV_NAME "lis2dh12_accel" #define LIS3L02DQ_ACCEL_DEV_NAME "lis3l02dq" +#define LNG2DM_ACCEL_DEV_NAME "lng2dm" /** * struct st_sensors_platform_data - default accel platform data diff --git a/drivers/iio/accel/st_accel_core.c b/drivers/iio/accel/st_accel_core.c index da3fb069ec5c..b242457d0c80 100644 --- a/drivers/iio/accel/st_accel_core.c +++ b/drivers/iio/accel/st_accel_core.c @@ -231,6 +231,12 @@ #define ST_ACCEL_7_DRDY_IRQ_INT1_MASK 0x04 #define ST_ACCEL_7_MULTIREAD_BIT false +/* CUSTOM VALUES FOR SENSOR 8 */ +#define ST_ACCEL_8_FS_AVL_2_GAIN IIO_G_TO_M_S_2(15600) +#define ST_ACCEL_8_FS_AVL_4_GAIN IIO_G_TO_M_S_2(31200) +#define ST_ACCEL_8_FS_AVL_8_GAIN IIO_G_TO_M_S_2(62500) +#define ST_ACCEL_8_FS_AVL_16_GAIN IIO_G_TO_M_S_2(187500) + static const struct iio_chan_spec st_accel_8bit_channels[] = { ST_SENSORS_LSM_CHANNELS(IIO_ACCEL, BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE), @@ -726,6 +732,73 @@ static const struct st_sensor_settings st_accel_sensors_settings[] = { .multi_read_bit = ST_ACCEL_7_MULTIREAD_BIT, .bootime = 2, }, + { + .wai = ST_ACCEL_1_WAI_EXP, + .wai_addr = ST_SENSORS_DEFAULT_WAI_ADDRESS, + .sensors_supported = { + [0] = LNG2DM_ACCEL_DEV_NAME, + }, + .ch = (struct iio_chan_spec *)st_accel_8bit_channels, + .odr = { + .addr = ST_ACCEL_1_ODR_ADDR, + .mask = ST_ACCEL_1_ODR_MASK, + .odr_avl = { + { 1, ST_ACCEL_1_ODR_AVL_1HZ_VAL, }, + { 10, ST_ACCEL_1_ODR_AVL_10HZ_VAL, }, + { 25, ST_ACCEL_1_ODR_AVL_25HZ_VAL, }, + { 50, ST_ACCEL_1_ODR_AVL_50HZ_VAL, }, + { 100, ST_ACCEL_1_ODR_AVL_100HZ_VAL, }, + { 200, ST_ACCEL_1_ODR_AVL_200HZ_VAL, }, + { 400, ST_ACCEL_1_ODR_AVL_400HZ_VAL, }, + { 1600, ST_ACCEL_1_ODR_AVL_1600HZ_VAL, }, + }, + }, + .pw = { + .addr = ST_ACCEL_1_ODR_ADDR, + .mask = ST_ACCEL_1_ODR_MASK, + .value_off = ST_SENSORS_DEFAULT_POWER_OFF_VALUE, + }, + .enable_axis = { + .addr = ST_SENSORS_DEFAULT_AXIS_ADDR, + .mask = ST_SENSORS_DEFAULT_AXIS_MASK, + }, + .fs = { + .addr = ST_ACCEL_1_FS_ADDR, + .mask = ST_ACCEL_1_FS_MASK, + .fs_avl = { + [0] = { + .num = ST_ACCEL_FS_AVL_2G, + .value = ST_ACCEL_1_FS_AVL_2_VAL, + .gain = ST_ACCEL_8_FS_AVL_2_GAIN, + }, + [1] = { + .num = ST_ACCEL_FS_AVL_4G, + .value = ST_ACCEL_1_FS_AVL_4_VAL, + .gain = ST_ACCEL_8_FS_AVL_4_GAIN, + }, + [2] = { + .num = ST_ACCEL_FS_AVL_8G, + .value = ST_ACCEL_1_FS_AVL_8_VAL, + .gain = ST_ACCEL_8_FS_AVL_8_GAIN, + }, + [3] = { + .num = ST_ACCEL_FS_AVL_16G, + .value = ST_ACCEL_1_FS_AVL_16_VAL, + .gain = ST_ACCEL_8_FS_AVL_16_GAIN, + }, + }, + }, + .drdy_irq = { + .addr = ST_ACCEL_1_DRDY_IRQ_ADDR, + .mask_int1 = ST_ACCEL_1_DRDY_IRQ_INT1_MASK, + .mask_int2 = ST_ACCEL_1_DRDY_IRQ_INT2_MASK, + .addr_ihl = ST_ACCEL_1_IHL_IRQ_ADDR, + .mask_ihl = ST_ACCEL_1_IHL_IRQ_MASK, + .addr_stat_drdy = ST_SENSORS_DEFAULT_STAT_ADDR, + }, + .multi_read_bit = ST_ACCEL_1_MULTIREAD_BIT, + .bootime = 2, + }, }; static int st_accel_read_raw(struct iio_dev *indio_dev, diff --git a/drivers/iio/accel/st_accel_i2c.c b/drivers/iio/accel/st_accel_i2c.c index e9d427a5df7c..c0f8867aa1ea 100644 --- a/drivers/iio/accel/st_accel_i2c.c +++ b/drivers/iio/accel/st_accel_i2c.c @@ -84,6 +84,10 @@ static const struct of_device_id st_accel_of_match[] = { .compatible = "st,lis3l02dq", .data = LIS3L02DQ_ACCEL_DEV_NAME, }, + { + .compatible = "st,lng2dm-accel", + .data = LNG2DM_ACCEL_DEV_NAME, + }, {}, }; MODULE_DEVICE_TABLE(of, st_accel_of_match); @@ -135,6 +139,7 @@ static const struct i2c_device_id st_accel_id_table[] = { { LSM303AGR_ACCEL_DEV_NAME }, { LIS2DH12_ACCEL_DEV_NAME }, { LIS3L02DQ_ACCEL_DEV_NAME }, + { LNG2DM_ACCEL_DEV_NAME }, {}, }; MODULE_DEVICE_TABLE(i2c, st_accel_id_table); diff --git a/drivers/iio/accel/st_accel_spi.c b/drivers/iio/accel/st_accel_spi.c index efd43941d45d..c25ac50d4600 100644 --- a/drivers/iio/accel/st_accel_spi.c +++ b/drivers/iio/accel/st_accel_spi.c @@ -60,6 +60,7 @@ static const struct spi_device_id st_accel_id_table[] = { { LSM303AGR_ACCEL_DEV_NAME }, { LIS2DH12_ACCEL_DEV_NAME }, { LIS3L02DQ_ACCEL_DEV_NAME }, + { LNG2DM_ACCEL_DEV_NAME }, {}, }; MODULE_DEVICE_TABLE(spi, st_accel_id_table); From 1fbf148106326a457779934d00f55fc5417e35d9 Mon Sep 17 00:00:00 2001 From: Lorenzo Bianconi Date: Tue, 25 Oct 2016 23:09:04 +0200 Subject: [PATCH 13/40] Documentation: dt: iio: accel: add lng2dm sensor device binding Signed-off-by: Lorenzo Bianconi Signed-off-by: Jonathan Cameron --- Documentation/devicetree/bindings/iio/st-sensors.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/Documentation/devicetree/bindings/iio/st-sensors.txt b/Documentation/devicetree/bindings/iio/st-sensors.txt index e41fe340162b..c040c9ad1889 100644 --- a/Documentation/devicetree/bindings/iio/st-sensors.txt +++ b/Documentation/devicetree/bindings/iio/st-sensors.txt @@ -42,6 +42,7 @@ Accelerometers: - st,lsm303agr-accel - st,lis2dh12-accel - st,h3lis331dl-accel +- st,lng2dm-accel Gyroscopes: - st,l3g4200d-gyro From a3b6b4b63beda16f3e91fdd942bbaaaf6b1f8b25 Mon Sep 17 00:00:00 2001 From: Brian Masney Date: Fri, 28 Oct 2016 06:00:12 -0400 Subject: [PATCH 14/40] staging: iio: tsl2583: add of_match table for device tree support Add device tree support for the tsl2583 IIO driver with no custom properties. Signed-off-by: Brian Masney Acked-by: Rob Herring Signed-off-by: Jonathan Cameron --- .../devicetree/bindings/iio/light/tsl2583.txt | 26 +++++++++++++++++++ drivers/staging/iio/light/tsl2583.c | 13 ++++++++++ 2 files changed, 39 insertions(+) create mode 100644 Documentation/devicetree/bindings/iio/light/tsl2583.txt diff --git a/Documentation/devicetree/bindings/iio/light/tsl2583.txt b/Documentation/devicetree/bindings/iio/light/tsl2583.txt new file mode 100644 index 000000000000..8e2066c83f70 --- /dev/null +++ b/Documentation/devicetree/bindings/iio/light/tsl2583.txt @@ -0,0 +1,26 @@ +* TAOS TSL 2580/2581/2583 ALS sensor + +Required properties: + + - compatible: Should be one of + "amstaos,tsl2580" + "amstaos,tsl2581" + "amstaos,tsl2583" + - reg: the I2C address of the device + +Optional properties: + + - interrupt-parent: should be the phandle for the interrupt controller + - interrupts: the sole interrupt generated by the device + + Refer to interrupt-controller/interrupts.txt for generic interrupt client + node bindings. + + - vcc-supply: phandle to the regulator that provides power to the sensor. + +Example: + +tsl2581@29 { + compatible = "amstaos,tsl2581"; + reg = <0x29>; +}; diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c index 08f1583ee34e..fd4b6efd3016 100644 --- a/drivers/staging/iio/light/tsl2583.c +++ b/drivers/staging/iio/light/tsl2583.c @@ -947,11 +947,24 @@ static struct i2c_device_id taos_idtable[] = { }; MODULE_DEVICE_TABLE(i2c, taos_idtable); +#ifdef CONFIG_OF +static const struct of_device_id taos2583_of_match[] = { + { .compatible = "amstaos,tsl2580", }, + { .compatible = "amstaos,tsl2581", }, + { .compatible = "amstaos,tsl2583", }, + { }, +}; +MODULE_DEVICE_TABLE(of, taos2583_of_match); +#else +#define taos2583_of_match NULL +#endif + /* Driver definition */ static struct i2c_driver taos_driver = { .driver = { .name = "tsl2583", .pm = TAOS_PM_OPS, + .of_match_table = taos2583_of_match, }, .id_table = taos_idtable, .probe = taos_probe, From 9d5f36d2419e488dfa6ba38e4cfc1e90f65a4cff Mon Sep 17 00:00:00 2001 From: Brian Masney Date: Fri, 28 Oct 2016 06:00:13 -0400 Subject: [PATCH 15/40] staging: iio: tsl2583: check for error code from i2c_smbus_read_byte() taos_i2c_read() and taos_als_calibrate() does not check to see if the value returned by i2c_smbus_read_byte() was an error code. This patch adds the appropriate error handling. Signed-off-by: Brian Masney Signed-off-by: Jonathan Cameron --- drivers/staging/iio/light/tsl2583.c | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c index fd4b6efd3016..35c1696a71c5 100644 --- a/drivers/staging/iio/light/tsl2583.c +++ b/drivers/staging/iio/light/tsl2583.c @@ -171,7 +171,14 @@ taos_i2c_read(struct i2c_client *client, u8 reg, u8 *val, unsigned int len) return ret; } /* read the data */ - *val = i2c_smbus_read_byte(client); + ret = i2c_smbus_read_byte(client); + if (ret < 0) { + dev_err(&client->dev, + "%s failed to read byte after writing to register %x\n", + __func__, reg); + return ret; + } + *val = ret; val++; reg++; } @@ -355,6 +362,13 @@ static int taos_als_calibrate(struct iio_dev *indio_dev) } reg_val = i2c_smbus_read_byte(chip->client); + if (reg_val < 0) { + dev_err(&chip->client->dev, + "%s failed to read after writing to the CNTRL register\n", + __func__); + return ret; + } + if ((reg_val & (TSL258X_CNTL_ADC_ENBL | TSL258X_CNTL_PWR_ON)) != (TSL258X_CNTL_ADC_ENBL | TSL258X_CNTL_PWR_ON)) { dev_err(&chip->client->dev, @@ -371,6 +385,12 @@ static int taos_als_calibrate(struct iio_dev *indio_dev) return ret; } reg_val = i2c_smbus_read_byte(chip->client); + if (reg_val < 0) { + dev_err(&chip->client->dev, + "%s failed to read after writing to the STATUS register\n", + __func__); + return ret; + } if ((reg_val & TSL258X_STA_ADC_VALID) != TSL258X_STA_ADC_VALID) { dev_err(&chip->client->dev, From 20d823afff200fa131eb0dfaf6d33d8863ba8acf Mon Sep 17 00:00:00 2001 From: Brian Masney Date: Fri, 28 Oct 2016 06:00:14 -0400 Subject: [PATCH 16/40] staging: iio: tsl2583: return proper error code instead of -1 taos_als_calibrate() has a code path where -1 is returned. This patch changes the code so that a proper error code is returned. Signed-off-by: Brian Masney Signed-off-by: Jonathan Cameron --- drivers/staging/iio/light/tsl2583.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c index 35c1696a71c5..47fd373e1734 100644 --- a/drivers/staging/iio/light/tsl2583.c +++ b/drivers/staging/iio/light/tsl2583.c @@ -373,7 +373,7 @@ static int taos_als_calibrate(struct iio_dev *indio_dev) != (TSL258X_CNTL_ADC_ENBL | TSL258X_CNTL_PWR_ON)) { dev_err(&chip->client->dev, "taos_als_calibrate failed: device not powered on with ADC enabled\n"); - return -1; + return -EINVAL; } ret = i2c_smbus_write_byte(chip->client, From 27f4fabd4f9455852ebff253b21423406f24878b Mon Sep 17 00:00:00 2001 From: Brian Masney Date: Fri, 28 Oct 2016 06:00:15 -0400 Subject: [PATCH 17/40] staging: iio: tsl2583: remove redundant power_state sysfs attribute IIO devices have a /sys/bus/iio/devices/iio:deviceX/power/ directory that allows viewing and controling various power parameters. The tsl2583 driver also has an additional custom sysfs attribute named power_state that is not needed. This patch removes the redundant power_state sysfs attribute. Signed-off-by: Brian Masney Signed-off-by: Jonathan Cameron --- drivers/staging/iio/light/tsl2583.c | 31 ----------------------------- 1 file changed, 31 deletions(-) diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c index 47fd373e1734..f8ccb4def0a9 100644 --- a/drivers/staging/iio/light/tsl2583.c +++ b/drivers/staging/iio/light/tsl2583.c @@ -511,33 +511,6 @@ static int taos_chip_off(struct iio_dev *indio_dev) /* Sysfs Interface Functions */ -static ssize_t taos_power_state_show(struct device *dev, - struct device_attribute *attr, char *buf) -{ - struct iio_dev *indio_dev = dev_to_iio_dev(dev); - struct tsl2583_chip *chip = iio_priv(indio_dev); - - return sprintf(buf, "%d\n", chip->taos_chip_status); -} - -static ssize_t taos_power_state_store(struct device *dev, - struct device_attribute *attr, - const char *buf, size_t len) -{ - struct iio_dev *indio_dev = dev_to_iio_dev(dev); - int value; - - if (kstrtoint(buf, 0, &value)) - return -EINVAL; - - if (!value) - taos_chip_off(indio_dev); - else - taos_chip_on(indio_dev); - - return len; -} - static ssize_t taos_gain_show(struct device *dev, struct device_attribute *attr, char *buf) { @@ -785,9 +758,6 @@ static ssize_t taos_luxtable_store(struct device *dev, return len; } -static DEVICE_ATTR(power_state, S_IRUGO | S_IWUSR, - taos_power_state_show, taos_power_state_store); - static DEVICE_ATTR(illuminance0_calibscale, S_IRUGO | S_IWUSR, taos_gain_show, taos_gain_store); static DEVICE_ATTR(illuminance0_calibscale_available, S_IRUGO, @@ -810,7 +780,6 @@ static DEVICE_ATTR(illuminance0_lux_table, S_IRUGO | S_IWUSR, taos_luxtable_show, taos_luxtable_store); static struct attribute *sysfs_attrs_ctrl[] = { - &dev_attr_power_state.attr, &dev_attr_illuminance0_calibscale.attr, /* Gain */ &dev_attr_illuminance0_calibscale_available.attr, &dev_attr_illuminance0_integration_time.attr, /* I time*/ From 84bc1704cbf438761f870ca4ce19bbfe5b949fb6 Mon Sep 17 00:00:00 2001 From: Brian Masney Date: Fri, 28 Oct 2016 06:00:16 -0400 Subject: [PATCH 18/40] staging: iio: tsl2583: check return values from taos_chip_{on,off} The return value from taos_chip_on() and taos_chip_off() was not checked in taos_luxtable_store() and taos_probe(). This patch adds proper error checking to these function calls. Signed-off-by: Brian Masney Signed-off-by: Jonathan Cameron --- drivers/staging/iio/light/tsl2583.c | 26 ++++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c index f8ccb4def0a9..e975bba0c786 100644 --- a/drivers/staging/iio/light/tsl2583.c +++ b/drivers/staging/iio/light/tsl2583.c @@ -727,7 +727,7 @@ static ssize_t taos_luxtable_store(struct device *dev, struct iio_dev *indio_dev = dev_to_iio_dev(dev); struct tsl2583_chip *chip = iio_priv(indio_dev); int value[ARRAY_SIZE(taos_device_lux) * 3 + 1]; - int n; + int n, ret = -EINVAL; get_options(buf, ARRAY_SIZE(value), value); @@ -739,23 +739,31 @@ static ssize_t taos_luxtable_store(struct device *dev, n = value[0]; if ((n % 3) || n < 6 || n > ((ARRAY_SIZE(taos_device_lux) - 1) * 3)) { dev_info(dev, "LUX TABLE INPUT ERROR 1 Value[0]=%d\n", n); - return -EINVAL; + goto done; } if ((value[(n - 2)] | value[(n - 1)] | value[n]) != 0) { dev_info(dev, "LUX TABLE INPUT ERROR 2 Value[0]=%d\n", n); - return -EINVAL; + goto done; } - if (chip->taos_chip_status == TSL258X_CHIP_WORKING) - taos_chip_off(indio_dev); + if (chip->taos_chip_status == TSL258X_CHIP_WORKING) { + ret = taos_chip_off(indio_dev); + if (ret < 0) + goto done; + } /* Zero out the table */ memset(taos_device_lux, 0, sizeof(taos_device_lux)); memcpy(taos_device_lux, &value[1], (value[0] * 4)); - taos_chip_on(indio_dev); + ret = taos_chip_on(indio_dev); + if (ret < 0) + goto done; - return len; + ret = len; + +done: + return ret; } static DEVICE_ATTR(illuminance0_calibscale, S_IRUGO | S_IWUSR, @@ -883,7 +891,9 @@ static int taos_probe(struct i2c_client *clientp, taos_defaults(chip); /* Make sure the chip is on */ - taos_chip_on(indio_dev); + ret = taos_chip_on(indio_dev); + if (ret < 0) + return ret; dev_info(&clientp->dev, "Light sensor found.\n"); return 0; From b3d941550d9d3753ed76f09c9b1015c777f41f17 Mon Sep 17 00:00:00 2001 From: Brian Masney Date: Fri, 28 Oct 2016 06:00:17 -0400 Subject: [PATCH 19/40] staging: iio: tsl2583: convert to use iio_chan_spec and {read,write}_raw The tsl2583 driver directly creates sysfs attributes that should instead be created by the IIO core on behalf of the driver. This patch adds the iio_chan_spec array, the relevant info_mask elements and the read_raw() and write_raw() functions to take advantage of features provided by the IIO core. These sysfs attributes were migrated with this patch: illuminance0_input, illuminance0_calibbias, illuminance0_integration_time. This also exposes the raw values read from the two channels on the sensor. With this change, these four sysfs entries have their prefix changed from illuminance0_ to in_illuminance_. This is deemed to be acceptable since none of the IIO light drivers in mainline use the illuminance0_ prefix, however 8 of the IIO light drivers in mainline use the in_illuminance_ prefix. Also fix the units of integration_time to meet with the ABI. Signed-off-by: Brian Masney Signed-off-by: Jonathan Cameron --- drivers/staging/iio/light/tsl2583.c | 236 +++++++++++++++++----------- 1 file changed, 143 insertions(+), 93 deletions(-) diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c index e975bba0c786..6a61a86d0d40 100644 --- a/drivers/staging/iio/light/tsl2583.c +++ b/drivers/staging/iio/light/tsl2583.c @@ -211,28 +211,23 @@ static int taos_get_lux(struct iio_dev *indio_dev) u32 ch0lux = 0; u32 ch1lux = 0; - if (mutex_trylock(&chip->als_mutex) == 0) { - dev_info(&chip->client->dev, "taos_get_lux device is busy\n"); - return chip->als_cur_info.lux; /* busy, so return LAST VALUE */ - } - if (chip->taos_chip_status != TSL258X_CHIP_WORKING) { /* device is not enabled */ dev_err(&chip->client->dev, "taos_get_lux device is not enabled\n"); ret = -EBUSY; - goto out_unlock; + goto done; } ret = taos_i2c_read(chip->client, (TSL258X_CMD_REG), &buf[0], 1); if (ret < 0) { dev_err(&chip->client->dev, "taos_get_lux failed to read CMD_REG\n"); - goto out_unlock; + goto done; } /* is data new & valid */ if (!(buf[0] & TSL258X_STA_ADC_INTR)) { dev_err(&chip->client->dev, "taos_get_lux data not valid\n"); ret = chip->als_cur_info.lux; /* return LAST VALUE */ - goto out_unlock; + goto done; } for (i = 0; i < 4; i++) { @@ -243,7 +238,7 @@ static int taos_get_lux(struct iio_dev *indio_dev) dev_err(&chip->client->dev, "taos_get_lux failed to read register %x\n", reg); - goto out_unlock; + goto done; } } @@ -259,7 +254,7 @@ static int taos_get_lux(struct iio_dev *indio_dev) dev_err(&chip->client->dev, "taos_i2c_write_command failed in taos_get_lux, err = %d\n", ret); - goto out_unlock; /* have no data, so return failure */ + goto done; /* have no data, so return failure */ } /* extract ALS/lux data */ @@ -276,7 +271,7 @@ static int taos_get_lux(struct iio_dev *indio_dev) /* have no data, so return LAST VALUE */ ret = 0; chip->als_cur_info.lux = 0; - goto out_unlock; + goto done; } /* calculate ratio */ ratio = (ch1 << 15) / ch0; @@ -302,7 +297,7 @@ static int taos_get_lux(struct iio_dev *indio_dev) dev_dbg(&chip->client->dev, "No Data - Return last value\n"); ret = 0; chip->als_cur_info.lux = 0; - goto out_unlock; + goto done; } /* adjust for active time scale */ @@ -334,8 +329,7 @@ static int taos_get_lux(struct iio_dev *indio_dev) chip->als_cur_info.lux = lux; ret = lux; -out_unlock: - mutex_unlock(&chip->als_mutex); +done: return ret; } @@ -575,69 +569,12 @@ static ssize_t taos_gain_available_show(struct device *dev, return sprintf(buf, "%s\n", "1 8 16 111"); } -static ssize_t taos_als_time_show(struct device *dev, - struct device_attribute *attr, char *buf) -{ - struct iio_dev *indio_dev = dev_to_iio_dev(dev); - struct tsl2583_chip *chip = iio_priv(indio_dev); - - return sprintf(buf, "%d\n", chip->taos_settings.als_time); -} - -static ssize_t taos_als_time_store(struct device *dev, - struct device_attribute *attr, - const char *buf, size_t len) -{ - struct iio_dev *indio_dev = dev_to_iio_dev(dev); - struct tsl2583_chip *chip = iio_priv(indio_dev); - int value; - - if (kstrtoint(buf, 0, &value)) - return -EINVAL; - - if ((value < 50) || (value > 650)) - return -EINVAL; - - if (value % 50) - return -EINVAL; - - chip->taos_settings.als_time = value; - - return len; -} - static ssize_t taos_als_time_available_show(struct device *dev, struct device_attribute *attr, char *buf) { return sprintf(buf, "%s\n", - "50 100 150 200 250 300 350 400 450 500 550 600 650"); -} - -static ssize_t taos_als_trim_show(struct device *dev, - struct device_attribute *attr, char *buf) -{ - struct iio_dev *indio_dev = dev_to_iio_dev(dev); - struct tsl2583_chip *chip = iio_priv(indio_dev); - - return sprintf(buf, "%d\n", chip->taos_settings.als_gain_trim); -} - -static ssize_t taos_als_trim_store(struct device *dev, - struct device_attribute *attr, - const char *buf, size_t len) -{ - struct iio_dev *indio_dev = dev_to_iio_dev(dev); - struct tsl2583_chip *chip = iio_priv(indio_dev); - int value; - - if (kstrtoint(buf, 0, &value)) - return -EINVAL; - - if (value) - chip->taos_settings.als_gain_trim = value; - - return len; + "0.000050 0.000100 0.000150 0.000200 0.000250 0.000300 0.000350 0.000400 0.000450 0.000500 0.000550 0.000600 0.000650"); } static ssize_t taos_als_cal_target_show(struct device *dev, @@ -667,18 +604,6 @@ static ssize_t taos_als_cal_target_store(struct device *dev, return len; } -static ssize_t taos_lux_show(struct device *dev, struct device_attribute *attr, - char *buf) -{ - int ret; - - ret = taos_get_lux(dev_to_iio_dev(dev)); - if (ret < 0) - return ret; - - return sprintf(buf, "%d\n", ret); -} - static ssize_t taos_do_calibrate(struct device *dev, struct device_attribute *attr, const char *buf, size_t len) @@ -771,18 +696,12 @@ static DEVICE_ATTR(illuminance0_calibscale, S_IRUGO | S_IWUSR, static DEVICE_ATTR(illuminance0_calibscale_available, S_IRUGO, taos_gain_available_show, NULL); -static DEVICE_ATTR(illuminance0_integration_time, S_IRUGO | S_IWUSR, - taos_als_time_show, taos_als_time_store); static DEVICE_ATTR(illuminance0_integration_time_available, S_IRUGO, taos_als_time_available_show, NULL); -static DEVICE_ATTR(illuminance0_calibbias, S_IRUGO | S_IWUSR, - taos_als_trim_show, taos_als_trim_store); - static DEVICE_ATTR(illuminance0_input_target, S_IRUGO | S_IWUSR, taos_als_cal_target_show, taos_als_cal_target_store); -static DEVICE_ATTR(illuminance0_input, S_IRUGO, taos_lux_show, NULL); static DEVICE_ATTR(illuminance0_calibrate, S_IWUSR, NULL, taos_do_calibrate); static DEVICE_ATTR(illuminance0_lux_table, S_IRUGO | S_IWUSR, taos_luxtable_show, taos_luxtable_store); @@ -790,11 +709,8 @@ static DEVICE_ATTR(illuminance0_lux_table, S_IRUGO | S_IWUSR, static struct attribute *sysfs_attrs_ctrl[] = { &dev_attr_illuminance0_calibscale.attr, /* Gain */ &dev_attr_illuminance0_calibscale_available.attr, - &dev_attr_illuminance0_integration_time.attr, /* I time*/ &dev_attr_illuminance0_integration_time_available.attr, - &dev_attr_illuminance0_calibbias.attr, /* trim */ &dev_attr_illuminance0_input_target.attr, - &dev_attr_illuminance0_input.attr, &dev_attr_illuminance0_calibrate.attr, &dev_attr_illuminance0_lux_table.attr, NULL @@ -810,9 +726,141 @@ static int taos_tsl258x_device(unsigned char *bufp) return ((bufp[TSL258X_CHIPID] & 0xf0) == 0x90); } +static const struct iio_chan_spec tsl2583_channels[] = { + { + .type = IIO_LIGHT, + .modified = 1, + .channel2 = IIO_MOD_LIGHT_IR, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), + }, + { + .type = IIO_LIGHT, + .modified = 1, + .channel2 = IIO_MOD_LIGHT_BOTH, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), + }, + { + .type = IIO_LIGHT, + .info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED) | + BIT(IIO_CHAN_INFO_CALIBBIAS) | + BIT(IIO_CHAN_INFO_INT_TIME), + }, +}; + +static int tsl2583_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask) +{ + struct tsl2583_chip *chip = iio_priv(indio_dev); + int ret = -EINVAL; + + mutex_lock(&chip->als_mutex); + + if (chip->taos_chip_status != TSL258X_CHIP_WORKING) { + ret = -EBUSY; + goto read_done; + } + + switch (mask) { + case IIO_CHAN_INFO_RAW: + if (chan->type == IIO_LIGHT) { + ret = taos_get_lux(indio_dev); + if (ret < 0) + goto read_done; + + /* + * From page 20 of the TSL2581, TSL2583 data + * sheet (TAOS134 − MARCH 2011): + * + * One of the photodiodes (channel 0) is + * sensitive to both visible and infrared light, + * while the second photodiode (channel 1) is + * sensitive primarily to infrared light. + */ + if (chan->channel2 == IIO_MOD_LIGHT_BOTH) + *val = chip->als_cur_info.als_ch0; + else + *val = chip->als_cur_info.als_ch1; + + ret = IIO_VAL_INT; + } + break; + case IIO_CHAN_INFO_PROCESSED: + if (chan->type == IIO_LIGHT) { + ret = taos_get_lux(indio_dev); + if (ret < 0) + goto read_done; + + *val = ret; + ret = IIO_VAL_INT; + } + break; + case IIO_CHAN_INFO_CALIBBIAS: + if (chan->type == IIO_LIGHT) { + *val = chip->taos_settings.als_gain_trim; + ret = IIO_VAL_INT; + } + break; + case IIO_CHAN_INFO_INT_TIME: + if (chan->type == IIO_LIGHT) { + *val = 0; + *val2 = chip->taos_settings.als_time; + ret = IIO_VAL_INT_PLUS_MICRO; + } + break; + default: + break; + } + +read_done: + mutex_unlock(&chip->als_mutex); + + return ret; +} + +static int tsl2583_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct tsl2583_chip *chip = iio_priv(indio_dev); + int ret = -EINVAL; + + mutex_lock(&chip->als_mutex); + + if (chip->taos_chip_status != TSL258X_CHIP_WORKING) { + ret = -EBUSY; + goto write_done; + } + + switch (mask) { + case IIO_CHAN_INFO_CALIBBIAS: + if (chan->type == IIO_LIGHT) { + chip->taos_settings.als_gain_trim = val; + ret = 0; + } + break; + case IIO_CHAN_INFO_INT_TIME: + if (chan->type == IIO_LIGHT && !val && val2 >= 50 && + val2 <= 650 && !(val2 % 50)) { + chip->taos_settings.als_time = val2; + ret = 0; + } + break; + default: + break; + } + +write_done: + mutex_unlock(&chip->als_mutex); + + return ret; +} + static const struct iio_info tsl2583_info = { .attrs = &tsl2583_attribute_group, .driver_module = THIS_MODULE, + .read_raw = tsl2583_read_raw, + .write_raw = tsl2583_write_raw, }; /* @@ -878,6 +926,8 @@ static int taos_probe(struct i2c_client *clientp, } indio_dev->info = &tsl2583_info; + indio_dev->channels = tsl2583_channels; + indio_dev->num_channels = ARRAY_SIZE(tsl2583_channels); indio_dev->dev.parent = &clientp->dev; indio_dev->modes = INDIO_DIRECT_MODE; indio_dev->name = chip->client->name; From 143c30f4a51ebff3474643994499468cf56240af Mon Sep 17 00:00:00 2001 From: Brian Masney Date: Fri, 28 Oct 2016 06:00:18 -0400 Subject: [PATCH 20/40] staging: iio: tsl2583: convert illuminance0_calibscale sysfs attr to use iio_chan_spec The illuminance0_calibscale sysfs attribute is not currently created by the IIO core. This patch adds the appropriate mask to iio_chan_spec, along with the appropriate data handling in the read_raw() and write_raw() functions, so that the sysfs attribute is created by the IIO core. With this change, this sysfs entry will have its prefix changed from illuminance0_ to in_illuminance_. Signed-off-by: Brian Masney Signed-off-by: Jonathan Cameron --- drivers/staging/iio/light/tsl2583.c | 89 ++++++++--------------------- 1 file changed, 25 insertions(+), 64 deletions(-) diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c index 6a61a86d0d40..bfff6ca54fe5 100644 --- a/drivers/staging/iio/light/tsl2583.c +++ b/drivers/staging/iio/light/tsl2583.c @@ -124,14 +124,15 @@ static struct taos_lux taos_device_lux[11] = { struct gainadj { s16 ch0; s16 ch1; + s16 mean; }; /* Index = (0 - 3) Used to validate the gain selection index */ static const struct gainadj gainadj[] = { - { 1, 1 }, - { 8, 8 }, - { 16, 16 }, - { 107, 115 } + { 1, 1, 1 }, + { 8, 8, 8 }, + { 16, 16, 16 }, + { 107, 115, 111 } }; /* @@ -505,63 +506,6 @@ static int taos_chip_off(struct iio_dev *indio_dev) /* Sysfs Interface Functions */ -static ssize_t taos_gain_show(struct device *dev, - struct device_attribute *attr, char *buf) -{ - struct iio_dev *indio_dev = dev_to_iio_dev(dev); - struct tsl2583_chip *chip = iio_priv(indio_dev); - char gain[4] = {0}; - - switch (chip->taos_settings.als_gain) { - case 0: - strcpy(gain, "001"); - break; - case 1: - strcpy(gain, "008"); - break; - case 2: - strcpy(gain, "016"); - break; - case 3: - strcpy(gain, "111"); - break; - } - - return sprintf(buf, "%s\n", gain); -} - -static ssize_t taos_gain_store(struct device *dev, - struct device_attribute *attr, - const char *buf, size_t len) -{ - struct iio_dev *indio_dev = dev_to_iio_dev(dev); - struct tsl2583_chip *chip = iio_priv(indio_dev); - int value; - - if (kstrtoint(buf, 0, &value)) - return -EINVAL; - - switch (value) { - case 1: - chip->taos_settings.als_gain = 0; - break; - case 8: - chip->taos_settings.als_gain = 1; - break; - case 16: - chip->taos_settings.als_gain = 2; - break; - case 111: - chip->taos_settings.als_gain = 3; - break; - default: - dev_err(dev, "Invalid Gain Index (must be 1,8,16,111)\n"); - return -1; - } - - return len; -} - static ssize_t taos_gain_available_show(struct device *dev, struct device_attribute *attr, char *buf) @@ -691,8 +635,6 @@ static ssize_t taos_luxtable_store(struct device *dev, return ret; } -static DEVICE_ATTR(illuminance0_calibscale, S_IRUGO | S_IWUSR, - taos_gain_show, taos_gain_store); static DEVICE_ATTR(illuminance0_calibscale_available, S_IRUGO, taos_gain_available_show, NULL); @@ -707,7 +649,6 @@ static DEVICE_ATTR(illuminance0_lux_table, S_IRUGO | S_IWUSR, taos_luxtable_show, taos_luxtable_store); static struct attribute *sysfs_attrs_ctrl[] = { - &dev_attr_illuminance0_calibscale.attr, /* Gain */ &dev_attr_illuminance0_calibscale_available.attr, &dev_attr_illuminance0_integration_time_available.attr, &dev_attr_illuminance0_input_target.attr, @@ -743,6 +684,7 @@ static const struct iio_chan_spec tsl2583_channels[] = { .type = IIO_LIGHT, .info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED) | BIT(IIO_CHAN_INFO_CALIBBIAS) | + BIT(IIO_CHAN_INFO_CALIBSCALE) | BIT(IIO_CHAN_INFO_INT_TIME), }, }; @@ -801,6 +743,12 @@ static int tsl2583_read_raw(struct iio_dev *indio_dev, ret = IIO_VAL_INT; } break; + case IIO_CHAN_INFO_CALIBSCALE: + if (chan->type == IIO_LIGHT) { + *val = gainadj[chip->taos_settings.als_gain].mean; + ret = IIO_VAL_INT; + } + break; case IIO_CHAN_INFO_INT_TIME: if (chan->type == IIO_LIGHT) { *val = 0; @@ -839,6 +787,19 @@ static int tsl2583_write_raw(struct iio_dev *indio_dev, ret = 0; } break; + case IIO_CHAN_INFO_CALIBSCALE: + if (chan->type == IIO_LIGHT) { + int i; + + for (i = 0; i < ARRAY_SIZE(gainadj); i++) { + if (gainadj[i].mean == val) { + chip->taos_settings.als_gain = i; + ret = 0; + break; + } + } + } + break; case IIO_CHAN_INFO_INT_TIME: if (chan->type == IIO_LIGHT && !val && val2 >= 50 && val2 <= 650 && !(val2 % 50)) { From b2fa81be96ca315a164ef7bc1d87fbf025fd9954 Mon Sep 17 00:00:00 2001 From: Brian Masney Date: Fri, 28 Oct 2016 06:00:19 -0400 Subject: [PATCH 21/40] staging: iio: tsl2583: use IIO_*_ATTR* macros to create sysfs entries Use the IIO_CONST_ATTR, IIO_DEVICE_ATTR_RW, and IIO_DEVICE_ATTR_WO macros for creating the in_illuminance_calibscale_available, in_illuminance_integration_time_available, in_illuminance_input_target, in_illuminance_calibrate, and in_illuminance_lux_table sysfs entries. Previously these sysfs entries were prefixed with illuminance0_, however they are now prefixed with in_illuminance_ to make these sysfs entries consistent with how the IIO core is creating the other sysfs entries. Signed-off-by: Brian Masney Signed-off-by: Jonathan Cameron --- drivers/staging/iio/light/tsl2583.c | 73 +++++++++++------------------ 1 file changed, 27 insertions(+), 46 deletions(-) diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c index bfff6ca54fe5..14623740621d 100644 --- a/drivers/staging/iio/light/tsl2583.c +++ b/drivers/staging/iio/light/tsl2583.c @@ -29,6 +29,7 @@ #include #include #include +#include #define TSL258X_MAX_DEVICE_REGS 32 @@ -506,24 +507,9 @@ static int taos_chip_off(struct iio_dev *indio_dev) /* Sysfs Interface Functions */ -static ssize_t taos_gain_available_show(struct device *dev, - struct device_attribute *attr, - char *buf) -{ - return sprintf(buf, "%s\n", "1 8 16 111"); -} - -static ssize_t taos_als_time_available_show(struct device *dev, - struct device_attribute *attr, - char *buf) -{ - return sprintf(buf, "%s\n", - "0.000050 0.000100 0.000150 0.000200 0.000250 0.000300 0.000350 0.000400 0.000450 0.000500 0.000550 0.000600 0.000650"); -} - -static ssize_t taos_als_cal_target_show(struct device *dev, - struct device_attribute *attr, - char *buf) +static ssize_t in_illuminance_input_target_show(struct device *dev, + struct device_attribute *attr, + char *buf) { struct iio_dev *indio_dev = dev_to_iio_dev(dev); struct tsl2583_chip *chip = iio_priv(indio_dev); @@ -531,9 +517,9 @@ static ssize_t taos_als_cal_target_show(struct device *dev, return sprintf(buf, "%d\n", chip->taos_settings.als_cal_target); } -static ssize_t taos_als_cal_target_store(struct device *dev, - struct device_attribute *attr, - const char *buf, size_t len) +static ssize_t in_illuminance_input_target_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t len) { struct iio_dev *indio_dev = dev_to_iio_dev(dev); struct tsl2583_chip *chip = iio_priv(indio_dev); @@ -548,9 +534,9 @@ static ssize_t taos_als_cal_target_store(struct device *dev, return len; } -static ssize_t taos_do_calibrate(struct device *dev, - struct device_attribute *attr, - const char *buf, size_t len) +static ssize_t in_illuminance_calibrate_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t len) { struct iio_dev *indio_dev = dev_to_iio_dev(dev); int value; @@ -564,8 +550,9 @@ static ssize_t taos_do_calibrate(struct device *dev, return len; } -static ssize_t taos_luxtable_show(struct device *dev, - struct device_attribute *attr, char *buf) +static ssize_t in_illuminance_lux_table_show(struct device *dev, + struct device_attribute *attr, + char *buf) { int i; int offset = 0; @@ -589,9 +576,9 @@ static ssize_t taos_luxtable_show(struct device *dev, return offset; } -static ssize_t taos_luxtable_store(struct device *dev, - struct device_attribute *attr, - const char *buf, size_t len) +static ssize_t in_illuminance_lux_table_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t len) { struct iio_dev *indio_dev = dev_to_iio_dev(dev); struct tsl2583_chip *chip = iio_priv(indio_dev); @@ -635,25 +622,19 @@ static ssize_t taos_luxtable_store(struct device *dev, return ret; } -static DEVICE_ATTR(illuminance0_calibscale_available, S_IRUGO, - taos_gain_available_show, NULL); - -static DEVICE_ATTR(illuminance0_integration_time_available, S_IRUGO, - taos_als_time_available_show, NULL); - -static DEVICE_ATTR(illuminance0_input_target, S_IRUGO | S_IWUSR, - taos_als_cal_target_show, taos_als_cal_target_store); - -static DEVICE_ATTR(illuminance0_calibrate, S_IWUSR, NULL, taos_do_calibrate); -static DEVICE_ATTR(illuminance0_lux_table, S_IRUGO | S_IWUSR, - taos_luxtable_show, taos_luxtable_store); +static IIO_CONST_ATTR(in_illuminance_calibscale_available, "1 8 16 111"); +static IIO_CONST_ATTR(in_illuminance_integration_time_available, + "0.000050 0.000100 0.000150 0.000200 0.000250 0.000300 0.000350 0.000400 0.000450 0.000500 0.000550 0.000600 0.000650"); +static IIO_DEVICE_ATTR_RW(in_illuminance_input_target, 0); +static IIO_DEVICE_ATTR_WO(in_illuminance_calibrate, 0); +static IIO_DEVICE_ATTR_RW(in_illuminance_lux_table, 0); static struct attribute *sysfs_attrs_ctrl[] = { - &dev_attr_illuminance0_calibscale_available.attr, - &dev_attr_illuminance0_integration_time_available.attr, - &dev_attr_illuminance0_input_target.attr, - &dev_attr_illuminance0_calibrate.attr, - &dev_attr_illuminance0_lux_table.attr, + &iio_const_attr_in_illuminance_calibscale_available.dev_attr.attr, + &iio_const_attr_in_illuminance_integration_time_available.dev_attr.attr, + &iio_dev_attr_in_illuminance_input_target.dev_attr.attr, + &iio_dev_attr_in_illuminance_calibrate.dev_attr.attr, + &iio_dev_attr_in_illuminance_lux_table.dev_attr.attr, NULL }; From f375a49f710535b0ac2923274e2a33f04e02514d Mon Sep 17 00:00:00 2001 From: Brian Masney Date: Fri, 28 Oct 2016 06:00:20 -0400 Subject: [PATCH 22/40] staging: iio: tsl2583: add error code to sysfs store functions in_illuminance_input_target_store() and in_illuminance_calibrate_store() validated the data from userspace, however it would not return an error code to userspace if an invalid value was passed in. This patch changes these functions so that they return -EINVAL if invalid data is passed in. Signed-off-by: Brian Masney Signed-off-by: Jonathan Cameron --- drivers/staging/iio/light/tsl2583.c | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c index 14623740621d..98afa5bdfb29 100644 --- a/drivers/staging/iio/light/tsl2583.c +++ b/drivers/staging/iio/light/tsl2583.c @@ -525,11 +525,10 @@ static ssize_t in_illuminance_input_target_store(struct device *dev, struct tsl2583_chip *chip = iio_priv(indio_dev); int value; - if (kstrtoint(buf, 0, &value)) + if (kstrtoint(buf, 0, &value) || !value) return -EINVAL; - if (value) - chip->taos_settings.als_cal_target = value; + chip->taos_settings.als_cal_target = value; return len; } @@ -541,11 +540,10 @@ static ssize_t in_illuminance_calibrate_store(struct device *dev, struct iio_dev *indio_dev = dev_to_iio_dev(dev); int value; - if (kstrtoint(buf, 0, &value)) + if (kstrtoint(buf, 0, &value) || value != 1) return -EINVAL; - if (value == 1) - taos_als_calibrate(indio_dev); + taos_als_calibrate(indio_dev); return len; } From 1b7e9e2cbd6478e0ebe40639ea36fefd3ffcf916 Mon Sep 17 00:00:00 2001 From: Brian Masney Date: Fri, 28 Oct 2016 06:00:21 -0400 Subject: [PATCH 23/40] staging: iio: tsl2583: add locking to sysfs attributes in_illuminance_input_target_show(), in_illuminance_input_target_store(), in_illuminance_calibrate_store(), and in_illuminance_lux_table_store() accesses data from the tsl2583_chip struct. Some of these fields can be modified by other parts of the driver concurrently. This patch adds the mutex locking to these sysfs attributes. Signed-off-by: Brian Masney Signed-off-by: Jonathan Cameron --- drivers/staging/iio/light/tsl2583.c | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c index 98afa5bdfb29..49b19f5c1d84 100644 --- a/drivers/staging/iio/light/tsl2583.c +++ b/drivers/staging/iio/light/tsl2583.c @@ -513,8 +513,13 @@ static ssize_t in_illuminance_input_target_show(struct device *dev, { struct iio_dev *indio_dev = dev_to_iio_dev(dev); struct tsl2583_chip *chip = iio_priv(indio_dev); + int ret; - return sprintf(buf, "%d\n", chip->taos_settings.als_cal_target); + mutex_lock(&chip->als_mutex); + ret = sprintf(buf, "%d\n", chip->taos_settings.als_cal_target); + mutex_unlock(&chip->als_mutex); + + return ret; } static ssize_t in_illuminance_input_target_store(struct device *dev, @@ -528,7 +533,9 @@ static ssize_t in_illuminance_input_target_store(struct device *dev, if (kstrtoint(buf, 0, &value) || !value) return -EINVAL; + mutex_lock(&chip->als_mutex); chip->taos_settings.als_cal_target = value; + mutex_unlock(&chip->als_mutex); return len; } @@ -538,12 +545,15 @@ static ssize_t in_illuminance_calibrate_store(struct device *dev, const char *buf, size_t len) { struct iio_dev *indio_dev = dev_to_iio_dev(dev); + struct tsl2583_chip *chip = iio_priv(indio_dev); int value; if (kstrtoint(buf, 0, &value) || value != 1) return -EINVAL; + mutex_lock(&chip->als_mutex); taos_als_calibrate(indio_dev); + mutex_unlock(&chip->als_mutex); return len; } @@ -583,6 +593,8 @@ static ssize_t in_illuminance_lux_table_store(struct device *dev, int value[ARRAY_SIZE(taos_device_lux) * 3 + 1]; int n, ret = -EINVAL; + mutex_lock(&chip->als_mutex); + get_options(buf, ARRAY_SIZE(value), value); /* We now have an array of ints starting at value[1], and @@ -617,6 +629,8 @@ static ssize_t in_illuminance_lux_table_store(struct device *dev, ret = len; done: + mutex_unlock(&chip->als_mutex); + return ret; } From 579d542534c96c9109b63e79a2c6d22e2bca21cc Mon Sep 17 00:00:00 2001 From: Eva Rachel Retuya Date: Fri, 28 Oct 2016 16:26:52 +0800 Subject: [PATCH 24/40] staging: iio: cdc: ad7746: add additional config defines Introduce defines for shifting and mask under the config register for better readability. Also, introduce helper variables for index calculation. Signed-off-by: Eva Rachel Retuya Acked-by: Lars-Peter Clausen Signed-off-by: Jonathan Cameron --- drivers/staging/iio/cdc/ad7746.c | 38 ++++++++++++++++++-------------- 1 file changed, 22 insertions(+), 16 deletions(-) diff --git a/drivers/staging/iio/cdc/ad7746.c b/drivers/staging/iio/cdc/ad7746.c index a5828f9aa437..81f8b9ee1120 100644 --- a/drivers/staging/iio/cdc/ad7746.c +++ b/drivers/staging/iio/cdc/ad7746.c @@ -70,8 +70,10 @@ #define AD7746_EXCSETUP_EXCLVL(x) (((x) & 0x3) << 0) /* Config Register Bit Designations (AD7746_REG_CFG) */ -#define AD7746_CONF_VTFS(x) ((x) << 6) -#define AD7746_CONF_CAPFS(x) ((x) << 3) +#define AD7746_CONF_VTFS_SHIFT 6 +#define AD7746_CONF_CAPFS_SHIFT 3 +#define AD7746_CONF_VTFS_MASK GENMASK(7, 6) +#define AD7746_CONF_CAPFS_MASK GENMASK(5, 3) #define AD7746_CONF_MODE_IDLE (0 << 0) #define AD7746_CONF_MODE_CONT_CONV (1 << 0) #define AD7746_CONF_MODE_SINGLE_CONV (2 << 0) @@ -217,15 +219,16 @@ static int ad7746_select_channel(struct iio_dev *indio_dev, struct iio_chan_spec const *chan) { struct ad7746_chip_info *chip = iio_priv(indio_dev); - int ret, delay; + int ret, delay, idx; u8 vt_setup, cap_setup; switch (chan->type) { case IIO_CAPACITANCE: cap_setup = (chan->address & 0xFF) | AD7746_CAPSETUP_CAPEN; vt_setup = chip->vt_setup & ~AD7746_VTSETUP_VTEN; - delay = ad7746_cap_filter_rate_table[(chip->config >> 3) & - 0x7][1]; + idx = (chip->config & AD7746_CONF_CAPFS_MASK) >> + AD7746_CONF_CAPFS_SHIFT; + delay = ad7746_cap_filter_rate_table[idx][1]; if (chip->capdac_set != chan->channel) { ret = i2c_smbus_write_byte_data(chip->client, @@ -246,8 +249,9 @@ static int ad7746_select_channel(struct iio_dev *indio_dev, case IIO_TEMP: vt_setup = (chan->address & 0xFF) | AD7746_VTSETUP_VTEN; cap_setup = chip->cap_setup & ~AD7746_CAPSETUP_CAPEN; - delay = ad7746_cap_filter_rate_table[(chip->config >> 6) & - 0x3][1]; + idx = (chip->config & AD7746_CONF_VTFS_MASK) >> + AD7746_CONF_VTFS_SHIFT; + delay = ad7746_cap_filter_rate_table[idx][1]; break; default: return -EINVAL; @@ -369,8 +373,8 @@ static int ad7746_store_cap_filter_rate_setup(struct ad7746_chip_info *chip, if (i >= ARRAY_SIZE(ad7746_cap_filter_rate_table)) i = ARRAY_SIZE(ad7746_cap_filter_rate_table) - 1; - chip->config &= ~AD7746_CONF_CAPFS(0x7); - chip->config |= AD7746_CONF_CAPFS(i); + chip->config &= ~AD7746_CONF_CAPFS_MASK; + chip->config |= i << AD7746_CONF_CAPFS_SHIFT; return 0; } @@ -387,8 +391,8 @@ static int ad7746_store_vt_filter_rate_setup(struct ad7746_chip_info *chip, if (i >= ARRAY_SIZE(ad7746_vt_filter_rate_table)) i = ARRAY_SIZE(ad7746_vt_filter_rate_table) - 1; - chip->config &= ~AD7746_CONF_VTFS(0x3); - chip->config |= AD7746_CONF_VTFS(i); + chip->config &= ~AD7746_CONF_VTFS_MASK; + chip->config |= i << AD7746_CONF_VTFS_SHIFT; return 0; } @@ -527,7 +531,7 @@ static int ad7746_read_raw(struct iio_dev *indio_dev, long mask) { struct ad7746_chip_info *chip = iio_priv(indio_dev); - int ret, delay; + int ret, delay, idx; u8 regval, reg; mutex_lock(&indio_dev->mlock); @@ -635,13 +639,15 @@ static int ad7746_read_raw(struct iio_dev *indio_dev, case IIO_CHAN_INFO_SAMP_FREQ: switch (chan->type) { case IIO_CAPACITANCE: - *val = ad7746_cap_filter_rate_table[ - (chip->config >> 3) & 0x7][0]; + idx = (chip->config & AD7746_CONF_CAPFS_MASK) >> + AD7746_CONF_CAPFS_SHIFT; + *val = ad7746_cap_filter_rate_table[idx][0]; ret = IIO_VAL_INT; break; case IIO_VOLTAGE: - *val = ad7746_vt_filter_rate_table[ - (chip->config >> 6) & 0x3][0]; + idx = (chip->config & AD7746_CONF_VTFS_MASK) >> + AD7746_CONF_VTFS_SHIFT; + *val = ad7746_vt_filter_rate_table[idx][0]; ret = IIO_VAL_INT; break; default: From c834e1718aae6150936af718db9e483993d2c63f Mon Sep 17 00:00:00 2001 From: Eva Rachel Retuya Date: Tue, 1 Nov 2016 01:04:30 +0800 Subject: [PATCH 25/40] staging: iio: set proper supply name to devm_regulator_get() The name passed to devm_regulator_get() should match the name of the supply as specified in the device datasheet. This makes it clear what power supply is being referred to in case of presence of other regulators. Currently, the supply name specified on the affected devices is 'vcc'. Use lowercase version of the datasheet name to specify the supply voltage. Suggested-by: Lars-Peter Clausen Signed-off-by: Eva Rachel Retuya Signed-off-by: Jonathan Cameron --- drivers/staging/iio/adc/ad7192.c | 2 +- drivers/staging/iio/adc/ad7780.c | 2 +- drivers/staging/iio/frequency/ad9832.c | 2 +- drivers/staging/iio/frequency/ad9834.c | 2 +- drivers/staging/iio/impedance-analyzer/ad5933.c | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/drivers/staging/iio/adc/ad7192.c b/drivers/staging/iio/adc/ad7192.c index bfa12ceb1e1f..41fb32de5992 100644 --- a/drivers/staging/iio/adc/ad7192.c +++ b/drivers/staging/iio/adc/ad7192.c @@ -633,7 +633,7 @@ static int ad7192_probe(struct spi_device *spi) st = iio_priv(indio_dev); - st->reg = devm_regulator_get(&spi->dev, "vcc"); + st->reg = devm_regulator_get(&spi->dev, "avdd"); if (!IS_ERR(st->reg)) { ret = regulator_enable(st->reg); if (ret) diff --git a/drivers/staging/iio/adc/ad7780.c b/drivers/staging/iio/adc/ad7780.c index c9a0c2aa602f..a88236e74cdb 100644 --- a/drivers/staging/iio/adc/ad7780.c +++ b/drivers/staging/iio/adc/ad7780.c @@ -173,7 +173,7 @@ static int ad7780_probe(struct spi_device *spi) ad_sd_init(&st->sd, indio_dev, spi, &ad7780_sigma_delta_info); - st->reg = devm_regulator_get(&spi->dev, "vcc"); + st->reg = devm_regulator_get(&spi->dev, "avdd"); if (!IS_ERR(st->reg)) { ret = regulator_enable(st->reg); if (ret) diff --git a/drivers/staging/iio/frequency/ad9832.c b/drivers/staging/iio/frequency/ad9832.c index 358400b22d33..744c8ee9c5a6 100644 --- a/drivers/staging/iio/frequency/ad9832.c +++ b/drivers/staging/iio/frequency/ad9832.c @@ -212,7 +212,7 @@ static int ad9832_probe(struct spi_device *spi) return -ENODEV; } - reg = devm_regulator_get(&spi->dev, "vcc"); + reg = devm_regulator_get(&spi->dev, "avdd"); if (!IS_ERR(reg)) { ret = regulator_enable(reg); if (ret) diff --git a/drivers/staging/iio/frequency/ad9834.c b/drivers/staging/iio/frequency/ad9834.c index 6366216e4f37..ca3cea68d8d7 100644 --- a/drivers/staging/iio/frequency/ad9834.c +++ b/drivers/staging/iio/frequency/ad9834.c @@ -329,7 +329,7 @@ static int ad9834_probe(struct spi_device *spi) return -ENODEV; } - reg = devm_regulator_get(&spi->dev, "vcc"); + reg = devm_regulator_get(&spi->dev, "avdd"); if (!IS_ERR(reg)) { ret = regulator_enable(reg); if (ret) diff --git a/drivers/staging/iio/impedance-analyzer/ad5933.c b/drivers/staging/iio/impedance-analyzer/ad5933.c index 5eecf1cb1028..62f61bcc0003 100644 --- a/drivers/staging/iio/impedance-analyzer/ad5933.c +++ b/drivers/staging/iio/impedance-analyzer/ad5933.c @@ -723,7 +723,7 @@ static int ad5933_probe(struct i2c_client *client, if (!pdata) pdata = &ad5933_default_pdata; - st->reg = devm_regulator_get(&client->dev, "vcc"); + st->reg = devm_regulator_get(&client->dev, "vdd"); if (!IS_ERR(st->reg)) { ret = regulator_enable(st->reg); if (ret) From 3925ff0fcba71a27be14b19b01c0b67efbab0be9 Mon Sep 17 00:00:00 2001 From: Eva Rachel Retuya Date: Tue, 1 Nov 2016 01:04:31 +0800 Subject: [PATCH 26/40] staging: iio: rework regulator handling Currently, the affected drivers ignore all errors from regulator_get(). The way it is now, it also breaks probe deferral (EPROBE_DEFER). The correct behavior is to propagate the error to the upper layers so they can handle it accordingly. Rework the regulator handling so that it matches the standard behavior. If the specific design uses a static always-on regulator and does not explicitly specify it, regulator_get() will return the dummy regulator. The following semantic patch was used to apply the change: @r1@ expression reg, dev, en, volt; @@ reg = \(devm_regulator_get\|regulator_get\)(dev, ...); if ( - ! IS_ERR(reg)) + return PTR_ERR(reg); ( - { en = regulator_enable(reg); - if (en) return en; } + + en = regulator_enable(reg); + if (en) { + dev_err(dev, "Failed to enable specified supply\n"); + return en; } | + - { en = regulator_enable(reg); - if (en) return en; - volt = regulator_get_voltage(reg); } + en = regulator_enable(reg); + if (en) { + dev_err(dev, "Failed to enable specified supply\n"); + return en; + } + volt = regulator_get_voltage(reg); ) @r2@ expression arg; @@ - if (!IS_ERR(arg)) regulator_disable(arg); + regulator_disable(arg); Hand-edit the debugging prints with the supply name to become more specific. Suggested-by: Lars-Peter Clausen Signed-off-by: Eva Rachel Retuya Signed-off-by: Jonathan Cameron --- drivers/staging/iio/adc/ad7192.c | 18 +++++++++--------- drivers/staging/iio/adc/ad7780.c | 18 +++++++++--------- drivers/staging/iio/frequency/ad9832.c | 17 +++++++++-------- drivers/staging/iio/frequency/ad9834.c | 17 +++++++++-------- .../staging/iio/impedance-analyzer/ad5933.c | 19 ++++++++++--------- 5 files changed, 46 insertions(+), 43 deletions(-) diff --git a/drivers/staging/iio/adc/ad7192.c b/drivers/staging/iio/adc/ad7192.c index 41fb32de5992..29e32b71f0f7 100644 --- a/drivers/staging/iio/adc/ad7192.c +++ b/drivers/staging/iio/adc/ad7192.c @@ -634,13 +634,15 @@ static int ad7192_probe(struct spi_device *spi) st = iio_priv(indio_dev); st->reg = devm_regulator_get(&spi->dev, "avdd"); - if (!IS_ERR(st->reg)) { - ret = regulator_enable(st->reg); - if (ret) - return ret; + if (IS_ERR(st->reg)) + return PTR_ERR(st->reg); - voltage_uv = regulator_get_voltage(st->reg); + ret = regulator_enable(st->reg); + if (ret) { + dev_err(&spi->dev, "Failed to enable specified AVdd supply\n"); + return ret; } + voltage_uv = regulator_get_voltage(st->reg); if (pdata->vref_mv) st->int_vref_mv = pdata->vref_mv; @@ -689,8 +691,7 @@ static int ad7192_probe(struct spi_device *spi) error_remove_trigger: ad_sd_cleanup_buffer_and_trigger(indio_dev); error_disable_reg: - if (!IS_ERR(st->reg)) - regulator_disable(st->reg); + regulator_disable(st->reg); return ret; } @@ -703,8 +704,7 @@ static int ad7192_remove(struct spi_device *spi) iio_device_unregister(indio_dev); ad_sd_cleanup_buffer_and_trigger(indio_dev); - if (!IS_ERR(st->reg)) - regulator_disable(st->reg); + regulator_disable(st->reg); return 0; } diff --git a/drivers/staging/iio/adc/ad7780.c b/drivers/staging/iio/adc/ad7780.c index a88236e74cdb..e14960038d3e 100644 --- a/drivers/staging/iio/adc/ad7780.c +++ b/drivers/staging/iio/adc/ad7780.c @@ -174,13 +174,15 @@ static int ad7780_probe(struct spi_device *spi) ad_sd_init(&st->sd, indio_dev, spi, &ad7780_sigma_delta_info); st->reg = devm_regulator_get(&spi->dev, "avdd"); - if (!IS_ERR(st->reg)) { - ret = regulator_enable(st->reg); - if (ret) - return ret; + if (IS_ERR(st->reg)) + return PTR_ERR(st->reg); - voltage_uv = regulator_get_voltage(st->reg); + ret = regulator_enable(st->reg); + if (ret) { + dev_err(&spi->dev, "Failed to enable specified AVdd supply\n"); + return ret; } + voltage_uv = regulator_get_voltage(st->reg); st->chip_info = &ad7780_chip_info_tbl[spi_get_device_id(spi)->driver_data]; @@ -222,8 +224,7 @@ static int ad7780_probe(struct spi_device *spi) error_cleanup_buffer_and_trigger: ad_sd_cleanup_buffer_and_trigger(indio_dev); error_disable_reg: - if (!IS_ERR(st->reg)) - regulator_disable(st->reg); + regulator_disable(st->reg); return ret; } @@ -236,8 +237,7 @@ static int ad7780_remove(struct spi_device *spi) iio_device_unregister(indio_dev); ad_sd_cleanup_buffer_and_trigger(indio_dev); - if (!IS_ERR(st->reg)) - regulator_disable(st->reg); + regulator_disable(st->reg); return 0; } diff --git a/drivers/staging/iio/frequency/ad9832.c b/drivers/staging/iio/frequency/ad9832.c index 744c8ee9c5a6..7d8dc6cab720 100644 --- a/drivers/staging/iio/frequency/ad9832.c +++ b/drivers/staging/iio/frequency/ad9832.c @@ -213,10 +213,13 @@ static int ad9832_probe(struct spi_device *spi) } reg = devm_regulator_get(&spi->dev, "avdd"); - if (!IS_ERR(reg)) { - ret = regulator_enable(reg); - if (ret) - return ret; + if (IS_ERR(reg)) + return PTR_ERR(reg); + + ret = regulator_enable(reg); + if (ret) { + dev_err(&spi->dev, "Failed to enable specified AVDD supply\n"); + return ret; } indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st)); @@ -311,8 +314,7 @@ static int ad9832_probe(struct spi_device *spi) return 0; error_disable_reg: - if (!IS_ERR(reg)) - regulator_disable(reg); + regulator_disable(reg); return ret; } @@ -323,8 +325,7 @@ static int ad9832_remove(struct spi_device *spi) struct ad9832_state *st = iio_priv(indio_dev); iio_device_unregister(indio_dev); - if (!IS_ERR(st->reg)) - regulator_disable(st->reg); + regulator_disable(st->reg); return 0; } diff --git a/drivers/staging/iio/frequency/ad9834.c b/drivers/staging/iio/frequency/ad9834.c index ca3cea68d8d7..19216af1dfc9 100644 --- a/drivers/staging/iio/frequency/ad9834.c +++ b/drivers/staging/iio/frequency/ad9834.c @@ -330,10 +330,13 @@ static int ad9834_probe(struct spi_device *spi) } reg = devm_regulator_get(&spi->dev, "avdd"); - if (!IS_ERR(reg)) { - ret = regulator_enable(reg); - if (ret) - return ret; + if (IS_ERR(reg)) + return PTR_ERR(reg); + + ret = regulator_enable(reg); + if (ret) { + dev_err(&spi->dev, "Failed to enable specified AVDD supply\n"); + return ret; } indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st)); @@ -416,8 +419,7 @@ static int ad9834_probe(struct spi_device *spi) return 0; error_disable_reg: - if (!IS_ERR(reg)) - regulator_disable(reg); + regulator_disable(reg); return ret; } @@ -428,8 +430,7 @@ static int ad9834_remove(struct spi_device *spi) struct ad9834_state *st = iio_priv(indio_dev); iio_device_unregister(indio_dev); - if (!IS_ERR(st->reg)) - regulator_disable(st->reg); + regulator_disable(st->reg); return 0; } diff --git a/drivers/staging/iio/impedance-analyzer/ad5933.c b/drivers/staging/iio/impedance-analyzer/ad5933.c index 62f61bcc0003..8f2736a0f854 100644 --- a/drivers/staging/iio/impedance-analyzer/ad5933.c +++ b/drivers/staging/iio/impedance-analyzer/ad5933.c @@ -724,12 +724,15 @@ static int ad5933_probe(struct i2c_client *client, pdata = &ad5933_default_pdata; st->reg = devm_regulator_get(&client->dev, "vdd"); - if (!IS_ERR(st->reg)) { - ret = regulator_enable(st->reg); - if (ret) - return ret; - voltage_uv = regulator_get_voltage(st->reg); + if (IS_ERR(st->reg)) + return PTR_ERR(st->reg); + + ret = regulator_enable(st->reg); + if (ret) { + dev_err(&client->dev, "Failed to enable specified VDD supply\n"); + return ret; } + voltage_uv = regulator_get_voltage(st->reg); if (voltage_uv) st->vref_mv = voltage_uv / 1000; @@ -772,8 +775,7 @@ static int ad5933_probe(struct i2c_client *client, error_unreg_ring: iio_kfifo_free(indio_dev->buffer); error_disable_reg: - if (!IS_ERR(st->reg)) - regulator_disable(st->reg); + regulator_disable(st->reg); return ret; } @@ -785,8 +787,7 @@ static int ad5933_remove(struct i2c_client *client) iio_device_unregister(indio_dev); iio_kfifo_free(indio_dev->buffer); - if (!IS_ERR(st->reg)) - regulator_disable(st->reg); + regulator_disable(st->reg); return 0; } From cb8bb16caf42fd4f7eba69ee1889260af6e43ca1 Mon Sep 17 00:00:00 2001 From: Eva Rachel Retuya Date: Tue, 1 Nov 2016 01:04:32 +0800 Subject: [PATCH 27/40] staging: iio: ad7192: add DVdd regulator The AD7190/AD7192/AD7193/AD7195 is supplied with two power sources: AVdd as analog supply voltage and DVdd as digital supply voltage. Attempt to fetch and enable the regulator 'dvdd'. Bail out if any error occurs. Suggested-by: Lars-Peter Clausen Signed-off-by: Eva Rachel Retuya Signed-off-by: Jonathan Cameron --- drivers/staging/iio/adc/ad7192.c | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/drivers/staging/iio/adc/ad7192.c b/drivers/staging/iio/adc/ad7192.c index 29e32b71f0f7..3f8dc6608fe7 100644 --- a/drivers/staging/iio/adc/ad7192.c +++ b/drivers/staging/iio/adc/ad7192.c @@ -153,6 +153,7 @@ struct ad7192_state { struct regulator *reg; + struct regulator *dvdd; u16 int_vref_mv; u32 mclk; u32 f_order; @@ -642,6 +643,19 @@ static int ad7192_probe(struct spi_device *spi) dev_err(&spi->dev, "Failed to enable specified AVdd supply\n"); return ret; } + + st->dvdd = devm_regulator_get(&spi->dev, "dvdd"); + if (IS_ERR(st->dvdd)) { + ret = PTR_ERR(st->dvdd); + goto error_disable_reg; + } + + ret = regulator_enable(st->dvdd); + if (ret) { + dev_err(&spi->dev, "Failed to enable specified DVdd supply\n"); + goto error_disable_reg; + } + voltage_uv = regulator_get_voltage(st->reg); if (pdata->vref_mv) @@ -677,7 +691,7 @@ static int ad7192_probe(struct spi_device *spi) ret = ad_sd_setup_buffer_and_trigger(indio_dev); if (ret) - goto error_disable_reg; + goto error_disable_dvdd; ret = ad7192_setup(st, pdata); if (ret) @@ -690,6 +704,8 @@ static int ad7192_probe(struct spi_device *spi) error_remove_trigger: ad_sd_cleanup_buffer_and_trigger(indio_dev); +error_disable_dvdd: + regulator_disable(st->dvdd); error_disable_reg: regulator_disable(st->reg); @@ -704,6 +720,7 @@ static int ad7192_remove(struct spi_device *spi) iio_device_unregister(indio_dev); ad_sd_cleanup_buffer_and_trigger(indio_dev); + regulator_disable(st->dvdd); regulator_disable(st->reg); return 0; From 9d1548143faf00142937e37c359047a52a2d0d1d Mon Sep 17 00:00:00 2001 From: Eva Rachel Retuya Date: Tue, 1 Nov 2016 01:04:33 +0800 Subject: [PATCH 28/40] staging: iio: ad7192: rename regulator 'reg' to 'avdd' Rename regulator 'reg' to 'avdd' so as to be clear what regulator it stands for specifically. Also, update the goto label accordingly. Signed-off-by: Eva Rachel Retuya Signed-off-by: Jonathan Cameron --- drivers/staging/iio/adc/ad7192.c | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/drivers/staging/iio/adc/ad7192.c b/drivers/staging/iio/adc/ad7192.c index 3f8dc6608fe7..1fb68c01abd5 100644 --- a/drivers/staging/iio/adc/ad7192.c +++ b/drivers/staging/iio/adc/ad7192.c @@ -152,7 +152,7 @@ */ struct ad7192_state { - struct regulator *reg; + struct regulator *avdd; struct regulator *dvdd; u16 int_vref_mv; u32 mclk; @@ -634,11 +634,11 @@ static int ad7192_probe(struct spi_device *spi) st = iio_priv(indio_dev); - st->reg = devm_regulator_get(&spi->dev, "avdd"); - if (IS_ERR(st->reg)) - return PTR_ERR(st->reg); + st->avdd = devm_regulator_get(&spi->dev, "avdd"); + if (IS_ERR(st->avdd)) + return PTR_ERR(st->avdd); - ret = regulator_enable(st->reg); + ret = regulator_enable(st->avdd); if (ret) { dev_err(&spi->dev, "Failed to enable specified AVdd supply\n"); return ret; @@ -647,16 +647,16 @@ static int ad7192_probe(struct spi_device *spi) st->dvdd = devm_regulator_get(&spi->dev, "dvdd"); if (IS_ERR(st->dvdd)) { ret = PTR_ERR(st->dvdd); - goto error_disable_reg; + goto error_disable_avdd; } ret = regulator_enable(st->dvdd); if (ret) { dev_err(&spi->dev, "Failed to enable specified DVdd supply\n"); - goto error_disable_reg; + goto error_disable_avdd; } - voltage_uv = regulator_get_voltage(st->reg); + voltage_uv = regulator_get_voltage(st->avdd); if (pdata->vref_mv) st->int_vref_mv = pdata->vref_mv; @@ -706,8 +706,8 @@ static int ad7192_probe(struct spi_device *spi) ad_sd_cleanup_buffer_and_trigger(indio_dev); error_disable_dvdd: regulator_disable(st->dvdd); -error_disable_reg: - regulator_disable(st->reg); +error_disable_avdd: + regulator_disable(st->avdd); return ret; } @@ -721,7 +721,7 @@ static int ad7192_remove(struct spi_device *spi) ad_sd_cleanup_buffer_and_trigger(indio_dev); regulator_disable(st->dvdd); - regulator_disable(st->reg); + regulator_disable(st->avdd); return 0; } From a98461d79ba5bd9c5a83be3f5860cba5d5b54617 Mon Sep 17 00:00:00 2001 From: Eva Rachel Retuya Date: Tue, 1 Nov 2016 01:04:34 +0800 Subject: [PATCH 29/40] staging: iio: ad9832: add DVDD regulator The AD9832/AD9835 is supplied with two power sources: AVDD as analog supply voltage and DVDD as digital supply voltage. Attempt to fetch and enable the regulator 'dvdd'. Bail out if any error occurs. Suggested-by: Lars-Peter Clausen Signed-off-by: Eva Rachel Retuya Signed-off-by: Jonathan Cameron --- drivers/staging/iio/frequency/ad9832.c | 33 +++++++++++++++++++------- drivers/staging/iio/frequency/ad9832.h | 2 ++ 2 files changed, 26 insertions(+), 9 deletions(-) diff --git a/drivers/staging/iio/frequency/ad9832.c b/drivers/staging/iio/frequency/ad9832.c index 7d8dc6cab720..6a5ab02e64ef 100644 --- a/drivers/staging/iio/frequency/ad9832.c +++ b/drivers/staging/iio/frequency/ad9832.c @@ -222,10 +222,22 @@ static int ad9832_probe(struct spi_device *spi) return ret; } + st->dvdd = devm_regulator_get(&spi->dev, "dvdd"); + if (IS_ERR(st->dvdd)) { + ret = PTR_ERR(st->dvdd); + goto error_disable_reg; + } + + ret = regulator_enable(st->dvdd); + if (ret) { + dev_err(&spi->dev, "Failed to enable specified DVDD supply\n"); + goto error_disable_reg; + } + indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st)); if (!indio_dev) { ret = -ENOMEM; - goto error_disable_reg; + goto error_disable_dvdd; } spi_set_drvdata(spi, indio_dev); st = iio_priv(indio_dev); @@ -280,39 +292,41 @@ static int ad9832_probe(struct spi_device *spi) ret = spi_sync(st->spi, &st->msg); if (ret) { dev_err(&spi->dev, "device init failed\n"); - goto error_disable_reg; + goto error_disable_dvdd; } ret = ad9832_write_frequency(st, AD9832_FREQ0HM, pdata->freq0); if (ret) - goto error_disable_reg; + goto error_disable_dvdd; ret = ad9832_write_frequency(st, AD9832_FREQ1HM, pdata->freq1); if (ret) - goto error_disable_reg; + goto error_disable_dvdd; ret = ad9832_write_phase(st, AD9832_PHASE0H, pdata->phase0); if (ret) - goto error_disable_reg; + goto error_disable_dvdd; ret = ad9832_write_phase(st, AD9832_PHASE1H, pdata->phase1); if (ret) - goto error_disable_reg; + goto error_disable_dvdd; ret = ad9832_write_phase(st, AD9832_PHASE2H, pdata->phase2); if (ret) - goto error_disable_reg; + goto error_disable_dvdd; ret = ad9832_write_phase(st, AD9832_PHASE3H, pdata->phase3); if (ret) - goto error_disable_reg; + goto error_disable_dvdd; ret = iio_device_register(indio_dev); if (ret) - goto error_disable_reg; + goto error_disable_dvdd; return 0; +error_disable_dvdd: + regulator_disable(st->dvdd); error_disable_reg: regulator_disable(reg); @@ -325,6 +339,7 @@ static int ad9832_remove(struct spi_device *spi) struct ad9832_state *st = iio_priv(indio_dev); iio_device_unregister(indio_dev); + regulator_disable(st->dvdd); regulator_disable(st->reg); return 0; diff --git a/drivers/staging/iio/frequency/ad9832.h b/drivers/staging/iio/frequency/ad9832.h index d32323b46be6..eb0e7f293169 100644 --- a/drivers/staging/iio/frequency/ad9832.h +++ b/drivers/staging/iio/frequency/ad9832.h @@ -59,6 +59,7 @@ * struct ad9832_state - driver instance specific data * @spi: spi_device * @reg: supply regulator + * @dvdd: supply regulator for the digital section * @mclk: external master clock * @ctrl_fp: cached frequency/phase control word * @ctrl_ss: cached sync/selsrc control word @@ -77,6 +78,7 @@ struct ad9832_state { struct spi_device *spi; struct regulator *reg; + struct regulator *dvdd; unsigned long mclk; unsigned short ctrl_fp; unsigned short ctrl_ss; From 43a07e48af44c153f960e4a204771d5911e10134 Mon Sep 17 00:00:00 2001 From: Eva Rachel Retuya Date: Tue, 1 Nov 2016 01:04:35 +0800 Subject: [PATCH 30/40] staging: iio: ad9832: clean-up regulator 'reg' Rename regulator 'reg' to 'avdd' so as to be clear what regulator it stands for specifically. Additionally, get rid of local variable 'reg' and use direct assignment instead. Update also the goto label pertaining to the avdd regulator during disable. Signed-off-by: Eva Rachel Retuya Signed-off-by: Jonathan Cameron --- drivers/staging/iio/frequency/ad9832.c | 20 +++++++++----------- drivers/staging/iio/frequency/ad9832.h | 4 ++-- 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/drivers/staging/iio/frequency/ad9832.c b/drivers/staging/iio/frequency/ad9832.c index 6a5ab02e64ef..639047fade30 100644 --- a/drivers/staging/iio/frequency/ad9832.c +++ b/drivers/staging/iio/frequency/ad9832.c @@ -204,7 +204,6 @@ static int ad9832_probe(struct spi_device *spi) struct ad9832_platform_data *pdata = dev_get_platdata(&spi->dev); struct iio_dev *indio_dev; struct ad9832_state *st; - struct regulator *reg; int ret; if (!pdata) { @@ -212,11 +211,11 @@ static int ad9832_probe(struct spi_device *spi) return -ENODEV; } - reg = devm_regulator_get(&spi->dev, "avdd"); - if (IS_ERR(reg)) - return PTR_ERR(reg); + st->avdd = devm_regulator_get(&spi->dev, "avdd"); + if (IS_ERR(st->avdd)) + return PTR_ERR(st->avdd); - ret = regulator_enable(reg); + ret = regulator_enable(st->avdd); if (ret) { dev_err(&spi->dev, "Failed to enable specified AVDD supply\n"); return ret; @@ -225,13 +224,13 @@ static int ad9832_probe(struct spi_device *spi) st->dvdd = devm_regulator_get(&spi->dev, "dvdd"); if (IS_ERR(st->dvdd)) { ret = PTR_ERR(st->dvdd); - goto error_disable_reg; + goto error_disable_avdd; } ret = regulator_enable(st->dvdd); if (ret) { dev_err(&spi->dev, "Failed to enable specified DVDD supply\n"); - goto error_disable_reg; + goto error_disable_avdd; } indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st)); @@ -241,7 +240,6 @@ static int ad9832_probe(struct spi_device *spi) } spi_set_drvdata(spi, indio_dev); st = iio_priv(indio_dev); - st->reg = reg; st->mclk = pdata->mclk; st->spi = spi; @@ -327,8 +325,8 @@ static int ad9832_probe(struct spi_device *spi) error_disable_dvdd: regulator_disable(st->dvdd); -error_disable_reg: - regulator_disable(reg); +error_disable_avdd: + regulator_disable(st->avdd); return ret; } @@ -340,7 +338,7 @@ static int ad9832_remove(struct spi_device *spi) iio_device_unregister(indio_dev); regulator_disable(st->dvdd); - regulator_disable(st->reg); + regulator_disable(st->avdd); return 0; } diff --git a/drivers/staging/iio/frequency/ad9832.h b/drivers/staging/iio/frequency/ad9832.h index eb0e7f293169..1b08b04482a4 100644 --- a/drivers/staging/iio/frequency/ad9832.h +++ b/drivers/staging/iio/frequency/ad9832.h @@ -58,7 +58,7 @@ /** * struct ad9832_state - driver instance specific data * @spi: spi_device - * @reg: supply regulator + * @avdd: supply regulator for the analog section * @dvdd: supply regulator for the digital section * @mclk: external master clock * @ctrl_fp: cached frequency/phase control word @@ -77,7 +77,7 @@ struct ad9832_state { struct spi_device *spi; - struct regulator *reg; + struct regulator *avdd; struct regulator *dvdd; unsigned long mclk; unsigned short ctrl_fp; From ab54163190f236e30772e6e8b4efc56a319ed0a6 Mon Sep 17 00:00:00 2001 From: Song Hongyan Date: Thu, 3 Nov 2016 00:45:48 +0000 Subject: [PATCH 31/40] iio: hid-sensor-attributes: Check sample_frequency/hysteresis write data legitimacy Neither sample frequency value nor hysteresis value can be set to be a negative number, check and return "Invalid argument" if they are negative. If not do this change, sample_frequency will be set into some unknown value, read hysteresis value after write negative number will return "Invalid argument". Signed-off-by: Song Hongyan Acked-by: Srinivas Pandruvada Signed-off-by: Jonathan Cameron --- drivers/iio/common/hid-sensors/hid-sensor-attributes.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/drivers/iio/common/hid-sensors/hid-sensor-attributes.c b/drivers/iio/common/hid-sensors/hid-sensor-attributes.c index dc33c1dd5191..4509f8475c54 100644 --- a/drivers/iio/common/hid-sensors/hid-sensor-attributes.c +++ b/drivers/iio/common/hid-sensors/hid-sensor-attributes.c @@ -201,7 +201,7 @@ int hid_sensor_write_samp_freq_value(struct hid_sensor_common *st, int ret; if (val1 < 0 || val2 < 0) - ret = -EINVAL; + return -EINVAL; value = val1 * pow_10(6) + val2; if (value) { @@ -250,6 +250,9 @@ int hid_sensor_write_raw_hyst_value(struct hid_sensor_common *st, s32 value; int ret; + if (val1 < 0 || val2 < 0) + return -EINVAL; + value = convert_to_vtf_format(st->sensitivity.size, st->sensitivity.unit_expo, val1, val2); From bc3ae982e2fb498e2f5492497ea927a161380bc1 Mon Sep 17 00:00:00 2001 From: Wenyou Yang Date: Wed, 2 Nov 2016 17:21:48 +0800 Subject: [PATCH 32/40] iio: adc: at91: add suspend and resume callback Add suspend/resume callback, support the pinctrl sleep state when the system suspend as well. Signed-off-by: Wenyou Yang Acked-by: Ludovic Desroches Signed-off-by: Jonathan Cameron --- drivers/iio/adc/at91_adc.c | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/drivers/iio/adc/at91_adc.c b/drivers/iio/adc/at91_adc.c index bbdac07f4aaa..34b928cefeed 100644 --- a/drivers/iio/adc/at91_adc.c +++ b/drivers/iio/adc/at91_adc.c @@ -30,6 +30,7 @@ #include #include #include +#include /* Registers */ #define AT91_ADC_CR 0x00 /* Control Register */ @@ -1347,6 +1348,32 @@ static int at91_adc_remove(struct platform_device *pdev) return 0; } +#ifdef CONFIG_PM_SLEEP +static int at91_adc_suspend(struct device *dev) +{ + struct iio_dev *idev = platform_get_drvdata(to_platform_device(dev)); + struct at91_adc_state *st = iio_priv(idev); + + pinctrl_pm_select_sleep_state(dev); + clk_disable_unprepare(st->clk); + + return 0; +} + +static int at91_adc_resume(struct device *dev) +{ + struct iio_dev *idev = platform_get_drvdata(to_platform_device(dev)); + struct at91_adc_state *st = iio_priv(idev); + + clk_prepare_enable(st->clk); + pinctrl_pm_select_default_state(dev); + + return 0; +} +#endif + +static SIMPLE_DEV_PM_OPS(at91_adc_pm_ops, at91_adc_suspend, at91_adc_resume); + static struct at91_adc_caps at91sam9260_caps = { .calc_startup_ticks = calc_startup_ticks_9260, .num_channels = 4, @@ -1441,6 +1468,7 @@ static struct platform_driver at91_adc_driver = { .driver = { .name = DRIVER_NAME, .of_match_table = of_match_ptr(at91_adc_dt_ids), + .pm = &at91_adc_pm_ops, }, }; From c9329d8638cfa1a86faf4fb8bd4922a3d9c6c437 Mon Sep 17 00:00:00 2001 From: Mugunthan V N Date: Wed, 5 Oct 2016 14:34:40 +0530 Subject: [PATCH 33/40] mfd: ti_am335x_tscadc: store physical address store the physical address of the device in its priv to use it for DMA addressing in the client drivers. Signed-off-by: Mugunthan V N Acked-for-MFD-by: Lee Jones Signed-off-by: Jonathan Cameron --- drivers/mfd/ti_am335x_tscadc.c | 1 + include/linux/mfd/ti_am335x_tscadc.h | 1 + 2 files changed, 2 insertions(+) diff --git a/drivers/mfd/ti_am335x_tscadc.c b/drivers/mfd/ti_am335x_tscadc.c index c8f027b4ea4c..0f3fab47fe48 100644 --- a/drivers/mfd/ti_am335x_tscadc.c +++ b/drivers/mfd/ti_am335x_tscadc.c @@ -183,6 +183,7 @@ static int ti_tscadc_probe(struct platform_device *pdev) tscadc->irq = err; res = platform_get_resource(pdev, IORESOURCE_MEM, 0); + tscadc->tscadc_phys_base = res->start; tscadc->tscadc_base = devm_ioremap_resource(&pdev->dev, res); if (IS_ERR(tscadc->tscadc_base)) return PTR_ERR(tscadc->tscadc_base); diff --git a/include/linux/mfd/ti_am335x_tscadc.h b/include/linux/mfd/ti_am335x_tscadc.h index 7f55b8b41032..e45a208d9944 100644 --- a/include/linux/mfd/ti_am335x_tscadc.h +++ b/include/linux/mfd/ti_am335x_tscadc.h @@ -155,6 +155,7 @@ struct ti_tscadc_dev { struct device *dev; struct regmap *regmap; void __iomem *tscadc_base; + phys_addr_t tscadc_phys_base; int irq; int used_cells; /* 1-2 */ int tsc_wires; From f438b9da75eb80eb6c4095a5b75324cc9a7f0570 Mon Sep 17 00:00:00 2001 From: Mugunthan V N Date: Wed, 5 Oct 2016 14:34:41 +0530 Subject: [PATCH 34/40] drivers: iio: ti_am335x_adc: add dma support This patch adds the required pieces to ti_am335x_adc driver for DMA support Signed-off-by: Mugunthan V N Reviewed-by: Peter Ujfalusi Signed-off-by: Jonathan Cameron --- drivers/iio/adc/ti_am335x_adc.c | 148 ++++++++++++++++++++++++++- include/linux/mfd/ti_am335x_tscadc.h | 7 ++ 2 files changed, 152 insertions(+), 3 deletions(-) diff --git a/drivers/iio/adc/ti_am335x_adc.c b/drivers/iio/adc/ti_am335x_adc.c index c3cfacca2541..ad9dec30bb30 100644 --- a/drivers/iio/adc/ti_am335x_adc.c +++ b/drivers/iio/adc/ti_am335x_adc.c @@ -30,10 +30,28 @@ #include #include +#include +#include + +#define DMA_BUFFER_SIZE SZ_2K + +struct tiadc_dma { + struct dma_slave_config conf; + struct dma_chan *chan; + dma_addr_t addr; + dma_cookie_t cookie; + u8 *buf; + int current_period; + int period_size; + u8 fifo_thresh; +}; + struct tiadc_device { struct ti_tscadc_dev *mfd_tscadc; + struct tiadc_dma dma; struct mutex fifo1_lock; /* to protect fifo access */ int channels; + int total_ch_enabled; u8 channel_line[8]; u8 channel_step[8]; int buffer_en_ch_steps; @@ -198,6 +216,67 @@ static irqreturn_t tiadc_worker_h(int irq, void *private) return IRQ_HANDLED; } +static void tiadc_dma_rx_complete(void *param) +{ + struct iio_dev *indio_dev = param; + struct tiadc_device *adc_dev = iio_priv(indio_dev); + struct tiadc_dma *dma = &adc_dev->dma; + u8 *data; + int i; + + data = dma->buf + dma->current_period * dma->period_size; + dma->current_period = 1 - dma->current_period; /* swap the buffer ID */ + + for (i = 0; i < dma->period_size; i += indio_dev->scan_bytes) { + iio_push_to_buffers(indio_dev, data); + data += indio_dev->scan_bytes; + } +} + +static int tiadc_start_dma(struct iio_dev *indio_dev) +{ + struct tiadc_device *adc_dev = iio_priv(indio_dev); + struct tiadc_dma *dma = &adc_dev->dma; + struct dma_async_tx_descriptor *desc; + + dma->current_period = 0; /* We start to fill period 0 */ + /* + * Make the fifo thresh as the multiple of total number of + * channels enabled, so make sure that cyclic DMA period + * length is also a multiple of total number of channels + * enabled. This ensures that no invalid data is reported + * to the stack via iio_push_to_buffers(). + */ + dma->fifo_thresh = rounddown(FIFO1_THRESHOLD + 1, + adc_dev->total_ch_enabled) - 1; + /* Make sure that period length is multiple of fifo thresh level */ + dma->period_size = rounddown(DMA_BUFFER_SIZE / 2, + (dma->fifo_thresh + 1) * sizeof(u16)); + + dma->conf.src_maxburst = dma->fifo_thresh + 1; + dmaengine_slave_config(dma->chan, &dma->conf); + + desc = dmaengine_prep_dma_cyclic(dma->chan, dma->addr, + dma->period_size * 2, + dma->period_size, DMA_DEV_TO_MEM, + DMA_PREP_INTERRUPT); + if (!desc) + return -EBUSY; + + desc->callback = tiadc_dma_rx_complete; + desc->callback_param = indio_dev; + + dma->cookie = dmaengine_submit(desc); + + dma_async_issue_pending(dma->chan); + + tiadc_writel(adc_dev, REG_FIFO1THR, dma->fifo_thresh); + tiadc_writel(adc_dev, REG_DMA1REQ, dma->fifo_thresh); + tiadc_writel(adc_dev, REG_DMAENABLE_SET, DMA_FIFO1); + + return 0; +} + static int tiadc_buffer_preenable(struct iio_dev *indio_dev) { struct tiadc_device *adc_dev = iio_priv(indio_dev); @@ -218,20 +297,30 @@ static int tiadc_buffer_preenable(struct iio_dev *indio_dev) static int tiadc_buffer_postenable(struct iio_dev *indio_dev) { struct tiadc_device *adc_dev = iio_priv(indio_dev); + struct tiadc_dma *dma = &adc_dev->dma; + unsigned int irq_enable; unsigned int enb = 0; u8 bit; tiadc_step_config(indio_dev); - for_each_set_bit(bit, indio_dev->active_scan_mask, adc_dev->channels) + for_each_set_bit(bit, indio_dev->active_scan_mask, adc_dev->channels) { enb |= (get_adc_step_bit(adc_dev, bit) << 1); + adc_dev->total_ch_enabled++; + } adc_dev->buffer_en_ch_steps = enb; + if (dma->chan) + tiadc_start_dma(indio_dev); + am335x_tsc_se_set_cache(adc_dev->mfd_tscadc, enb); tiadc_writel(adc_dev, REG_IRQSTATUS, IRQENB_FIFO1THRES | IRQENB_FIFO1OVRRUN | IRQENB_FIFO1UNDRFLW); - tiadc_writel(adc_dev, REG_IRQENABLE, IRQENB_FIFO1THRES - | IRQENB_FIFO1OVRRUN); + + irq_enable = IRQENB_FIFO1OVRRUN; + if (!dma->chan) + irq_enable |= IRQENB_FIFO1THRES; + tiadc_writel(adc_dev, REG_IRQENABLE, irq_enable); return 0; } @@ -239,12 +328,18 @@ static int tiadc_buffer_postenable(struct iio_dev *indio_dev) static int tiadc_buffer_predisable(struct iio_dev *indio_dev) { struct tiadc_device *adc_dev = iio_priv(indio_dev); + struct tiadc_dma *dma = &adc_dev->dma; int fifo1count, i, read; tiadc_writel(adc_dev, REG_IRQCLR, (IRQENB_FIFO1THRES | IRQENB_FIFO1OVRRUN | IRQENB_FIFO1UNDRFLW)); am335x_tsc_se_clr(adc_dev->mfd_tscadc, adc_dev->buffer_en_ch_steps); adc_dev->buffer_en_ch_steps = 0; + adc_dev->total_ch_enabled = 0; + if (dma->chan) { + tiadc_writel(adc_dev, REG_DMAENABLE_CLEAR, 0x2); + dmaengine_terminate_async(dma->chan); + } /* Flush FIFO of leftover data in the time it takes to disable adc */ fifo1count = tiadc_readl(adc_dev, REG_FIFO1CNT); @@ -430,6 +525,41 @@ static const struct iio_info tiadc_info = { .driver_module = THIS_MODULE, }; +static int tiadc_request_dma(struct platform_device *pdev, + struct tiadc_device *adc_dev) +{ + struct tiadc_dma *dma = &adc_dev->dma; + dma_cap_mask_t mask; + + /* Default slave configuration parameters */ + dma->conf.direction = DMA_DEV_TO_MEM; + dma->conf.src_addr_width = DMA_SLAVE_BUSWIDTH_2_BYTES; + dma->conf.src_addr = adc_dev->mfd_tscadc->tscadc_phys_base + REG_FIFO1; + + dma_cap_zero(mask); + dma_cap_set(DMA_CYCLIC, mask); + + /* Get a channel for RX */ + dma->chan = dma_request_chan(adc_dev->mfd_tscadc->dev, "fifo1"); + if (IS_ERR(dma->chan)) { + int ret = PTR_ERR(dma->chan); + + dma->chan = NULL; + return ret; + } + + /* RX buffer */ + dma->buf = dma_alloc_coherent(dma->chan->device->dev, DMA_BUFFER_SIZE, + &dma->addr, GFP_KERNEL); + if (!dma->buf) + goto err; + + return 0; +err: + dma_release_channel(dma->chan); + return -ENOMEM; +} + static int tiadc_parse_dt(struct platform_device *pdev, struct tiadc_device *adc_dev) { @@ -512,8 +642,14 @@ static int tiadc_probe(struct platform_device *pdev) platform_set_drvdata(pdev, indio_dev); + err = tiadc_request_dma(pdev, adc_dev); + if (err && err == -EPROBE_DEFER) + goto err_dma; + return 0; +err_dma: + iio_device_unregister(indio_dev); err_buffer_unregister: tiadc_iio_buffered_hardware_remove(indio_dev); err_free_channels: @@ -525,8 +661,14 @@ static int tiadc_remove(struct platform_device *pdev) { struct iio_dev *indio_dev = platform_get_drvdata(pdev); struct tiadc_device *adc_dev = iio_priv(indio_dev); + struct tiadc_dma *dma = &adc_dev->dma; u32 step_en; + if (dma->chan) { + dma_free_coherent(dma->chan->device->dev, DMA_BUFFER_SIZE, + dma->buf, dma->addr); + dma_release_channel(dma->chan); + } iio_device_unregister(indio_dev); tiadc_iio_buffered_hardware_remove(indio_dev); tiadc_channels_remove(indio_dev); diff --git a/include/linux/mfd/ti_am335x_tscadc.h b/include/linux/mfd/ti_am335x_tscadc.h index e45a208d9944..b9a53e013bff 100644 --- a/include/linux/mfd/ti_am335x_tscadc.h +++ b/include/linux/mfd/ti_am335x_tscadc.h @@ -23,6 +23,8 @@ #define REG_IRQENABLE 0x02C #define REG_IRQCLR 0x030 #define REG_IRQWAKEUP 0x034 +#define REG_DMAENABLE_SET 0x038 +#define REG_DMAENABLE_CLEAR 0x03c #define REG_CTRL 0x040 #define REG_ADCFSM 0x044 #define REG_CLKDIV 0x04C @@ -36,6 +38,7 @@ #define REG_FIFO0THR 0xE8 #define REG_FIFO1CNT 0xF0 #define REG_FIFO1THR 0xF4 +#define REG_DMA1REQ 0xF8 #define REG_FIFO0 0x100 #define REG_FIFO1 0x200 @@ -126,6 +129,10 @@ #define FIFOREAD_DATA_MASK (0xfff << 0) #define FIFOREAD_CHNLID_MASK (0xf << 16) +/* DMA ENABLE/CLEAR Register */ +#define DMA_FIFO0 BIT(0) +#define DMA_FIFO1 BIT(1) + /* Sequencer Status */ #define SEQ_STATUS BIT(5) #define CHARGE_STEP 0x11 From 6ba5dee90b2483f8a78081376f56e6b65c10ea92 Mon Sep 17 00:00:00 2001 From: Brian Masney Date: Thu, 3 Nov 2016 08:56:12 -0400 Subject: [PATCH 35/40] staging: iio: tsl2583: i2c_smbus_write_byte() / i2c_smbus_read_byte() migration There were several places where the driver would first call i2c_smbus_write_byte() to select the register on the device, and then call i2c_smbus_read_byte() to get the contents of that register. The code would look roughly like: /* Select register */ i2c_smbus_write_byte(client, REGISTER); /* Read the the last register that was written to */ int data = i2c_smbus_read_byte(client); Rewrite this to use i2c_smbus_read_byte_data() to combine the two calls into one: int data = i2c_smbus_read_byte_data(chip->client, REGISTER); Verified that the driver still functions correctly using a TSL2581 hooked up to a Raspberry Pi 2. This fixes the following warnings that were found by the kbuild test robot that were introduced by commit 8ba355cce3c6 ("staging: iio: tsl2583: check for error code from i2c_smbus_read_byte()"). drivers/staging/iio/light/tsl2583.c:365:5-12: WARNING: Unsigned expression compared with zero: reg_val < 0 drivers/staging/iio/light/tsl2583.c:388:5-12: WARNING: Unsigned expression compared with zero: reg_val < 0 This also removes the need for the taos_i2c_read() function since all callers were only calling the function with a length of 1. Signed-off-by: Brian Masney Cc: Julia Lawall Signed-off-by: Jonathan Cameron --- drivers/staging/iio/light/tsl2583.c | 85 ++++++----------------------- 1 file changed, 16 insertions(+), 69 deletions(-) diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c index 49b19f5c1d84..a3a909560c88 100644 --- a/drivers/staging/iio/light/tsl2583.c +++ b/drivers/staging/iio/light/tsl2583.c @@ -154,39 +154,6 @@ static void taos_defaults(struct tsl2583_chip *chip) /* Known external ALS reading used for calibration */ } -/* - * Read a number of bytes starting at register (reg) location. - * Return 0, or i2c_smbus_write_byte ERROR code. - */ -static int -taos_i2c_read(struct i2c_client *client, u8 reg, u8 *val, unsigned int len) -{ - int i, ret; - - for (i = 0; i < len; i++) { - /* select register to write */ - ret = i2c_smbus_write_byte(client, (TSL258X_CMD_REG | reg)); - if (ret < 0) { - dev_err(&client->dev, - "taos_i2c_read failed to write register %x\n", - reg); - return ret; - } - /* read the data */ - ret = i2c_smbus_read_byte(client); - if (ret < 0) { - dev_err(&client->dev, - "%s failed to read byte after writing to register %x\n", - __func__, reg); - return ret; - } - *val = ret; - val++; - reg++; - } - return 0; -} - /* * Reads and calculates current lux value. * The raw ch0 and ch1 values of the ambient light sensed in the last @@ -220,13 +187,14 @@ static int taos_get_lux(struct iio_dev *indio_dev) goto done; } - ret = taos_i2c_read(chip->client, (TSL258X_CMD_REG), &buf[0], 1); + ret = i2c_smbus_read_byte_data(chip->client, TSL258X_CMD_REG); if (ret < 0) { dev_err(&chip->client->dev, "taos_get_lux failed to read CMD_REG\n"); goto done; } + /* is data new & valid */ - if (!(buf[0] & TSL258X_STA_ADC_INTR)) { + if (!(ret & TSL258X_STA_ADC_INTR)) { dev_err(&chip->client->dev, "taos_get_lux data not valid\n"); ret = chip->als_cur_info.lux; /* return LAST VALUE */ goto done; @@ -235,13 +203,14 @@ static int taos_get_lux(struct iio_dev *indio_dev) for (i = 0; i < 4; i++) { int reg = TSL258X_CMD_REG | (TSL258X_ALS_CHAN0LO + i); - ret = taos_i2c_read(chip->client, reg, &buf[i], 1); + ret = i2c_smbus_read_byte_data(chip->client, reg); if (ret < 0) { dev_err(&chip->client->dev, "taos_get_lux failed to read register %x\n", reg); goto done; } + buf[i] = ret; } /* @@ -343,52 +312,36 @@ static int taos_get_lux(struct iio_dev *indio_dev) static int taos_als_calibrate(struct iio_dev *indio_dev) { struct tsl2583_chip *chip = iio_priv(indio_dev); - u8 reg_val; unsigned int gain_trim_val; int ret; int lux_val; - ret = i2c_smbus_write_byte(chip->client, - (TSL258X_CMD_REG | TSL258X_CNTRL)); + ret = i2c_smbus_read_byte_data(chip->client, + TSL258X_CMD_REG | TSL258X_CNTRL); if (ret < 0) { dev_err(&chip->client->dev, - "taos_als_calibrate failed to reach the CNTRL register, ret=%d\n", - ret); - return ret; - } - - reg_val = i2c_smbus_read_byte(chip->client); - if (reg_val < 0) { - dev_err(&chip->client->dev, - "%s failed to read after writing to the CNTRL register\n", + "%s failed to read from the CNTRL register\n", __func__); return ret; } - if ((reg_val & (TSL258X_CNTL_ADC_ENBL | TSL258X_CNTL_PWR_ON)) + if ((ret & (TSL258X_CNTL_ADC_ENBL | TSL258X_CNTL_PWR_ON)) != (TSL258X_CNTL_ADC_ENBL | TSL258X_CNTL_PWR_ON)) { dev_err(&chip->client->dev, "taos_als_calibrate failed: device not powered on with ADC enabled\n"); return -EINVAL; } - ret = i2c_smbus_write_byte(chip->client, - (TSL258X_CMD_REG | TSL258X_CNTRL)); + ret = i2c_smbus_read_byte_data(chip->client, + TSL258X_CMD_REG | TSL258X_CNTRL); if (ret < 0) { dev_err(&chip->client->dev, - "taos_als_calibrate failed to reach the STATUS register, ret=%d\n", - ret); - return ret; - } - reg_val = i2c_smbus_read_byte(chip->client); - if (reg_val < 0) { - dev_err(&chip->client->dev, - "%s failed to read after writing to the STATUS register\n", + "%s failed to read from the CNTRL register\n", __func__); return ret; } - if ((reg_val & TSL258X_STA_ADC_VALID) != TSL258X_STA_ADC_VALID) { + if ((ret & TSL258X_STA_ADC_VALID) != TSL258X_STA_ADC_VALID) { dev_err(&chip->client->dev, "taos_als_calibrate failed: STATUS - ADC not valid.\n"); return -ENODATA; @@ -847,15 +800,9 @@ static int taos_probe(struct i2c_client *clientp, memcpy(chip->taos_config, taos_config, sizeof(chip->taos_config)); for (i = 0; i < TSL258X_MAX_DEVICE_REGS; i++) { - ret = i2c_smbus_write_byte(clientp, - (TSL258X_CMD_REG | (TSL258X_CNTRL + i))); - if (ret < 0) { - dev_err(&clientp->dev, - "i2c_smbus_write_byte to cmd reg failed in taos_probe(), err = %d\n", - ret); - return ret; - } - ret = i2c_smbus_read_byte(clientp); + ret = i2c_smbus_read_byte_data(clientp, + (TSL258X_CMD_REG | + (TSL258X_CNTRL + i))); if (ret < 0) { dev_err(&clientp->dev, "i2c_smbus_read_byte from reg failed in taos_probe(), err = %d\n", From 6bd0cb2b594b4a735301245cf9387c9c671e6168 Mon Sep 17 00:00:00 2001 From: Brian Masney Date: Thu, 3 Nov 2016 08:56:13 -0400 Subject: [PATCH 36/40] staging: iio: tsl2583: removed unused code from device probing taos_probe() queries the all of the sensor's registers and loads all of the values into a buffer stored on the stack. Only the chip ID register was actually used. Change the probe function to just query the chip ID register on the device. Verified that the driver still functions correctly using a TSL2581 hooked up to a Raspberry Pi 2. Signed-off-by: Brian Masney Signed-off-by: Jonathan Cameron --- drivers/staging/iio/light/tsl2583.c | 37 ++++++++++------------------- 1 file changed, 13 insertions(+), 24 deletions(-) diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c index a3a909560c88..388440b4fcd7 100644 --- a/drivers/staging/iio/light/tsl2583.c +++ b/drivers/staging/iio/light/tsl2583.c @@ -31,8 +31,6 @@ #include #include -#define TSL258X_MAX_DEVICE_REGS 32 - /* Triton register offsets */ #define TSL258X_REG_MAX 8 @@ -66,6 +64,9 @@ /* Lux calculation constants */ #define TSL258X_LUX_CALC_OVER_FLOW 65535 +#define TSL2583_CHIP_ID 0x90 +#define TSL2583_CHIP_ID_MASK 0xf0 + enum { TSL258X_CHIP_UNKNOWN = 0, TSL258X_CHIP_WORKING = 1, @@ -607,12 +608,6 @@ static const struct attribute_group tsl2583_attribute_group = { .attrs = sysfs_attrs_ctrl, }; -/* Use the default register values to identify the Taos device */ -static int taos_tsl258x_device(unsigned char *bufp) -{ - return ((bufp[TSL258X_CHIPID] & 0xf0) == 0x90); -} - static const struct iio_chan_spec tsl2583_channels[] = { { .type = IIO_LIGHT, @@ -777,8 +772,7 @@ static const struct iio_info tsl2583_info = { static int taos_probe(struct i2c_client *clientp, const struct i2c_device_id *idp) { - int i, ret; - unsigned char buf[TSL258X_MAX_DEVICE_REGS]; + int ret; struct tsl2583_chip *chip; struct iio_dev *indio_dev; @@ -799,22 +793,17 @@ static int taos_probe(struct i2c_client *clientp, chip->taos_chip_status = TSL258X_CHIP_UNKNOWN; memcpy(chip->taos_config, taos_config, sizeof(chip->taos_config)); - for (i = 0; i < TSL258X_MAX_DEVICE_REGS; i++) { - ret = i2c_smbus_read_byte_data(clientp, - (TSL258X_CMD_REG | - (TSL258X_CNTRL + i))); - if (ret < 0) { - dev_err(&clientp->dev, - "i2c_smbus_read_byte from reg failed in taos_probe(), err = %d\n", - ret); - return ret; - } - buf[i] = ret; + ret = i2c_smbus_read_byte_data(clientp, + TSL258X_CMD_REG | TSL258X_CHIPID); + if (ret < 0) { + dev_err(&clientp->dev, + "%s failed to read the chip ID register\n", __func__); + return ret; } - if (!taos_tsl258x_device(buf)) { - dev_info(&clientp->dev, - "i2c device found but does not match expected id in taos_probe()\n"); + if ((ret & TSL2583_CHIP_ID_MASK) != TSL2583_CHIP_ID) { + dev_info(&clientp->dev, "%s received an unknown chip ID %x\n", + __func__, ret); return -EINVAL; } From 4a0f36144c2504313f62cad31564e405ebbbb47b Mon Sep 17 00:00:00 2001 From: Brian Masney Date: Thu, 3 Nov 2016 08:56:14 -0400 Subject: [PATCH 37/40] staging: iio: tsl2583: fixed ordering of comments in taos_defaults() The comments in taos_defaults() appear after the line of code that they apply to. This patch moves the comments so that they appear before the code. Some of the comments were updated to be more informative. Signed-off-by: Brian Masney Signed-off-by: Jonathan Cameron --- drivers/staging/iio/light/tsl2583.c | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c index 388440b4fcd7..709f44662a9f 100644 --- a/drivers/staging/iio/light/tsl2583.c +++ b/drivers/staging/iio/light/tsl2583.c @@ -143,16 +143,23 @@ static const struct gainadj gainadj[] = { */ static void taos_defaults(struct tsl2583_chip *chip) { - /* Operational parameters */ + /* + * The integration time must be a multiple of 50ms and within the + * range [50, 600] ms. + */ chip->taos_settings.als_time = 100; - /* must be a multiple of 50mS */ + + /* + * This is an index into the gainadj table. Assume clear glass as the + * default. + */ chip->taos_settings.als_gain = 0; - /* this is actually an index into the gain table */ - /* assume clear glass as default */ + + /* Default gain trim to account for aperture effects */ chip->taos_settings.als_gain_trim = 1000; - /* default gain trim to account for aperture effects */ - chip->taos_settings.als_cal_target = 130; + /* Known external ALS reading used for calibration */ + chip->taos_settings.als_cal_target = 130; } /* From babe444798ebfe02f2e870322927c7018957c7f3 Mon Sep 17 00:00:00 2001 From: Brian Masney Date: Thu, 3 Nov 2016 08:56:15 -0400 Subject: [PATCH 38/40] staging: iio: tsl2583: remove redundant power off sequence in taos_chip_on() taos_chip_on() explicitly turns the sensor power on and then writes the 8 registers that are stored in taos_config. The first register in taos_config is the CONTROL register and the configuration is set to turn the power off. The existing state sequence in taos_chip_on() is: - Turn device power on - Turn device power off (via taos_config) - Configure other 7 registers (via taos_config) - Turn device power on, enable ADC This patch changes the code so that the device is not powered off via taos_config. Verified that the driver still functions correctly using a TSL2581 hooked up to a Raspberry Pi 2. Signed-off-by: Brian Masney Signed-off-by: Jonathan Cameron --- drivers/staging/iio/light/tsl2583.c | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c index 709f44662a9f..7fb09c641822 100644 --- a/drivers/staging/iio/light/tsl2583.c +++ b/drivers/staging/iio/light/tsl2583.c @@ -410,17 +410,8 @@ static int taos_chip_on(struct iio_dev *indio_dev) chip->als_saturation = als_count * 922; /* 90% of full scale */ chip->als_time_scale = (als_time + 25) / 50; - /* - * TSL258x Specific power-on / adc enable sequence - * Power on the device 1st. - */ - utmp = TSL258X_CNTL_PWR_ON; - ret = i2c_smbus_write_byte_data(chip->client, - TSL258X_CMD_REG | TSL258X_CNTRL, utmp); - if (ret < 0) { - dev_err(&chip->client->dev, "taos_chip_on failed on CNTRL reg.\n"); - return ret; - } + /* Power on the device; ADC off. */ + chip->taos_config[TSL258X_CNTRL] = TSL258X_CNTL_PWR_ON; /* * Use the following shadow copy for our delay before enabling ADC. From 0561155f6fc5d4e3702b3805fea0ec1f55e5f08e Mon Sep 17 00:00:00 2001 From: Brian Masney Date: Thu, 3 Nov 2016 08:56:16 -0400 Subject: [PATCH 39/40] staging: iio: tsl2583: don't shutdown chip when updating the lux table in_illuminance_lux_table_store() shuts down the chip, updates the contents of the lux table, and then turns the chip back on. The values in lux table are not used by the chip and are only used internally by the driver. It is not necessary to change the power state on the chip. This patch removes the calls to taos_chip_off() and taos_chip_on() in in_illuminance_lux_table_store(). Signed-off-by: Brian Masney Signed-off-by: Jonathan Cameron --- drivers/staging/iio/light/tsl2583.c | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c index 7fb09c641822..af1cf9bdbd85 100644 --- a/drivers/staging/iio/light/tsl2583.c +++ b/drivers/staging/iio/light/tsl2583.c @@ -564,20 +564,10 @@ static ssize_t in_illuminance_lux_table_store(struct device *dev, goto done; } - if (chip->taos_chip_status == TSL258X_CHIP_WORKING) { - ret = taos_chip_off(indio_dev); - if (ret < 0) - goto done; - } - /* Zero out the table */ memset(taos_device_lux, 0, sizeof(taos_device_lux)); memcpy(taos_device_lux, &value[1], (value[0] * 4)); - ret = taos_chip_on(indio_dev); - if (ret < 0) - goto done; - ret = len; done: From c2b0d2cfb0317275cf04257fc3f5dfbdd9ffa274 Mon Sep 17 00:00:00 2001 From: Brian Masney Date: Thu, 3 Nov 2016 08:56:17 -0400 Subject: [PATCH 40/40] staging: iio: tsl2583: remove redudant i2c call in taos_als_calibrate() taos_als_calibrate() queries the control register to determine if the unit is powered on and has the ADC enabled. It then queries the same register a second time to determine if the ADC reading is valid. This patch removes the redundant i2c_smbus_read_byte_data() call. Verified that the driver still functions correctly using a TSL2581 hooked up to a Raspberry Pi 2. Signed-off-by: Brian Masney Signed-off-by: Jonathan Cameron --- drivers/staging/iio/light/tsl2583.c | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c index af1cf9bdbd85..7eab17f4557e 100644 --- a/drivers/staging/iio/light/tsl2583.c +++ b/drivers/staging/iio/light/tsl2583.c @@ -338,18 +338,7 @@ static int taos_als_calibrate(struct iio_dev *indio_dev) dev_err(&chip->client->dev, "taos_als_calibrate failed: device not powered on with ADC enabled\n"); return -EINVAL; - } - - ret = i2c_smbus_read_byte_data(chip->client, - TSL258X_CMD_REG | TSL258X_CNTRL); - if (ret < 0) { - dev_err(&chip->client->dev, - "%s failed to read from the CNTRL register\n", - __func__); - return ret; - } - - if ((ret & TSL258X_STA_ADC_VALID) != TSL258X_STA_ADC_VALID) { + } else if ((ret & TSL258X_STA_ADC_VALID) != TSL258X_STA_ADC_VALID) { dev_err(&chip->client->dev, "taos_als_calibrate failed: STATUS - ADC not valid.\n"); return -ENODATA;