iv/linux
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

iio: imu: st_lsm6dsx: add regmap API support

Browse Source 
Introduce regmap API support to access to i2c/spi bus instead of
using a custom support. Set max bulk read to
(32 / SAMPLE_SIZE) * SAMPLE_SIZE since spi_write_then_read() used in
regmap_spi indicates that is the max buffer length to use in order to
avoid a kmalloc for each bus access.
Remove lock mutex since concurrency is already managed by regmap API

Signed-off-by: Lorenzo Bianconi <lorenzo.bianconi@redhat.com>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>
This commit is contained in:
Lorenzo Bianconi 2018-01-01 19:54:44 +01:00 committed by Greg Kroah-Hartman
parent 335eaedce4
commit 51a8b70762
6 changed files with 131 additions and 211 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
drivers/iio/imu/st_lsm6dsx/Kconfig
View File

@ -16,7 +16,9 @@ config IIO_ST_LSM6DSX
config IIO_ST_LSM6DSX_I2C config IIO_ST_LSM6DSX_I2C
tristate tristate
depends on IIO_ST_LSM6DSX depends on IIO_ST_LSM6DSX
select REGMAP_I2C
config IIO_ST_LSM6DSX_SPI config IIO_ST_LSM6DSX_SPI
tristate tristate
depends on IIO_ST_LSM6DSX depends on IIO_ST_LSM6DSX
select REGMAP_SPI

31
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
View File

@ -29,21 +29,9 @@ enum st_lsm6dsx_hw_id {
#define ST_LSM6DSX_CHAN_SIZE 2 #define ST_LSM6DSX_CHAN_SIZE 2
#define ST_LSM6DSX_SAMPLE_SIZE 6 #define ST_LSM6DSX_SAMPLE_SIZE 6
#define ST_LSM6DSX_MAX_WORD_LEN ((32 / ST_LSM6DSX_SAMPLE_SIZE) * \
#if defined(CONFIG_SPI_MASTER) ST_LSM6DSX_SAMPLE_SIZE)
#define ST_LSM6DSX_RX_MAX_LENGTH 256 #define ST_LSM6DSX_SHIFT_VAL(val, mask) (((val) << __ffs(mask)) & (mask))
#define ST_LSM6DSX_TX_MAX_LENGTH 8
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_SPI_MASTER */
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);
};
struct st_lsm6dsx_reg { struct st_lsm6dsx_reg {
u8 addr; u8 addr;
@ -127,8 +115,8 @@ struct st_lsm6dsx_sensor {
/** /**
* struct st_lsm6dsx_hw - ST IMU MEMS hw instance * struct st_lsm6dsx_hw - ST IMU MEMS hw instance
* @dev: Pointer to instance of struct device (I2C or SPI). * @dev: Pointer to instance of struct device (I2C or SPI).
* @regmap: Register map of the device.
* @irq: Device interrupt line (I2C or SPI). * @irq: Device interrupt line (I2C or SPI).
* @lock: Mutex to protect read and write operations.
* @fifo_lock: Mutex to prevent concurrent access to the hw FIFO. * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO.
* @conf_lock: Mutex to prevent concurrent FIFO configuration update. * @conf_lock: Mutex to prevent concurrent FIFO configuration update.
* @fifo_mode: FIFO operating mode supported by the device. * @fifo_mode: FIFO operating mode supported by the device.
@ -136,14 +124,12 @@ struct st_lsm6dsx_sensor {
* @sip: Total number of samples (acc/gyro) in a given pattern. * @sip: Total number of samples (acc/gyro) in a given pattern.
* @iio_devs: Pointers to acc/gyro iio_dev instances. * @iio_devs: Pointers to acc/gyro iio_dev instances.
* @settings: Pointer to the specific sensor settings in use. * @settings: Pointer to the specific sensor settings in use.
* @tf: Transfer function structure used by I/O operations.
* @tb: Transfer buffers used by SPI I/O operations.
*/ */
struct st_lsm6dsx_hw { struct st_lsm6dsx_hw {
struct device *dev; struct device *dev;
struct regmap *regmap;
int irq; int irq;
struct mutex lock;
struct mutex fifo_lock; struct mutex fifo_lock;
struct mutex conf_lock; struct mutex conf_lock;
@ -154,17 +140,12 @@ struct st_lsm6dsx_hw {
struct iio_dev *iio_devs[ST_LSM6DSX_ID_MAX]; struct iio_dev *iio_devs[ST_LSM6DSX_ID_MAX];
const struct st_lsm6dsx_settings *settings; const struct st_lsm6dsx_settings *settings;
const struct st_lsm6dsx_transfer_function *tf;
#if defined(CONFIG_SPI_MASTER)
struct st_lsm6dsx_transfer_buffer tb;
#endif /* CONFIG_SPI_MASTER */
}; };
extern const struct dev_pm_ops st_lsm6dsx_pm_ops; extern const struct dev_pm_ops st_lsm6dsx_pm_ops;
int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name, 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);
int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor); int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor);
int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor); int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor);
int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw); int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw);

