Add support to STM LSM6DS3-LSM6DSM 6-axis (acc + gyro) Mems sensor http://www.st.com/resource/en/datasheet/lsm6ds3.pdf http://www.st.com/resource/en/datasheet/lsm6dsm.pdf - continuos mode support - i2c support - spi support - trigger mode support - supported devices: lsm6ds3, lsm6dsm Signed-off-by: Lorenzo Bianconi <lorenzo.bianconi@xxxxxx> --- drivers/iio/imu/Kconfig | 1 + drivers/iio/imu/Makefile | 1 + drivers/iio/imu/st_lsm6dsx/Kconfig | 23 + drivers/iio/imu/st_lsm6dsx/Makefile | 6 + drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h | 105 ++++ drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c | 241 +++++++++ drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 705 +++++++++++++++++++++++++ drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c | 137 +++++ drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c | 155 ++++++ 9 files changed, 1374 insertions(+) create mode 100644 drivers/iio/imu/st_lsm6dsx/Kconfig create mode 100644 drivers/iio/imu/st_lsm6dsx/Makefile create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c diff --git a/drivers/iio/imu/Kconfig b/drivers/iio/imu/Kconfig index 1f1ad41..156630a 100644 --- a/drivers/iio/imu/Kconfig +++ b/drivers/iio/imu/Kconfig @@ -39,6 +39,7 @@ config KMX61 be called kmx61. source "drivers/iio/imu/inv_mpu6050/Kconfig" +source "drivers/iio/imu/st_lsm6dsx/Kconfig" endmenu diff --git a/drivers/iio/imu/Makefile b/drivers/iio/imu/Makefile index c71bcd3..549621b 100644 --- a/drivers/iio/imu/Makefile +++ b/drivers/iio/imu/Makefile @@ -15,5 +15,6 @@ obj-$(CONFIG_IIO_ADIS_LIB) += adis_lib.o obj-y += bmi160/ obj-y += inv_mpu6050/ +obj-y += st_lsm6dsx/ obj-$(CONFIG_KMX61) += kmx61.o diff --git a/drivers/iio/imu/st_lsm6dsx/Kconfig b/drivers/iio/imu/st_lsm6dsx/Kconfig new file mode 100644 index 0000000..5001f41 --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/Kconfig @@ -0,0 +1,23 @@ + +config IIO_ST_LSM6DSX + tristate "STMicroelectronics LSM6DS3/LSM6DSM sensor driver" + depends on (I2C || SPI) && SYSFS + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + select IIO_ST_LSM6DSX_I2C if (I2C) + select IIO_ST_LSM6DSX_SPI if (SPI_MASTER) + help + Say yes here to build support for STMicroelectronics LSM6DSX imu + sensor. Supported devices: lsm6ds3, lsm6dsm + + To compile this driver as a module, choose M here: the module + will be called st_lsm6dsx. + +config IIO_ST_LSM6DSX_I2C + tristate + depends on IIO_ST_LSM6DSX + +config IIO_ST_LSM6DSX_SPI + tristate + depends on IIO_ST_LSM6DSX + diff --git a/drivers/iio/imu/st_lsm6dsx/Makefile b/drivers/iio/imu/st_lsm6dsx/Makefile new file mode 100644 index 0000000..257d6ee --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/Makefile @@ -0,0 +1,6 @@ +st_lsm6dsx-y := st_lsm6dsx_core.o +st_lsm6dsx-$(CONFIG_IIO_BUFFER) += st_lsm6dsx_buffer.o + +obj-$(CONFIG_IIO_ST_LSM6DSX) += st_lsm6dsx.o +obj-$(CONFIG_IIO_ST_LSM6DSX_I2C) += st_lsm6dsx_i2c.o +obj-$(CONFIG_IIO_ST_LSM6DSX_SPI) += st_lsm6dsx_spi.o diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h new file mode 100644 index 0000000..e79d301 --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h @@ -0,0 +1,105 @@ +/* + * STMicroelectronics st_lsm6dsx sensor driver + * + * Copyright 2016 STMicroelectronics Inc. + * + * Lorenzo Bianconi <lorenzo.bianconi@xxxxxx> + * Denis Ciocca <denis.ciocca@xxxxxx> + * + * Licensed under the GPL-2. + */ + +#ifndef ST_LSM6DSX_H +#define ST_LSM6DSX_H + +#include <linux/device.h> + +#define ST_LSM6DS3_DEV_NAME "lsm6ds3" +#define ST_LSM6DSM_DEV_NAME "lsm6dsm" + +#define ST_LSM6DSX_SAMPLE_SIZE 6 + +#if defined(CONFIG_IIO_ST_LSM6DSX_SPI) || \ + defined(CONFIG_IIO_ST_LSM6DSX_SPI_MODULE) +#define ST_LSM6DSX_RX_MAX_LENGTH 12 +#define ST_LSM6DSX_TX_MAX_LENGTH 8193 + +struct st_lsm6dsx_transfer_buffer { + u8 rx_buf[ST_LSM6DSX_RX_MAX_LENGTH]; + u8 tx_buf[ST_LSM6DSX_TX_MAX_LENGTH] ____cacheline_aligned; +}; +#endif /* CONFIG_IIO_ST_LSM6DSX_SPI */ + +struct st_lsm6dsx_transfer_function { + int (*read)(struct device *dev, u8 addr, int len, u8 *data); + int (*write)(struct device *dev, u8 addr, int len, u8 *data); +}; + +enum st_lsm6dsx_sensor_id { + ST_LSM6DSX_ID_ACC, + ST_LSM6DSX_ID_GYRO, + ST_LSM6DSX_ID_MAX, +}; + +struct st_lsm6dsx_sensor { + enum st_lsm6dsx_sensor_id id; + struct st_lsm6dsx_dev *dev; + struct iio_trigger *trig; + + u16 odr; + u32 gain; + + u8 drdy_data_mask; + u8 drdy_irq_mask; +}; + +struct st_lsm6dsx_dev { + const char *name; + struct device *dev; + int irq; + struct mutex lock; + + s64 hw_timestamp; + + struct iio_dev *iio_devs[ST_LSM6DSX_ID_MAX]; + + const struct st_lsm6dsx_transfer_function *tf; +#if defined(CONFIG_IIO_ST_LSM6DSX_SPI) || \ + defined(CONFIG_IIO_ST_LSM6DSX_SPI_MODULE) + struct st_lsm6dsx_transfer_buffer tb; +#endif /* CONFIG_IIO_ST_LSM6DSX_SPI */ +}; + +int st_lsm6dsx_probe(struct st_lsm6dsx_dev *dev); +int st_lsm6dsx_remove(struct st_lsm6dsx_dev *dev); +int st_lsm6dsx_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable); +int st_lsm6dsx_write_with_mask(struct st_lsm6dsx_dev *dev, u8 addr, u8 mask, + u8 val); +#ifdef CONFIG_IIO_BUFFER +int st_lsm6dsx_allocate_triggers(struct st_lsm6dsx_dev *dev); +int st_lsm6dsx_deallocate_triggers(struct st_lsm6dsx_dev *dev); +int st_lsm6dsx_allocate_buffers(struct st_lsm6dsx_dev *dev); +int st_lsm6dsx_deallocate_buffers(struct st_lsm6dsx_dev *dev); +#else +static inline int st_lsm6dsx_allocate_triggers(struct st_lsm6dsx_dev *dev) +{ + return 0; +} + +static inline int st_lsm6dsx_deallocate_triggers(struct st_lsm6dsx_dev *dev) +{ + return 0; +} + +static inline int st_lsm6dsx_allocate_buffers(struct st_lsm6dsx_dev *dev) +{ + return 0; +} + +static inline int st_lsm6dsx_deallocate_buffers(struct st_lsm6dsx_dev *dev) +{ + return 0; +} +#endif /* CONFIG_IIO_BUFFER */ +#endif /* ST_LSM6DSX_H */ + diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c new file mode 100644 index 0000000..5fcbb4d --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c @@ -0,0 +1,241 @@ +/* + * STMicroelectronics st_lsm6dsx sensor driver + * + * Copyright 2016 STMicroelectronics Inc. + * + * Lorenzo Bianconi <lorenzo.bianconi@xxxxxx> + * Denis Ciocca <denis.ciocca@xxxxxx> + * + * Licensed under the GPL-2. + */ +#include <linux/module.h> +#include <linux/device.h> +#include <linux/kernel.h> +#include <linux/interrupt.h> +#include <linux/slab.h> +#include <linux/irqreturn.h> +#include <linux/iio/iio.h> +#include <linux/iio/trigger.h> +#include <linux/interrupt.h> +#include <linux/iio/events.h> +#include <linux/iio/trigger_consumer.h> +#include <linux/iio/triggered_buffer.h> +#include <linux/iio/buffer.h> +#include "st_lsm6dsx.h" + +#define REG_INT1_ADDR 0x0d +#define REG_DATA_AVL_ADDR 0x1e + +static int st_lsm6dsx_trig_set_state(struct iio_trigger *trig, bool state) +{ + u8 val = (state) ? 1 : 0; + struct iio_dev *iio_dev = iio_trigger_get_drvdata(trig); + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + + return st_lsm6dsx_write_with_mask(sensor->dev, REG_INT1_ADDR, + sensor->drdy_irq_mask, val); +} + +static const struct iio_trigger_ops st_lsm6dsx_trigger_ops = { + .owner = THIS_MODULE, + .set_trigger_state = st_lsm6dsx_trig_set_state, +}; + +static irqreturn_t st_lsm6dsx_trigger_handler_th(int irq, void *private) +{ + struct st_lsm6dsx_dev *dev = (struct st_lsm6dsx_dev *)private; + + dev->hw_timestamp = iio_get_time_ns(dev->iio_devs[0]); + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t st_lsm6dsx_trigger_handler_bh(int irq, void *private) +{ + int i, err; + u8 avl_data; + struct st_lsm6dsx_sensor *sensor; + struct st_lsm6dsx_dev *dev = (struct st_lsm6dsx_dev *)private; + + err = dev->tf->read(dev->dev, REG_DATA_AVL_ADDR, 1, &avl_data); + if (err < 0) + return IRQ_HANDLED; + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + sensor = iio_priv(dev->iio_devs[i]); + + if (avl_data & sensor->drdy_data_mask) + iio_trigger_poll_chained(sensor->trig); + } + + return IRQ_HANDLED; +} + +int st_lsm6dsx_allocate_triggers(struct st_lsm6dsx_dev *dev) +{ + int i, err, count = 0; + struct st_lsm6dsx_sensor *sensor; + unsigned long irq_type; + + irq_type = irqd_get_trigger_type(irq_get_irq_data(dev->irq)); + + switch (irq_type) { + case IRQF_TRIGGER_HIGH: + case IRQF_TRIGGER_LOW: + case IRQF_TRIGGER_RISING: + break; + default: + dev_info(dev->dev, + "mode %lx unsupported, use IRQF_TRIGGER_RISING\n", + irq_type); + irq_type = IRQF_TRIGGER_RISING; + break; + } + + err = devm_request_threaded_irq(dev->dev, dev->irq, + st_lsm6dsx_trigger_handler_th, + st_lsm6dsx_trigger_handler_bh, + irq_type | IRQF_ONESHOT, + dev->name, dev); + if (err) { + dev_err(dev->dev, "failed to request trigger irq %d\n", + dev->irq); + return err; + } + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + sensor = iio_priv(dev->iio_devs[i]); + sensor->trig = iio_trigger_alloc("%s-trigger", + dev->iio_devs[i]->name); + if (!sensor->trig) { + err = -ENOMEM; + goto iio_trigger_error; + } + + iio_trigger_set_drvdata(sensor->trig, dev->iio_devs[i]); + sensor->trig->ops = &st_lsm6dsx_trigger_ops; + sensor->trig->dev.parent = dev->dev; + + err = iio_trigger_register(sensor->trig); + if (err < 0) { + dev_err(dev->dev, "failed to register iio trigger\n"); + goto iio_trigger_error; + } + dev->iio_devs[i]->trig = iio_trigger_get(sensor->trig); + + count++; + } + + return 0; + +iio_trigger_error: + for (i = count - 1; i >= 0; i--) { + sensor = iio_priv(dev->iio_devs[i]); + iio_trigger_unregister(sensor->trig); + } + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + sensor = iio_priv(dev->iio_devs[i]); + if (sensor->trig) + iio_trigger_free(sensor->trig); + } + + return err; +} + +int st_lsm6dsx_deallocate_triggers(struct st_lsm6dsx_dev *dev) +{ + int i; + struct st_lsm6dsx_sensor *sensor; + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + sensor = iio_priv(dev->iio_devs[i]); + iio_trigger_unregister(sensor->trig); + iio_trigger_free(sensor->trig); + } + + return 0; +} + +static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev) +{ + return st_lsm6dsx_set_enable(iio_priv(iio_dev), true); +} + +static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev) +{ + return st_lsm6dsx_set_enable(iio_priv(iio_dev), false); +} + +static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = { + .preenable = st_lsm6dsx_buffer_preenable, + .postenable = iio_triggered_buffer_postenable, + .predisable = iio_triggered_buffer_predisable, + .postdisable = st_lsm6dsx_buffer_postdisable, +}; + +static irqreturn_t st_lsm6dsx_buffer_handler_bh(int irq, void *p) +{ + int i, err, chan_byte, in = 0, out = 0; + struct iio_poll_func *pf = p; + struct iio_dev *iio_dev = pf->indio_dev; + struct iio_chan_spec const *ch = iio_dev->channels; + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsx_dev *dev = sensor->dev; + u8 out_data[iio_dev->scan_bytes], buffer[ST_LSM6DSX_SAMPLE_SIZE]; + + err = dev->tf->read(dev->dev, ch[0].address, ST_LSM6DSX_SAMPLE_SIZE, + buffer); + if (err < 0) + goto out; + + for (i = 0; i < iio_dev->num_channels; i++) { + chan_byte = ch[i].scan_type.storagebits >> 3; + + if (test_bit(i, iio_dev->active_scan_mask)) { + memcpy(&out_data[out], &buffer[in], chan_byte); + out += chan_byte; + } + in += chan_byte; + } + + iio_push_to_buffers_with_timestamp(iio_dev, out_data, + dev->hw_timestamp); + +out: + iio_trigger_notify_done(sensor->trig); + + return IRQ_HANDLED; +} + +int st_lsm6dsx_allocate_buffers(struct st_lsm6dsx_dev *dev) +{ + int i, err, count = 0; + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + err = iio_triggered_buffer_setup(dev->iio_devs[i], NULL, + st_lsm6dsx_buffer_handler_bh, + &st_lsm6dsx_buffer_ops); + if (err < 0) + goto iio_buffer_error; + count++; + } + + return 0; + +iio_buffer_error: + for (i = count - 1; i >= 0; i--) + iio_triggered_buffer_cleanup(dev->iio_devs[i]); + + return err; +} + +int st_lsm6dsx_deallocate_buffers(struct st_lsm6dsx_dev *dev) +{ + int i; + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) + iio_triggered_buffer_cleanup(dev->iio_devs[i]); + + return 0; +} + diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c new file mode 100644 index 0000000..ed3e714 --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c @@ -0,0 +1,705 @@ +/* + * STMicroelectronics st_lsm6dsx sensor driver + * + * Copyright 2016 STMicroelectronics Inc. + * + * Lorenzo Bianconi <lorenzo.bianconi@xxxxxx> + * Denis Ciocca <denis.ciocca@xxxxxx> + * + * Licensed under the GPL-2. + */ + +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/interrupt.h> +#include <linux/slab.h> +#include <linux/delay.h> +#include <linux/iio/iio.h> +#include <linux/iio/sysfs.h> +#include <asm/unaligned.h> +#include "st_lsm6dsx.h" + +#define REG_ACC_DRDY_IRQ_MASK 0x01 +#define REG_GYRO_DRDY_IRQ_MASK 0x02 +#define REG_WHOAMI_ADDR 0x0f +#define REG_RESET_ADDR 0x12 +#define REG_RESET_MASK 0x01 +#define REG_BDU_ADDR 0x12 +#define REG_BDU_MASK 0x40 +#define REG_INT2_ON_INT1_ADDR 0x13 +#define REG_INT2_ON_INT1_MASK 0x20 +#define REG_ROUNDING_ADDR 0x16 +#define REG_ROUNDING_MASK 0x04 +#define REG_LIR_ADDR 0x58 +#define REG_LIR_MASK 0x01 + +#define REG_ACC_ODR_ADDR 0x10 +#define REG_ACC_ODR_MASK 0xf0 +#define REG_ACC_FS_ADDR 0x10 +#define REG_ACC_FS_MASK 0x0c +#define REG_ACC_OUT_X_L_ADDR 0x28 +#define REG_ACC_OUT_Y_L_ADDR 0x2a +#define REG_ACC_OUT_Z_L_ADDR 0x2c + +#define REG_GYRO_ODR_ADDR 0x11 +#define REG_GYRO_ODR_MASK 0xf0 +#define REG_GYRO_FS_ADDR 0x11 +#define REG_GYRO_FS_MASK 0x0c +#define REG_GYRO_OUT_X_L_ADDR 0x22 +#define REG_GYRO_OUT_Y_L_ADDR 0x24 +#define REG_GYRO_OUT_Z_L_ADDR 0x26 + +#define ST_LSM6DS3_WHOAMI 0x69 +#define ST_LSM6DSM_WHOAMI 0x6a + +#define ST_LSM6DSX_ACC_FS_2G_GAIN IIO_G_TO_M_S_2(61) +#define ST_LSM6DSX_ACC_FS_4G_GAIN IIO_G_TO_M_S_2(122) +#define ST_LSM6DSX_ACC_FS_8G_GAIN IIO_G_TO_M_S_2(244) +#define ST_LSM6DSX_ACC_FS_16G_GAIN IIO_G_TO_M_S_2(488) + +#define ST_LSM6DSX_DATA_ACC_AVL_MASK 0x01 + +#define ST_LSM6DSX_GYRO_FS_245_GAIN IIO_DEGREE_TO_RAD(4375) +#define ST_LSM6DSX_GYRO_FS_500_GAIN IIO_DEGREE_TO_RAD(8750) +#define ST_LSM6DSX_GYRO_FS_1000_GAIN IIO_DEGREE_TO_RAD(17500) +#define ST_LSM6DSX_GYRO_FS_2000_GAIN IIO_DEGREE_TO_RAD(70000) + +#define ST_LSM6DSX_DATA_GYRO_AVL_MASK 0x02 + +struct st_lsm6dsx_odr { + u32 hz; + u8 val; +}; + +#define ST_LSM6DSX_ODR_LIST_SIZE 6 +struct st_lsm6dsx_odr_table_entry { + u8 addr; + u8 mask; + struct st_lsm6dsx_odr odr_avl[ST_LSM6DSX_ODR_LIST_SIZE]; +}; + +static const struct st_lsm6dsx_odr_table_entry st_lsm6dsx_odr_table[] = { + [ST_LSM6DSX_ID_ACC] = { + .addr = REG_ACC_ODR_ADDR, + .mask = REG_ACC_ODR_MASK, + .odr_avl[0] = { 13, 0x01 }, + .odr_avl[1] = { 26, 0x02 }, + .odr_avl[2] = { 52, 0x03 }, + .odr_avl[3] = { 104, 0x04 }, + .odr_avl[4] = { 208, 0x05 }, + .odr_avl[5] = { 416, 0x06 }, + }, + [ST_LSM6DSX_ID_GYRO] = { + .addr = REG_GYRO_ODR_ADDR, + .mask = REG_GYRO_ODR_MASK, + .odr_avl[0] = { 13, 0x01 }, + .odr_avl[1] = { 26, 0x02 }, + .odr_avl[2] = { 52, 0x03 }, + .odr_avl[3] = { 104, 0x04 }, + .odr_avl[4] = { 208, 0x05 }, + .odr_avl[5] = { 416, 0x06 }, + } +}; + +struct st_lsm6dsx_fs { + u32 gain; + u8 val; +}; + +#define ST_LSM6DSX_FS_LIST_SIZE 4 +struct st_lsm6dsx_fs_table_entry { + u8 addr; + u8 mask; + struct st_lsm6dsx_fs fs_avl[ST_LSM6DSX_FS_LIST_SIZE]; +}; + +static const struct st_lsm6dsx_fs_table_entry st_lsm6dsx_fs_table[] = { + [ST_LSM6DSX_ID_ACC] = { + .addr = REG_ACC_FS_ADDR, + .mask = REG_ACC_FS_MASK, + .fs_avl[0] = { ST_LSM6DSX_ACC_FS_2G_GAIN, 0x0 }, + .fs_avl[1] = { ST_LSM6DSX_ACC_FS_4G_GAIN, 0x2 }, + .fs_avl[2] = { ST_LSM6DSX_ACC_FS_8G_GAIN, 0x3 }, + .fs_avl[3] = { ST_LSM6DSX_ACC_FS_16G_GAIN, 0x1 }, + }, + [ST_LSM6DSX_ID_GYRO] = { + .addr = REG_GYRO_FS_ADDR, + .mask = REG_GYRO_FS_MASK, + .fs_avl[0] = { ST_LSM6DSX_GYRO_FS_245_GAIN, 0x0 }, + .fs_avl[1] = { ST_LSM6DSX_GYRO_FS_500_GAIN, 0x1 }, + .fs_avl[2] = { ST_LSM6DSX_GYRO_FS_1000_GAIN, 0x2 }, + .fs_avl[3] = { ST_LSM6DSX_GYRO_FS_2000_GAIN, 0x3 }, + } +}; + +static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = { + { + .type = IIO_ACCEL, + .address = REG_ACC_OUT_X_L_ADDR, + .modified = 1, + .channel2 = IIO_MOD_X, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | + BIT(IIO_CHAN_INFO_SCALE), + .scan_index = 0, + .scan_type = { + .sign = 's', + .realbits = 16, + .storagebits = 16, + .shift = 0, + .endianness = IIO_LE, + }, + }, + { + .type = IIO_ACCEL, + .address = REG_ACC_OUT_Y_L_ADDR, + .modified = 1, + .channel2 = IIO_MOD_Y, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | + BIT(IIO_CHAN_INFO_SCALE), + .scan_index = 1, + .scan_type = { + .sign = 's', + .realbits = 16, + .storagebits = 16, + .shift = 0, + .endianness = IIO_LE, + }, + }, + { + .type = IIO_ACCEL, + .address = REG_ACC_OUT_Z_L_ADDR, + .modified = 1, + .channel2 = IIO_MOD_Z, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | + BIT(IIO_CHAN_INFO_SCALE), + .scan_index = 2, + .scan_type = { + .sign = 's', + .realbits = 16, + .storagebits = 16, + .shift = 0, + .endianness = IIO_LE, + }, + }, + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static const struct iio_chan_spec st_lsm6dsx_gyro_channels[] = { + { + .type = IIO_ANGL_VEL, + .address = REG_GYRO_OUT_X_L_ADDR, + .modified = 1, + .channel2 = IIO_MOD_X, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | + BIT(IIO_CHAN_INFO_SCALE), + .scan_index = 0, + .scan_type = { + .sign = 's', + .realbits = 16, + .storagebits = 16, + .shift = 0, + .endianness = IIO_LE, + }, + }, + { + .type = IIO_ANGL_VEL, + .address = REG_GYRO_OUT_Y_L_ADDR, + .modified = 1, + .channel2 = IIO_MOD_Y, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | + BIT(IIO_CHAN_INFO_SCALE), + .scan_index = 1, + .scan_type = { + .sign = 's', + .realbits = 16, + .storagebits = 16, + .shift = 0, + .endianness = IIO_LE, + }, + }, + { + .type = IIO_ANGL_VEL, + .address = REG_GYRO_OUT_Z_L_ADDR, + .modified = 1, + .channel2 = IIO_MOD_Z, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | + BIT(IIO_CHAN_INFO_SCALE), + .scan_index = 2, + .scan_type = { + .sign = 's', + .realbits = 16, + .storagebits = 16, + .shift = 0, + .endianness = IIO_LE, + }, + }, + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +int st_lsm6dsx_write_with_mask(struct st_lsm6dsx_dev *dev, u8 addr, u8 mask, + u8 val) +{ + u8 data; + int err; + + mutex_lock(&dev->lock); + + err = dev->tf->read(dev->dev, addr, 1, &data); + if (err < 0) { + dev_err(dev->dev, "failed to read %02x register\n", addr); + mutex_unlock(&dev->lock); + + return err; + } + + data = (data & ~mask) | ((val << __ffs(mask)) & mask); + + err = dev->tf->write(dev->dev, addr, 1, &data); + if (err < 0) { + dev_err(dev->dev, "failed to write %02x register\n", addr); + mutex_unlock(&dev->lock); + + return err; + } + + mutex_unlock(&dev->lock); + + return 0; +} + +static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_dev *dev) +{ + u8 data; + int err; + + err = dev->tf->read(dev->dev, REG_WHOAMI_ADDR, 1, &data); + if (err < 0) { + dev_err(dev->dev, "failed to read whoami register\n"); + return err; + } + + if (data != ST_LSM6DS3_WHOAMI && + data != ST_LSM6DSM_WHOAMI) { + dev_err(dev->dev, "wrong whoami [%02x]\n", data); + return -ENODEV; + } + + return 0; +} + +static int st_lsm6dsx_set_fs(struct st_lsm6dsx_sensor *sensor, u32 gain) +{ + u8 val; + int i, err; + enum st_lsm6dsx_sensor_id id = sensor->id; + + for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++) + if (st_lsm6dsx_fs_table[id].fs_avl[i].gain == gain) + break; + + if (i == ST_LSM6DSX_FS_LIST_SIZE) + return -EINVAL; + + val = st_lsm6dsx_fs_table[id].fs_avl[i].val; + err = st_lsm6dsx_write_with_mask(sensor->dev, + st_lsm6dsx_fs_table[id].addr, + st_lsm6dsx_fs_table[id].mask, val); + if (err < 0) + return err; + + sensor->gain = gain; + + return 0; +} + +static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr) +{ + u8 val; + int i, err; + enum st_lsm6dsx_sensor_id id = sensor->id; + struct st_lsm6dsx_dev *dev = sensor->dev; + + for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++) + if (st_lsm6dsx_odr_table[id].odr_avl[i].hz == odr) + break; + + if (i == ST_LSM6DSX_ODR_LIST_SIZE) + return -EINVAL; + + disable_irq(dev->irq); + + val = st_lsm6dsx_odr_table[id].odr_avl[i].val; + err = st_lsm6dsx_write_with_mask(sensor->dev, + st_lsm6dsx_odr_table[id].addr, + st_lsm6dsx_odr_table[id].mask, val); + if (err < 0) { + enable_irq(dev->irq); + return err; + } + + sensor->odr = odr; + enable_irq(dev->irq); + + return 0; +} + +int st_lsm6dsx_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable) +{ + enum st_lsm6dsx_sensor_id id = sensor->id; + + if (enable) + return st_lsm6dsx_set_odr(sensor, sensor->odr); + else + return st_lsm6dsx_write_with_mask(sensor->dev, + st_lsm6dsx_odr_table[id].addr, + st_lsm6dsx_odr_table[id].mask, 0); +} + +static int st_lsm6dsx_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + + switch (mask) { + case IIO_CHAN_INFO_RAW: { + int err; + u8 data[2]; + struct st_lsm6dsx_dev *dev; + + mutex_lock(&iio_dev->mlock); + + if (iio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&iio_dev->mlock); + return -EBUSY; + } + + dev = sensor->dev; + err = st_lsm6dsx_set_enable(sensor, true); + if (err < 0) { + mutex_unlock(&iio_dev->mlock); + return err; + } + + msleep(120); + + err = dev->tf->read(dev->dev, ch->address, 2, data); + if (err < 0) { + mutex_unlock(&iio_dev->mlock); + return err; + } + + st_lsm6dsx_set_enable(sensor, false); + + *val = (s16)get_unaligned_le16(data); + *val = *val >> ch->scan_type.shift; + + mutex_unlock(&iio_dev->mlock); + + return IIO_VAL_INT; + } + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sensor->gain; + return IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } + + return 0; +} + +static int st_lsm6dsx_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + int err; + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + mutex_lock(&iio_dev->mlock); + + if (iio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { + mutex_unlock(&iio_dev->mlock); + return -EBUSY; + } + err = st_lsm6dsx_set_fs(sensor, val2); + + mutex_unlock(&iio_dev->mlock); + break; + default: + return -EINVAL; + } + + return err < 0 ? err : 0; +} + +static ssize_t +st_lsm6dsx_sysfs_get_sampling_frequency(struct device *device, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(device)); + + return sprintf(buf, "%d\n", sensor->odr); +} + +static ssize_t +st_lsm6dsx_sysfs_set_sampling_frequency(struct device *device, + struct device_attribute *attr, + const char *buf, size_t size) +{ + int err; + u32 odr; + struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(device)); + + err = kstrtoint(buf, 10, &odr); + if (err < 0) + return err; + + err = st_lsm6dsx_set_odr(sensor, odr); + + return err < 0 ? err : size; +} + +static ssize_t +st_lsm6dsx_sysfs_sampling_frequency_avl(struct device *device, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0; + struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(device)); + enum st_lsm6dsx_sensor_id id = sensor->id; + + for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + st_lsm6dsx_odr_table[id].odr_avl[i].hz); + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_lsm6dsx_sysfs_scale_avail(struct device *device, + struct device_attribute *attr, + char *buf) +{ + int i, len = 0; + struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(device)); + enum st_lsm6dsx_sensor_id id = sensor->id; + + for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", + st_lsm6dsx_fs_table[id].fs_avl[i].gain); + buf[len - 1] = '\n'; + + return len; +} + +static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, + st_lsm6dsx_sysfs_get_sampling_frequency, + st_lsm6dsx_sysfs_set_sampling_frequency); +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_sysfs_sampling_frequency_avl); +static IIO_DEVICE_ATTR(in_accel_scale_available, S_IRUGO, + st_lsm6dsx_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(in_anglvel_scale_available, S_IRUGO, + st_lsm6dsx_sysfs_scale_avail, NULL, 0); + +static struct attribute *st_lsm6dsx_acc_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_in_accel_scale_available.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsx_acc_attribute_group = { + .attrs = st_lsm6dsx_acc_attributes, +}; + +static const struct iio_info st_lsm6dsx_acc_info = { + .driver_module = THIS_MODULE, + .attrs = &st_lsm6dsx_acc_attribute_group, + .read_raw = st_lsm6dsx_read_raw, + .write_raw = st_lsm6dsx_write_raw, +}; + +static struct attribute *st_lsm6dsx_gyro_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_in_anglvel_scale_available.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsx_gyro_attribute_group = { + .attrs = st_lsm6dsx_gyro_attributes, +}; + +static const struct iio_info st_lsm6dsx_gyro_info = { + .driver_module = THIS_MODULE, + .attrs = &st_lsm6dsx_gyro_attribute_group, + .read_raw = st_lsm6dsx_read_raw, + .write_raw = st_lsm6dsx_write_raw, +}; + +static int st_lsm6dsx_init_device(struct st_lsm6dsx_dev *dev) +{ + int err; + u8 data; + + data = REG_RESET_MASK; + err = dev->tf->write(dev->dev, REG_RESET_ADDR, 1, &data); + if (err < 0) + return err; + + msleep(200); + + /* latch interrupts */ + err = st_lsm6dsx_write_with_mask(dev, REG_LIR_ADDR, REG_LIR_MASK, 1); + if (err < 0) + return err; + + /* enable BDU */ + err = st_lsm6dsx_write_with_mask(dev, REG_BDU_ADDR, REG_BDU_MASK, 1); + if (err < 0) + return err; + + err = st_lsm6dsx_write_with_mask(dev, REG_ROUNDING_ADDR, + REG_ROUNDING_MASK, 1); + if (err < 0) + return err; + + /* redirect INT2 on INT1 */ + err = st_lsm6dsx_write_with_mask(dev, REG_INT2_ON_INT1_ADDR, + REG_INT2_ON_INT1_MASK, 1); + if (err < 0) + return err; + + return 0; +} + +static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_dev *dev, + enum st_lsm6dsx_sensor_id id) +{ + struct iio_dev *iio_dev; + struct st_lsm6dsx_sensor *sensor; + + iio_dev = iio_device_alloc(sizeof(*sensor)); + if (!iio_dev) + return NULL; + + iio_dev->modes = INDIO_DIRECT_MODE; + iio_dev->dev.parent = dev->dev; + + sensor = iio_priv(iio_dev); + sensor->id = id; + sensor->dev = dev; + sensor->odr = st_lsm6dsx_odr_table[id].odr_avl[0].hz; + sensor->gain = st_lsm6dsx_fs_table[id].fs_avl[0].gain; + + switch (id) { + case ST_LSM6DSX_ID_ACC: + iio_dev->channels = st_lsm6dsx_acc_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsx_acc_channels); + iio_dev->name = "lsm6dsx_accel"; + iio_dev->info = &st_lsm6dsx_acc_info; + + sensor->drdy_data_mask = ST_LSM6DSX_DATA_ACC_AVL_MASK; + sensor->drdy_irq_mask = REG_ACC_DRDY_IRQ_MASK; + break; + case ST_LSM6DSX_ID_GYRO: + iio_dev->channels = st_lsm6dsx_gyro_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsx_gyro_channels); + iio_dev->name = "lsm6dsx_gyro"; + iio_dev->info = &st_lsm6dsx_gyro_info; + + sensor->drdy_data_mask = ST_LSM6DSX_DATA_GYRO_AVL_MASK; + sensor->drdy_irq_mask = REG_GYRO_DRDY_IRQ_MASK; + break; + default: + iio_device_free(iio_dev); + return NULL; + } + + return iio_dev; +} + +int st_lsm6dsx_probe(struct st_lsm6dsx_dev *dev) +{ + int i, j, err; + + mutex_init(&dev->lock); + + err = st_lsm6dsx_check_whoami(dev); + if (err < 0) + return err; + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + dev->iio_devs[i] = st_lsm6dsx_alloc_iiodev(dev, i); + if (!dev->iio_devs[i]) { + err = -ENOMEM; + goto iio_device_free; + } + } + + err = st_lsm6dsx_init_device(dev); + if (err < 0) + goto iio_device_free; + + if (dev->irq > 0) { + err = st_lsm6dsx_allocate_buffers(dev); + if (err < 0) + goto iio_device_free; + + err = st_lsm6dsx_allocate_triggers(dev); + if (err < 0) + goto deallocate_buffers; + } + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + err = iio_device_register(dev->iio_devs[i]); + if (err) + goto iio_device_unregister; + } + + return 0; + +iio_device_unregister: + for (j = i - 1; j >= 0; j--) + iio_device_unregister(dev->iio_devs[i]); + if (dev->irq > 0) + st_lsm6dsx_deallocate_triggers(dev); +deallocate_buffers: + if (dev->irq > 0) + st_lsm6dsx_deallocate_buffers(dev); +iio_device_free: + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) + if (dev->iio_devs[i]) + iio_device_free(dev->iio_devs[i]); + + return err; +} +EXPORT_SYMBOL(st_lsm6dsx_probe); + +int st_lsm6dsx_remove(struct st_lsm6dsx_dev *dev) +{ + int i; + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) + iio_device_unregister(dev->iio_devs[i]); + + if (dev->irq > 0) { + st_lsm6dsx_deallocate_triggers(dev); + st_lsm6dsx_deallocate_buffers(dev); + } + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) + iio_device_free(dev->iio_devs[i]); + + return 0; +} +EXPORT_SYMBOL(st_lsm6dsx_remove); + +MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@xxxxxx>"); +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@xxxxxx>"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c new file mode 100644 index 0000000..88b6794 --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c @@ -0,0 +1,137 @@ +/* + * STMicroelectronics st_lsm6dsx i2c driver + * + * Copyright 2016 STMicroelectronics Inc. + * + * Lorenzo Bianconi <lorenzo.bianconi@xxxxxx> + * Denis Ciocca <denis.ciocca@xxxxxx> + * + * Licensed under the GPL-2. + */ + +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/i2c.h> +#include <linux/slab.h> +#include "st_lsm6dsx.h" + +static int st_lsm6dsx_i2c_read(struct device *dev, u8 addr, int len, u8 *data) +{ + struct i2c_msg msg[2]; + struct i2c_client *client = to_i2c_client(dev); + + msg[0].addr = client->addr; + msg[0].flags = client->flags; + msg[0].len = 1; + msg[0].buf = &addr; + + msg[1].addr = client->addr; + msg[1].flags = client->flags | I2C_M_RD; + msg[1].len = len; + msg[1].buf = data; + + return i2c_transfer(client->adapter, msg, 2); +} + +static int st_lsm6dsx_i2c_write(struct device *dev, u8 addr, int len, u8 *data) +{ + u8 send[len + 1]; + struct i2c_msg msg; + struct i2c_client *client = to_i2c_client(dev); + + send[0] = addr; + memcpy(&send[1], data, len * sizeof(u8)); + + msg.addr = client->addr; + msg.flags = client->flags; + msg.len = len + 1; + msg.buf = send; + + return i2c_transfer(client->adapter, &msg, 1); +} + +static const struct st_lsm6dsx_transfer_function st_lsm6dsx_transfer_fn = { + .read = st_lsm6dsx_i2c_read, + .write = st_lsm6dsx_i2c_write, +}; + +static int st_lsm6dsx_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + int err; + struct st_lsm6dsx_dev *dev; + + dev = kzalloc(sizeof(*dev), GFP_KERNEL); + if (!dev) + return -ENOMEM; + + i2c_set_clientdata(client, dev); + dev->name = client->name; + dev->dev = &client->dev; + dev->irq = client->irq; + dev->tf = &st_lsm6dsx_transfer_fn; + + err = st_lsm6dsx_probe(dev); + if (err < 0) { + kfree(dev); + return err; + } + + dev_info(&client->dev, "sensor probed\n"); + + return 0; +} + +static int st_lsm6dsx_i2c_remove(struct i2c_client *client) +{ + int err; + struct st_lsm6dsx_dev *dev = i2c_get_clientdata(client); + + err = st_lsm6dsx_remove(dev); + if (err < 0) + return err; + + dev_info(&client->dev, "sensor removed\n"); + + return 0; +} + +#ifdef CONFIG_OF +static const struct of_device_id st_lsm6dsx_i2c_of_match[] = { + { + .compatible = "st,lsm6ds3", + .data = ST_LSM6DS3_DEV_NAME, + }, + { + .compatible = "st,lsm6dsm", + .data = ST_LSM6DSM_DEV_NAME, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lsm6dsx_i2c_of_match); +#endif /* CONFIG_OF */ + +static const struct i2c_device_id st_lsm6dsx_i2c_id_table[] = { + { ST_LSM6DS3_DEV_NAME }, + { ST_LSM6DSM_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(i2c, st_lsm6dsx_i2c_id_table); + +static struct i2c_driver st_lsm6dsx_driver = { + .driver = { + .name = "st_lsm6dsx_i2c", +#ifdef CONFIG_OF + .of_match_table = st_lsm6dsx_i2c_of_match, +#endif /* CONFIG_OF */ + }, + .probe = st_lsm6dsx_i2c_probe, + .remove = st_lsm6dsx_i2c_remove, + .id_table = st_lsm6dsx_i2c_id_table, +}; +module_i2c_driver(st_lsm6dsx_driver); + +MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@xxxxxx>"); +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@xxxxxx>"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c new file mode 100644 index 0000000..b5042b2 --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c @@ -0,0 +1,155 @@ +/* + * STMicroelectronics st_lsm6dsx spi driver + * + * Copyright 2016 STMicroelectronics Inc. + * + * Lorenzo Bianconi <lorenzo.bianconi@xxxxxx> + * Denis Ciocca <denis.ciocca@xxxxxx> + * + * Licensed under the GPL-2. + */ + +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/spi/spi.h> +#include <linux/slab.h> +#include "st_lsm6dsx.h" + +#define SENSORS_SPI_READ 0x80 + +static int st_lsm6dsx_spi_read(struct device *device, u8 addr, int len, + u8 *data) +{ + int err; + struct spi_device *spi = to_spi_device(device); + struct st_lsm6dsx_dev *dev = spi_get_drvdata(spi); + + struct spi_transfer xfers[] = { + { + .tx_buf = dev->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = dev->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + + dev->tb.tx_buf[0] = addr | SENSORS_SPI_READ; + + err = spi_sync_transfer(spi, xfers, ARRAY_SIZE(xfers)); + if (err < 0) + return err; + + memcpy(data, dev->tb.rx_buf, len * sizeof(u8)); + + return len; +} + +static int st_lsm6dsx_spi_write(struct device *device, u8 addr, int len, + u8 *data) +{ + struct spi_device *spi = to_spi_device(device); + struct st_lsm6dsx_dev *dev = spi_get_drvdata(spi); + + struct spi_transfer xfers = { + .tx_buf = dev->tb.tx_buf, + .bits_per_word = 8, + .len = len + 1, + }; + + if (len >= ST_LSM6DSX_TX_MAX_LENGTH) + return -ENOMEM; + + dev->tb.tx_buf[0] = addr; + memcpy(&dev->tb.tx_buf[1], data, len); + + return spi_sync_transfer(spi, &xfers, 1); +} + +static const struct st_lsm6dsx_transfer_function st_lsm6dsx_transfer_fn = { + .read = st_lsm6dsx_spi_read, + .write = st_lsm6dsx_spi_write, +}; + +static int st_lsm6dsx_spi_probe(struct spi_device *spi) +{ + int err; + struct st_lsm6dsx_dev *dev; + + dev = kzalloc(sizeof(*dev), GFP_KERNEL); + if (!dev) + return -ENOMEM; + + spi_set_drvdata(spi, dev); + dev->name = spi->modalias; + dev->dev = &spi->dev; + dev->irq = spi->irq; + dev->tf = &st_lsm6dsx_transfer_fn; + + err = st_lsm6dsx_probe(dev); + if (err < 0) { + kfree(dev); + return err; + } + + dev_info(&spi->dev, "sensor probed\n"); + + return 0; +} + +static int st_lsm6dsx_spi_remove(struct spi_device *spi) +{ + int err; + struct st_lsm6dsx_dev *dev = spi_get_drvdata(spi); + + err = st_lsm6dsx_remove(dev); + if (err < 0) + return err; + + dev_info(&spi->dev, "sensor removed\n"); + + return 0; +} + +#ifdef CONFIG_OF +static const struct of_device_id st_lsm6dsx_spi_of_match[] = { + { + .compatible = "st,lsm6ds3", + .data = ST_LSM6DS3_DEV_NAME, + }, + { + .compatible = "st,lsm6dsm", + .data = ST_LSM6DSM_DEV_NAME, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lsm6dsx_spi_of_match); +#endif /* CONFIG_OF */ + +static const struct spi_device_id st_lsm6dsx_spi_id_table[] = { + { ST_LSM6DS3_DEV_NAME }, + { ST_LSM6DSM_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(spi, st_lsm6dsx_spi_id_table); + +static struct spi_driver st_lsm6dsx_driver = { + .driver = { + .name = "st_lsm6dsx_spi", +#ifdef CONFIG_OF + .of_match_table = st_lsm6dsx_spi_of_match, +#endif /* CONFIG_OF */ + }, + .probe = st_lsm6dsx_spi_probe, + .remove = st_lsm6dsx_spi_remove, + .id_table = st_lsm6dsx_spi_id_table, +}; +module_spi_driver(st_lsm6dsx_driver); + +MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@xxxxxx>"); +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@xxxxxx>"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx spi driver"); +MODULE_LICENSE("GPL v2"); -- 2.7.4 -- To unsubscribe from this list: send the line "unsubscribe linux-iio" in the body of a message to majordomo@xxxxxxxxxxxxxxx More majordomo info at http://vger.kernel.org/majordomo-info.html
