aboutsummaryrefslogtreecommitdiffstats
path: root/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c')
-rw-r--r--drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c115
1 files changed, 51 insertions, 64 deletions
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index 239c735242be..c2fa3239b9c6 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -37,6 +37,8 @@
#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>
@@ -277,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++) {
@@ -322,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;
@@ -342,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;
@@ -385,7 +359,8 @@ static int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, u16 odr,
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;
@@ -393,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)
@@ -414,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;
}
@@ -431,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;
@@ -441,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;
}
@@ -528,7 +503,12 @@ static int st_lsm6dsx_set_watermark(struct iio_dev *iio_dev, unsigned int val)
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;
@@ -652,20 +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);
/* enable Block Data Update */
- err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_BDU_ADDR,
- ST_LSM6DSX_REG_BDU_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;
@@ -674,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,
@@ -726,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;
@@ -737,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)
@@ -778,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++) {
@@ -785,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;
}
Copyright © 1996 – 2023 Jason A. Donenfeld. All Rights Reverse Engineered.