81
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
View File

@ -30,6 +30,8 @@
#include <linux/iio/kfifo_buf.h> #include <linux/iio/kfifo_buf.h>
#include <linux/iio/iio.h> #include <linux/iio/iio.h>
#include <linux/iio/buffer.h> #include <linux/iio/buffer.h>
#include <linux/regmap.h>
#include <linux/bitfield.h>
#include <linux/platform_data/st_sensors_pdata.h> #include <linux/platform_data/st_sensors_pdata.h>
@ -120,8 +122,10 @@ static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
dec_reg = &hw->settings->decimator[sensor->id]; dec_reg = &hw->settings->decimator[sensor->id];
if (dec_reg->addr) { if (dec_reg->addr) {
err = st_lsm6dsx_write_with_mask(hw, dec_reg->addr, int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask);
dec_reg->mask, data);
err = regmap_update_bits(hw->regmap, dec_reg->addr,
dec_reg->mask, val);
if (err < 0) if (err < 0)
return err; return err;
} }
@ -137,8 +141,10 @@ int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
{ {
int err; int err;
err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR, err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode); ST_LSM6DSX_FIFO_MODE_MASK,
FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK,
fifo_mode));
if (err < 0) if (err < 0)
return err; return err;
@ -154,8 +160,9 @@ static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
u8 data; u8 data;
data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0; data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0;
return st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR, return regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
ST_LSM6DSX_FIFO_ODR_MASK, data); ST_LSM6DSX_FIFO_ODR_MASK,
FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK, data));
} }
int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark) int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
@ -163,9 +170,8 @@ int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
u16 fifo_watermark = ~0, cur_watermark, sip = 0, fifo_th_mask; u16 fifo_watermark = ~0, cur_watermark, sip = 0, fifo_th_mask;
struct st_lsm6dsx_hw *hw = sensor->hw; struct st_lsm6dsx_hw *hw = sensor->hw;
struct st_lsm6dsx_sensor *cur_sensor; struct st_lsm6dsx_sensor *cur_sensor;
int i, err, data;
__le16 wdata; __le16 wdata;
int i, err;
u8 data;
for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
cur_sensor = iio_priv(hw->iio_devs[i]); cur_sensor = iio_priv(hw->iio_devs[i]);
@ -187,24 +193,42 @@ int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
fifo_watermark = (fifo_watermark / sip) * sip; fifo_watermark = (fifo_watermark / sip) * sip;
fifo_watermark = fifo_watermark * hw->settings->fifo_ops.th_wl; fifo_watermark = fifo_watermark * hw->settings->fifo_ops.th_wl;
mutex_lock(&hw->lock); err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1,
&data);
err = hw->tf->read(hw->dev, hw->settings->fifo_ops.fifo_th.addr + 1,
sizeof(data), &data);
if (err < 0) if (err < 0)
goto out; return err;
fifo_th_mask = hw->settings->fifo_ops.fifo_th.mask; fifo_th_mask = hw->settings->fifo_ops.fifo_th.mask;
fifo_watermark = ((data << 8) & ~fifo_th_mask) | fifo_watermark = ((data << 8) & ~fifo_th_mask) |
(fifo_watermark & fifo_th_mask); (fifo_watermark & fifo_th_mask);
wdata = cpu_to_le16(fifo_watermark); wdata = cpu_to_le16(fifo_watermark);
err = hw->tf->write(hw->dev, hw->settings->fifo_ops.fifo_th.addr, return regmap_bulk_write(hw->regmap,
sizeof(wdata), (u8 *)&wdata); hw->settings->fifo_ops.fifo_th.addr,
out: &wdata, sizeof(wdata));
mutex_unlock(&hw->lock); }
return err < 0 ? err : 0; /*
* Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN in order to avoid
* a kmalloc for each bus access
*/
static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 *data,
unsigned int data_len)
{
unsigned int word_len, read_len = 0;
int err;
while (read_len < data_len) {
word_len = min_t(unsigned int, data_len - read_len,
ST_LSM6DSX_MAX_WORD_LEN);
err = regmap_bulk_read(hw->regmap,
ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
data + read_len, word_len);
if (err < 0)
return err;
read_len += word_len;
}
return 0;
} }
/** /**
@ -226,8 +250,9 @@ static int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
u8 buff[pattern_len]; u8 buff[pattern_len];
__le16 fifo_status; __le16 fifo_status;
err = hw->tf->read(hw->dev, hw->settings->fifo_ops.fifo_diff.addr, err = regmap_bulk_read(hw->regmap,
sizeof(fifo_status), (u8 *)&fifo_status); hw->settings->fifo_ops.fifo_diff.addr,
&fifo_status, sizeof(fifo_status));
if (err < 0) if (err < 0)
return err; return err;
@ -255,8 +280,7 @@ static int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
samples); samples);
for (read_len = 0; read_len < fifo_len; read_len += pattern_len) { for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_OUTL_ADDR, err = st_lsm6dsx_read_block(hw, buff, sizeof(buff));
sizeof(buff), buff);
if (err < 0) if (err < 0)
return err; return err;
@ -449,17 +473,20 @@ int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
return -EINVAL; return -EINVAL;
} }
err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_HLACTIVE_ADDR, err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_HLACTIVE_ADDR,
ST_LSM6DSX_REG_HLACTIVE_MASK, ST_LSM6DSX_REG_HLACTIVE_MASK,
irq_active_low); FIELD_PREP(ST_LSM6DSX_REG_HLACTIVE_MASK,
irq_active_low));
if (err < 0) if (err < 0)
return err; return err;
pdata = (struct st_sensors_platform_data *)hw->dev->platform_data; pdata = (struct st_sensors_platform_data *)hw->dev->platform_data;
if ((np && of_property_read_bool(np, "drive-open-drain")) || if ((np && of_property_read_bool(np, "drive-open-drain")) ||
(pdata && pdata->open_drain)) { (pdata && pdata->open_drain)) {
err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_PP_OD_ADDR, err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_PP_OD_ADDR,
ST_LSM6DSX_REG_PP_OD_MASK, 1); ST_LSM6DSX_REG_PP_OD_MASK,
FIELD_PREP(ST_LSM6DSX_REG_PP_OD_MASK,
1));
if (err < 0) if (err < 0)
return err; return err;

103
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
View File

@ -37,6 +37,8 @@
#include <linux/iio/iio.h> #include <linux/iio/iio.h>
#include <linux/iio/sysfs.h> #include <linux/iio/sysfs.h>
#include <linux/pm.h> #include <linux/pm.h>
#include <linux/regmap.h>
#include <linux/bitfield.h>
#include <linux/platform_data/st_sensors_pdata.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), 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) static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw, int id)
{ {
int err, i, j; int err, i, j, data;
u8 data;
for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) { for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) {
for (j = 0; j < ST_LSM6DSX_MAX_ID; j++) { 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; return -ENODEV;
} }
err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_WHOAMI_ADDR, sizeof(data), err = regmap_read(hw->regmap, ST_LSM6DSX_REG_WHOAMI_ADDR, &data);
&data);
if (err < 0) { if (err < 0) {
dev_err(hw->dev, "failed to read whoami register\n"); dev_err(hw->dev, "failed to read whoami register\n");
return err; 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, static int st_lsm6dsx_set_full_scale(struct st_lsm6dsx_sensor *sensor,
u32 gain) 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; int i, err;
u8 val; u8 val;
for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++) 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; break;
if (i == ST_LSM6DSX_FS_LIST_SIZE) if (i == ST_LSM6DSX_FS_LIST_SIZE)
return -EINVAL; return -EINVAL;
val = st_lsm6dsx_fs_table[id].fs_avl[i].val; val = st_lsm6dsx_fs_table[sensor->id].fs_avl[i].val;
err = st_lsm6dsx_write_with_mask(sensor->hw, reg = &st_lsm6dsx_fs_table[sensor->id].reg;
st_lsm6dsx_fs_table[id].reg.addr, err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,
st_lsm6dsx_fs_table[id].reg.mask, ST_LSM6DSX_SHIFT_VAL(val, reg->mask));
val);
if (err < 0) if (err < 0)
return err; 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) 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; int err;
u8 val; u8 val;
@ -393,10 +368,9 @@ static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr)
if (err < 0) if (err < 0)
return err; return err;
return st_lsm6dsx_write_with_mask(sensor->hw, reg = &st_lsm6dsx_odr_table[sensor->id].reg;
st_lsm6dsx_odr_table[id].reg.addr, return regmap_update_bits(hw->regmap, reg->addr, reg->mask,
st_lsm6dsx_odr_table[id].reg.mask, ST_LSM6DSX_SHIFT_VAL(val, reg->mask));
val);
} }
int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor) 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) 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; int err;
err = st_lsm6dsx_write_with_mask(sensor->hw, reg = &st_lsm6dsx_odr_table[sensor->id].reg;
st_lsm6dsx_odr_table[id].reg.addr, err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,
st_lsm6dsx_odr_table[id].reg.mask, 0); ST_LSM6DSX_SHIFT_VAL(0, reg->mask));
if (err < 0) if (err < 0)
return err; return err;
sensor->hw->enable_mask &= ~BIT(id); sensor->hw->enable_mask &= ~BIT(sensor->id);
return 0; 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, static int st_lsm6dsx_read_oneshot(struct st_lsm6dsx_sensor *sensor,
u8 addr, int *val) u8 addr, int *val)
{ {
struct st_lsm6dsx_hw *hw = sensor->hw;
int err, delay; int err, delay;
__le16 data; __le16 data;
@ -441,8 +417,7 @@ static int st_lsm6dsx_read_oneshot(struct st_lsm6dsx_sensor *sensor,
delay = 1000000 / sensor->odr; delay = 1000000 / sensor->odr;
usleep_range(delay, 2 * delay); usleep_range(delay, 2 * delay);
err = sensor->hw->tf->read(sensor->hw->dev, addr, sizeof(data), err = regmap_bulk_read(hw->regmap, addr, &data, sizeof(data));
(u8 *)&data);
if (err < 0) if (err < 0)
return err; return err;
@ -657,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) static int st_lsm6dsx_init_device(struct st_lsm6dsx_hw *hw)
{ {
u8 data, drdy_int_reg; u8 drdy_int_reg;
int err; int err;
data = ST_LSM6DSX_REG_RESET_MASK; err = regmap_write(hw->regmap, ST_LSM6DSX_REG_RESET_ADDR,
err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_RESET_ADDR, sizeof(data), ST_LSM6DSX_REG_RESET_MASK);
&data);
if (err < 0) if (err < 0)
return err; return err;
msleep(200); msleep(200);
/* enable Block Data Update */ /* enable Block Data Update */
err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_BDU_ADDR, err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_BDU_ADDR,
ST_LSM6DSX_REG_BDU_MASK, 1); ST_LSM6DSX_REG_BDU_MASK,
FIELD_PREP(ST_LSM6DSX_REG_BDU_MASK, 1));
if (err < 0) if (err < 0)
return err; return err;
@ -679,8 +654,10 @@ static int st_lsm6dsx_init_device(struct st_lsm6dsx_hw *hw)
if (err < 0) if (err < 0)
return err; return err;
return st_lsm6dsx_write_with_mask(hw, drdy_int_reg, return regmap_update_bits(hw->regmap, drdy_int_reg,
ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK, 1); 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, static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw,
@ -731,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, 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; struct st_lsm6dsx_hw *hw;
int i, err; int i, err;
@ -742,13 +719,12 @@ int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
dev_set_drvdata(dev, (void *)hw); dev_set_drvdata(dev, (void *)hw);
mutex_init(&hw->lock);
mutex_init(&hw->fifo_lock); mutex_init(&hw->fifo_lock);
mutex_init(&hw->conf_lock); mutex_init(&hw->conf_lock);
hw->dev = dev; hw->dev = dev;
hw->irq = irq; hw->irq = irq;
hw->tf = tf_ops; hw->regmap = regmap;
err = st_lsm6dsx_check_whoami(hw, hw_id); err = st_lsm6dsx_check_whoami(hw, hw_id);
if (err < 0) if (err < 0)
@ -784,6 +760,7 @@ static int __maybe_unused st_lsm6dsx_suspend(struct device *dev)
{ {
struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev); struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev);
struct st_lsm6dsx_sensor *sensor; struct st_lsm6dsx_sensor *sensor;
const struct st_lsm6dsx_reg *reg;
int i, err = 0; int i, err = 0;
for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
@ -791,9 +768,9 @@ static int __maybe_unused st_lsm6dsx_suspend(struct device *dev)
if (!(hw->enable_mask & BIT(sensor->id))) if (!(hw->enable_mask & BIT(sensor->id)))
continue; continue;
err = st_lsm6dsx_write_with_mask(hw, reg = &st_lsm6dsx_odr_table[sensor->id].reg;
st_lsm6dsx_odr_table[sensor->id].reg.addr, err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,
st_lsm6dsx_odr_table[sensor->id].reg.mask, 0); ST_LSM6DSX_SHIFT_VAL(0, reg->mask));
if (err < 0) if (err < 0)
return err; return err;
} }

