diff options
Diffstat (limited to 'drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c')
| -rw-r--r-- | drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 213 |
1 files changed, 121 insertions, 92 deletions
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c index b485540da89e..c2fa3239b9c6 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c @@ -37,13 +37,13 @@ #include <linux/iio/iio.h> #include <linux/iio/sysfs.h> #include <linux/pm.h> +#include <linux/regmap.h> +#include <linux/bitfield.h> #include <linux/platform_data/st_sensors_pdata.h> #include "st_lsm6dsx.h" -#define ST_LSM6DSX_REG_ACC_DEC_MASK GENMASK(2, 0) -#define ST_LSM6DSX_REG_GYRO_DEC_MASK GENMASK(5, 3) #define ST_LSM6DSX_REG_INT1_ADDR 0x0d #define ST_LSM6DSX_REG_INT2_ADDR 0x0e #define ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK BIT(3) @@ -54,10 +54,6 @@ #define ST_LSM6DSX_REG_BDU_MASK BIT(6) #define ST_LSM6DSX_REG_INT2_ON_INT1_ADDR 0x13 #define ST_LSM6DSX_REG_INT2_ON_INT1_MASK BIT(5) -#define ST_LSM6DSX_REG_ROUNDING_ADDR 0x16 -#define ST_LSM6DSX_REG_ROUNDING_MASK BIT(2) -#define ST_LSM6DSX_REG_LIR_ADDR 0x58 -#define ST_LSM6DSX_REG_LIR_MASK BIT(0) #define ST_LSM6DSX_REG_ACC_ODR_ADDR 0x10 #define ST_LSM6DSX_REG_ACC_ODR_MASK GENMASK(7, 4) @@ -160,25 +156,88 @@ static const struct st_lsm6dsx_fs_table_entry st_lsm6dsx_fs_table[] = { static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = { { .wai = 0x69, - .max_fifo_size = 8192, + .max_fifo_size = 1365, .id = { [0] = ST_LSM6DS3_ID, }, + .decimator = { + [ST_LSM6DSX_ID_ACC] = { + .addr = 0x08, + .mask = GENMASK(2, 0), + }, + [ST_LSM6DSX_ID_GYRO] = { + .addr = 0x08, + .mask = GENMASK(5, 3), + }, + }, + .fifo_ops = { + .fifo_th = { + .addr = 0x06, + .mask = GENMASK(11, 0), + }, + .fifo_diff = { + .addr = 0x3a, + .mask = GENMASK(11, 0), + }, + .th_wl = 3, /* 1LSB = 2B */ + }, }, { .wai = 0x69, - .max_fifo_size = 4096, + .max_fifo_size = 682, .id = { [0] = ST_LSM6DS3H_ID, }, + .decimator = { + [ST_LSM6DSX_ID_ACC] = { + .addr = 0x08, + .mask = GENMASK(2, 0), + }, + [ST_LSM6DSX_ID_GYRO] = { + .addr = 0x08, + .mask = GENMASK(5, 3), + }, + }, + .fifo_ops = { + .fifo_th = { + .addr = 0x06, + .mask = GENMASK(11, 0), + }, + .fifo_diff = { + .addr = 0x3a, + .mask = GENMASK(11, 0), + }, + .th_wl = 3, /* 1LSB = 2B */ + }, }, { .wai = 0x6a, - .max_fifo_size = 4096, + .max_fifo_size = 682, .id = { [0] = ST_LSM6DSL_ID, [1] = ST_LSM6DSM_ID, }, + .decimator = { + [ST_LSM6DSX_ID_ACC] = { + .addr = 0x08, + .mask = GENMASK(2, 0), + }, + [ST_LSM6DSX_ID_GYRO] = { + .addr = 0x08, + .mask = GENMASK(5, 3), + }, + }, + .fifo_ops = { + .fifo_th = { + .addr = 0x06, + .mask = GENMASK(11, 0), + }, + .fifo_diff = { + .addr = 0x3a, + .mask = GENMASK(11, 0), + }, + .th_wl = 3, /* 1LSB = 2B */ + }, }, }; @@ -220,36 +279,9 @@ static const struct iio_chan_spec st_lsm6dsx_gyro_channels[] = { IIO_CHAN_SOFT_TIMESTAMP(3), }; -int st_lsm6dsx_write_with_mask(struct st_lsm6dsx_hw *hw, u8 addr, u8 mask, - u8 val) -{ - u8 data; - int err; - - mutex_lock(&hw->lock); - - err = hw->tf->read(hw->dev, addr, sizeof(data), &data); - if (err < 0) { - dev_err(hw->dev, "failed to read %02x register\n", addr); - goto out; - } - - data = (data & ~mask) | ((val << __ffs(mask)) & mask); - - err = hw->tf->write(hw->dev, addr, sizeof(data), &data); - if (err < 0) - dev_err(hw->dev, "failed to write %02x register\n", addr); - -out: - mutex_unlock(&hw->lock); - - return err; -} - static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw, int id) { - int err, i, j; - u8 data; + int err, i, j, data; for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) { for (j = 0; j < ST_LSM6DSX_MAX_ID; j++) { @@ -265,8 +297,7 @@ static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw, int id) return -ENODEV; } - err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_WHOAMI_ADDR, sizeof(data), - &data); + err = regmap_read(hw->regmap, ST_LSM6DSX_REG_WHOAMI_ADDR, &data); if (err < 0) { dev_err(hw->dev, "failed to read whoami register\n"); return err; @@ -285,22 +316,22 @@ static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw, int id) static int st_lsm6dsx_set_full_scale(struct st_lsm6dsx_sensor *sensor, u32 gain) { - enum st_lsm6dsx_sensor_id id = sensor->id; + struct st_lsm6dsx_hw *hw = sensor->hw; + const struct st_lsm6dsx_reg *reg; int i, err; u8 val; for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++) - if (st_lsm6dsx_fs_table[id].fs_avl[i].gain == gain) + if (st_lsm6dsx_fs_table[sensor->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->hw, - st_lsm6dsx_fs_table[id].reg.addr, - st_lsm6dsx_fs_table[id].reg.mask, - val); + val = st_lsm6dsx_fs_table[sensor->id].fs_avl[i].val; + reg = &st_lsm6dsx_fs_table[sensor->id].reg; + err = regmap_update_bits(hw->regmap, reg->addr, reg->mask, + ST_LSM6DSX_SHIFT_VAL(val, reg->mask)); if (err < 0) return err; @@ -322,14 +353,14 @@ static int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, u16 odr, return -EINVAL; *val = st_lsm6dsx_odr_table[sensor->id].odr_avl[i].val; - sensor->odr = odr; return 0; } static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr) { - enum st_lsm6dsx_sensor_id id = sensor->id; + struct st_lsm6dsx_hw *hw = sensor->hw; + const struct st_lsm6dsx_reg *reg; int err; u8 val; @@ -337,10 +368,9 @@ static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr) if (err < 0) return err; - return st_lsm6dsx_write_with_mask(sensor->hw, - st_lsm6dsx_odr_table[id].reg.addr, - st_lsm6dsx_odr_table[id].reg.mask, - val); + reg = &st_lsm6dsx_odr_table[sensor->id].reg; + return regmap_update_bits(hw->regmap, reg->addr, reg->mask, + ST_LSM6DSX_SHIFT_VAL(val, reg->mask)); } int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor) @@ -358,16 +388,17 @@ int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor) int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor) { - enum st_lsm6dsx_sensor_id id = sensor->id; + struct st_lsm6dsx_hw *hw = sensor->hw; + const struct st_lsm6dsx_reg *reg; int err; - err = st_lsm6dsx_write_with_mask(sensor->hw, - st_lsm6dsx_odr_table[id].reg.addr, - st_lsm6dsx_odr_table[id].reg.mask, 0); + reg = &st_lsm6dsx_odr_table[sensor->id].reg; + err = regmap_update_bits(hw->regmap, reg->addr, reg->mask, + ST_LSM6DSX_SHIFT_VAL(0, reg->mask)); if (err < 0) return err; - sensor->hw->enable_mask &= ~BIT(id); + sensor->hw->enable_mask &= ~BIT(sensor->id); return 0; } @@ -375,6 +406,7 @@ int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor) static int st_lsm6dsx_read_oneshot(struct st_lsm6dsx_sensor *sensor, u8 addr, int *val) { + struct st_lsm6dsx_hw *hw = sensor->hw; int err, delay; __le16 data; @@ -385,14 +417,13 @@ static int st_lsm6dsx_read_oneshot(struct st_lsm6dsx_sensor *sensor, delay = 1000000 / sensor->odr; usleep_range(delay, 2 * delay); - err = sensor->hw->tf->read(sensor->hw->dev, addr, sizeof(data), - (u8 *)&data); + err = regmap_bulk_read(hw->regmap, addr, &data, sizeof(data)); if (err < 0) return err; st_lsm6dsx_sensor_disable(sensor); - *val = (s16)data; + *val = (s16)le16_to_cpu(data); return IIO_VAL_INT; } @@ -449,6 +480,8 @@ static int st_lsm6dsx_write_raw(struct iio_dev *iio_dev, u8 data; err = st_lsm6dsx_check_odr(sensor, val, &data); + if (!err) + sensor->odr = val; break; } default: @@ -465,13 +498,17 @@ static int st_lsm6dsx_set_watermark(struct iio_dev *iio_dev, unsigned int val) { struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); struct st_lsm6dsx_hw *hw = sensor->hw; - int err, max_fifo_len; + int err; - max_fifo_len = hw->settings->max_fifo_size / ST_LSM6DSX_SAMPLE_SIZE; - if (val < 1 || val > max_fifo_len) + if (val < 1 || val > hw->settings->max_fifo_size) return -EINVAL; + mutex_lock(&hw->conf_lock); + err = st_lsm6dsx_update_watermark(sensor, val); + + mutex_unlock(&hw->conf_lock); + if (err < 0) return err; @@ -530,7 +567,6 @@ static const struct attribute_group st_lsm6dsx_acc_attribute_group = { }; 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, @@ -548,7 +584,6 @@ static const struct attribute_group st_lsm6dsx_gyro_attribute_group = { }; 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, @@ -597,31 +632,20 @@ static int st_lsm6dsx_get_drdy_reg(struct st_lsm6dsx_hw *hw, u8 *drdy_reg) static int st_lsm6dsx_init_device(struct st_lsm6dsx_hw *hw) { - u8 data, drdy_int_reg; + u8 drdy_int_reg; int err; - data = ST_LSM6DSX_REG_RESET_MASK; - err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_RESET_ADDR, sizeof(data), - &data); + err = regmap_write(hw->regmap, ST_LSM6DSX_REG_RESET_ADDR, + ST_LSM6DSX_REG_RESET_MASK); if (err < 0) return err; msleep(200); - /* latch interrupts */ - err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_LIR_ADDR, - ST_LSM6DSX_REG_LIR_MASK, 1); - if (err < 0) - return err; - /* enable Block Data Update */ - err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_BDU_ADDR, - ST_LSM6DSX_REG_BDU_MASK, 1); - if (err < 0) - return err; - - err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_ROUNDING_ADDR, - ST_LSM6DSX_REG_ROUNDING_MASK, 1); + err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_BDU_ADDR, + ST_LSM6DSX_REG_BDU_MASK, + FIELD_PREP(ST_LSM6DSX_REG_BDU_MASK, 1)); if (err < 0) return err; @@ -630,8 +654,10 @@ static int st_lsm6dsx_init_device(struct st_lsm6dsx_hw *hw) if (err < 0) return err; - return st_lsm6dsx_write_with_mask(hw, drdy_int_reg, - ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK, 1); + return regmap_update_bits(hw->regmap, drdy_int_reg, + ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK, + FIELD_PREP(ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK, + 1)); } static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw, @@ -662,7 +688,6 @@ static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw, iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsx_acc_channels); iio_dev->info = &st_lsm6dsx_acc_info; - sensor->decimator_mask = ST_LSM6DSX_REG_ACC_DEC_MASK; scnprintf(sensor->name, sizeof(sensor->name), "%s_accel", name); break; @@ -671,7 +696,6 @@ static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw, iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsx_gyro_channels); iio_dev->info = &st_lsm6dsx_gyro_info; - sensor->decimator_mask = ST_LSM6DSX_REG_GYRO_DEC_MASK; scnprintf(sensor->name, sizeof(sensor->name), "%s_gyro", name); break; @@ -684,7 +708,7 @@ static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw, } int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name, - const struct st_lsm6dsx_transfer_function *tf_ops) + struct regmap *regmap) { struct st_lsm6dsx_hw *hw; int i, err; @@ -695,12 +719,16 @@ int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name, dev_set_drvdata(dev, (void *)hw); - mutex_init(&hw->lock); mutex_init(&hw->fifo_lock); + mutex_init(&hw->conf_lock); + + hw->buff = devm_kzalloc(dev, ST_LSM6DSX_BUFF_SIZE, GFP_KERNEL); + if (!hw->buff) + return -ENOMEM; hw->dev = dev; hw->irq = irq; - hw->tf = tf_ops; + hw->regmap = regmap; err = st_lsm6dsx_check_whoami(hw, hw_id); if (err < 0) @@ -736,6 +764,7 @@ static int __maybe_unused st_lsm6dsx_suspend(struct device *dev) { struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev); struct st_lsm6dsx_sensor *sensor; + const struct st_lsm6dsx_reg *reg; int i, err = 0; for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { @@ -743,9 +772,9 @@ static int __maybe_unused st_lsm6dsx_suspend(struct device *dev) if (!(hw->enable_mask & BIT(sensor->id))) continue; - err = st_lsm6dsx_write_with_mask(hw, - st_lsm6dsx_odr_table[sensor->id].reg.addr, - st_lsm6dsx_odr_table[sensor->id].reg.mask, 0); + reg = &st_lsm6dsx_odr_table[sensor->id].reg; + err = regmap_update_bits(hw->regmap, reg->addr, reg->mask, + ST_LSM6DSX_SHIFT_VAL(0, reg->mask)); if (err < 0) return err; } |