55
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c
View File

@ -14,55 +14,30 @@
#include <linux/i2c.h> #include <linux/i2c.h>
#include <linux/slab.h> #include <linux/slab.h>
#include <linux/of.h> #include <linux/of.h>
#include <linux/regmap.h>
#include "st_lsm6dsx.h" #include "st_lsm6dsx.h"
static int st_lsm6dsx_i2c_read(struct device *dev, u8 addr, int len, u8 *data) static const struct regmap_config st_lsm6dsx_i2c_regmap_config = {
{ .reg_bits = 8,
struct i2c_client *client = to_i2c_client(dev); .val_bits = 8,
struct i2c_msg msg[2];
msg[0].addr = client->addr;
msg[0].flags = client->flags;
msg[0].len = 1;
msg[0].buf = &addr;
msg[1].addr = client->addr;
msg[1].flags = client->flags | I2C_M_RD;
msg[1].len = len;
msg[1].buf = data;
return i2c_transfer(client->adapter, msg, 2);
}
static int st_lsm6dsx_i2c_write(struct device *dev, u8 addr, int len, u8 *data)
{
struct i2c_client *client = to_i2c_client(dev);
struct i2c_msg msg;
u8 send[len + 1];
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, static int st_lsm6dsx_i2c_probe(struct i2c_client *client,
const struct i2c_device_id *id) const struct i2c_device_id *id)
{ {
int hw_id = id->driver_data;
struct regmap *regmap;
regmap = devm_regmap_init_i2c(client, &st_lsm6dsx_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);
}
return st_lsm6dsx_probe(&client->dev, client->irq, return st_lsm6dsx_probe(&client->dev, client->irq,
(int)id->driver_data, id->name, hw_id, id->name, regmap);
&st_lsm6dsx_transfer_fn);
} }
static const struct of_device_id st_lsm6dsx_i2c_of_match[] = { static const struct of_device_id st_lsm6dsx_i2c_of_match[] = {

70
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c
View File

@ -14,72 +14,30 @@
#include <linux/spi/spi.h> #include <linux/spi/spi.h>
#include <linux/slab.h> #include <linux/slab.h>
#include <linux/of.h> #include <linux/of.h>
#include <linux/regmap.h>
#include "st_lsm6dsx.h" #include "st_lsm6dsx.h"
#define SENSORS_SPI_READ BIT(7) static const struct regmap_config st_lsm6dsx_spi_regmap_config = {
.reg_bits = 8,
static int st_lsm6dsx_spi_read(struct device *dev, u8 addr, int len, .val_bits = 8,
u8 *data)
{
struct spi_device *spi = to_spi_device(dev);
struct st_lsm6dsx_hw *hw = spi_get_drvdata(spi);
int err;
struct spi_transfer xfers[] = {
{
.tx_buf = hw->tb.tx_buf,
.bits_per_word = 8,
.len = 1,
},
{
.rx_buf = hw->tb.rx_buf,
.bits_per_word = 8,
.len = len,
}
};
hw->tb.tx_buf[0] = addr | SENSORS_SPI_READ;
err = spi_sync_transfer(spi, xfers, ARRAY_SIZE(xfers));
if (err < 0)
return err;
memcpy(data, hw->tb.rx_buf, len * sizeof(u8));
return len;
}
static int st_lsm6dsx_spi_write(struct device *dev, u8 addr, int len,
u8 *data)
{
struct st_lsm6dsx_hw *hw;
struct spi_device *spi;
if (len >= ST_LSM6DSX_TX_MAX_LENGTH)
return -ENOMEM;
spi = to_spi_device(dev);
hw = spi_get_drvdata(spi);
hw->tb.tx_buf[0] = addr;
memcpy(&hw->tb.tx_buf[1], data, len);
return spi_write(spi, hw->tb.tx_buf, len + 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) static int st_lsm6dsx_spi_probe(struct spi_device *spi)
{ {
const struct spi_device_id *id = spi_get_device_id(spi); const struct spi_device_id *id = spi_get_device_id(spi);
int hw_id = id->driver_data;
struct regmap *regmap;
regmap = devm_regmap_init_spi(spi, &st_lsm6dsx_spi_regmap_config);
if (IS_ERR(regmap)) {
dev_err(&spi->dev, "Failed to register spi regmap %d\n",
(int)PTR_ERR(regmap));
return PTR_ERR(regmap);
}
return st_lsm6dsx_probe(&spi->dev, spi->irq, return st_lsm6dsx_probe(&spi->dev, spi->irq,
(int)id->driver_data, id->name, hw_id, id->name, regmap);
&st_lsm6dsx_transfer_fn);
} }
static const struct of_device_id st_lsm6dsx_spi_of_match[] = { static const struct of_device_id st_lsm6dsx_spi_of_match[] = {