summaryrefslogtreecommitdiff
path: root/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c')
-rw-r--r--drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c159
1 files changed, 53 insertions, 106 deletions
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
index b0f3da1976e4..cb536b81a1c2 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -14,10 +14,10 @@
* (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the
* value of the decimation factor and ODR set for each FIFO data set.
*
- * LSM6DSO/LSM6DSOX/ASM330LHH/LSM6DSR/ISM330DHCX: The FIFO buffer can be
- * configured to store data from gyroscope and accelerometer. Each sample
- * is queued with a tag (1B) indicating data source (gyroscope, accelerometer,
- * hw timer).
+ * LSM6DSO/LSM6DSOX/ASM330LHH/LSM6DSR/LSM6DSRX/ISM330DHCX:
+ * The FIFO buffer can be configured to store data from gyroscope and
+ * accelerometer. Each sample is queued with a tag (1B) indicating data
+ * source (gyroscope, accelerometer, hw timer).
*
* FIFO supported modes:
* - BYPASS: FIFO disabled
@@ -30,8 +30,6 @@
* Denis Ciocca <denis.ciocca@st.com>
*/
#include <linux/module.h>
-#include <linux/interrupt.h>
-#include <linux/irq.h>
#include <linux/iio/kfifo_buf.h>
#include <linux/iio/iio.h>
#include <linux/iio/buffer.h>
@@ -42,10 +40,6 @@
#include "st_lsm6dsx.h"
-#define ST_LSM6DSX_REG_HLACTIVE_ADDR 0x12
-#define ST_LSM6DSX_REG_HLACTIVE_MASK BIT(5)
-#define ST_LSM6DSX_REG_PP_OD_ADDR 0x12
-#define ST_LSM6DSX_REG_PP_OD_MASK BIT(4)
#define ST_LSM6DSX_REG_FIFO_MODE_ADDR 0x0a
#define ST_LSM6DSX_FIFO_MODE_MASK GENMASK(2, 0)
#define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3)
@@ -56,7 +50,6 @@
#define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08
-#define ST_LSM6DSX_TS_SENSITIVITY 25000UL /* 25us */
#define ST_LSM6DSX_TS_RESET_VAL 0xaa
struct st_lsm6dsx_decimator_entry {
@@ -85,20 +78,26 @@ struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = {
{ 32, 0x7 },
};
-static int st_lsm6dsx_get_decimator_val(u8 val)
+static int
+st_lsm6dsx_get_decimator_val(struct st_lsm6dsx_sensor *sensor, u32 max_odr)
{
const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
+ u32 decimator = max_odr / sensor->odr;
int i;
- for (i = 0; i < max_size; i++)
- if (st_lsm6dsx_decimator_table[i].decimator == val)
+ if (decimator > 1)
+ decimator = round_down(decimator, 2);
+
+ for (i = 0; i < max_size; i++) {
+ if (st_lsm6dsx_decimator_table[i].decimator == decimator)
break;
+ }
return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
}
static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
- u16 *max_odr, u16 *min_odr)
+ u32 *max_odr, u32 *min_odr)
{
struct st_lsm6dsx_sensor *sensor;
int i;
@@ -113,16 +112,24 @@ static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
if (!(hw->enable_mask & BIT(sensor->id)))
continue;
- *max_odr = max_t(u16, *max_odr, sensor->odr);
- *min_odr = min_t(u16, *min_odr, sensor->odr);
+ *max_odr = max_t(u32, *max_odr, sensor->odr);
+ *min_odr = min_t(u32, *min_odr, sensor->odr);
}
}
+static u8 st_lsm6dsx_get_sip(struct st_lsm6dsx_sensor *sensor, u32 min_odr)
+{
+ u8 sip = sensor->odr / min_odr;
+
+ return sip > 1 ? round_down(sip, 2) : sip;
+}
+
static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
{
- u16 max_odr, min_odr, sip = 0, ts_sip = 0;
const struct st_lsm6dsx_reg *ts_dec_reg;
struct st_lsm6dsx_sensor *sensor;
+ u16 sip = 0, ts_sip = 0;
+ u32 max_odr, min_odr;
int err = 0, i;
u8 data;
@@ -137,12 +144,10 @@ static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
sensor = iio_priv(hw->iio_devs[i]);
/* update fifo decimators and sample in pattern */
if (hw->enable_mask & BIT(sensor->id)) {
- sensor->sip = sensor->odr / min_odr;
- sensor->decimator = max_odr / sensor->odr;
- data = st_lsm6dsx_get_decimator_val(sensor->decimator);
+ sensor->sip = st_lsm6dsx_get_sip(sensor, min_odr);
+ data = st_lsm6dsx_get_decimator_val(sensor, max_odr);
} else {
sensor->sip = 0;
- sensor->decimator = 0;
data = 0;
}
ts_sip = max_t(u16, ts_sip, sensor->sip);
@@ -182,17 +187,10 @@ int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
enum st_lsm6dsx_fifo_mode fifo_mode)
{
unsigned int data;
- int err;
data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode);
- err = st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
- ST_LSM6DSX_FIFO_MODE_MASK, data);
- if (err < 0)
- return err;
-
- hw->fifo_mode = fifo_mode;
-
- return 0;
+ return st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
+ ST_LSM6DSX_FIFO_MODE_MASK, data);
}
static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
@@ -429,7 +427,7 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
*/
if (!reset_ts && ts >= 0xff0000)
reset_ts = true;
- ts *= ST_LSM6DSX_TS_SENSITIVITY;
+ ts *= hw->ts_gain;
offset += ST_LSM6DSX_SAMPLE_SIZE;
}
@@ -456,13 +454,19 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
return read_len;
}
+#define ST_LSM6DSX_INVALID_SAMPLE 0x7ffd
static int
st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag,
u8 *data, s64 ts)
{
+ s16 val = le16_to_cpu(*(__le16 *)data);
struct st_lsm6dsx_sensor *sensor;
struct iio_dev *iio_dev;
+ /* invalid sample during bootstrap phase */
+ if (val >= ST_LSM6DSX_INVALID_SAMPLE)
+ return -EINVAL;
+
/*
* EXT_TAG are managed in FIFO fashion so ST_LSM6DSX_EXT0_TAG
* corresponds to the first enabled channel, ST_LSM6DSX_EXT1_TAG
@@ -572,7 +576,7 @@ int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
*/
if (!reset_ts && ts >= 0xffff0000)
reset_ts = true;
- ts *= ST_LSM6DSX_TS_SENSITIVITY;
+ ts *= hw->ts_gain;
} else {
st_lsm6dsx_push_tagged_data(hw, tag, iio_buff,
ts);
@@ -592,6 +596,9 @@ int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
{
int err;
+ if (!hw->settings->fifo_ops.read_fifo)
+ return -ENOTSUPP;
+
mutex_lock(&hw->fifo_lock);
hw->settings->fifo_ops.read_fifo(hw);
@@ -605,11 +612,17 @@ int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
{
struct st_lsm6dsx_hw *hw = sensor->hw;
+ u8 fifo_mask;
int err;
mutex_lock(&hw->conf_lock);
- if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
+ if (enable)
+ fifo_mask = hw->fifo_mask | BIT(sensor->id);
+ else
+ fifo_mask = hw->fifo_mask & ~BIT(sensor->id);
+
+ if (hw->fifo_mask) {
err = st_lsm6dsx_flush_fifo(hw);
if (err < 0)
goto out;
@@ -639,40 +652,25 @@ int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
if (err < 0)
goto out;
- if (hw->enable_mask) {
+ if (fifo_mask) {
/* reset hw ts counter */
err = st_lsm6dsx_reset_hw_ts(hw);
if (err < 0)
goto out;
err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
+ if (err < 0)
+ goto out;
}
+ hw->fifo_mask = fifo_mask;
+
out:
mutex_unlock(&hw->conf_lock);
return err;
}
-static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private)
-{
- struct st_lsm6dsx_hw *hw = private;
-
- return hw->sip > 0 ? IRQ_WAKE_THREAD : IRQ_NONE;
-}
-
-static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private)
-{
- struct st_lsm6dsx_hw *hw = private;
- int count;
-
- mutex_lock(&hw->fifo_lock);
- count = hw->settings->fifo_ops.read_fifo(hw);
- mutex_unlock(&hw->fifo_lock);
-
- return count ? IRQ_HANDLED : IRQ_NONE;
-}
-
static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev)
{
struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
@@ -702,59 +700,8 @@ static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
{
- struct device_node *np = hw->dev->of_node;
- struct st_sensors_platform_data *pdata;
struct iio_buffer *buffer;
- unsigned long irq_type;
- bool irq_active_low;
- int i, err;
-
- irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
-
- switch (irq_type) {
- case IRQF_TRIGGER_HIGH:
- case IRQF_TRIGGER_RISING:
- irq_active_low = false;
- break;
- case IRQF_TRIGGER_LOW:
- case IRQF_TRIGGER_FALLING:
- irq_active_low = true;
- break;
- default:
- dev_info(hw->dev, "mode %lx unsupported\n", irq_type);
- return -EINVAL;
- }
-
- err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_HLACTIVE_ADDR,
- ST_LSM6DSX_REG_HLACTIVE_MASK,
- FIELD_PREP(ST_LSM6DSX_REG_HLACTIVE_MASK,
- irq_active_low));
- if (err < 0)
- return err;
-
- pdata = (struct st_sensors_platform_data *)hw->dev->platform_data;
- if ((np && of_property_read_bool(np, "drive-open-drain")) ||
- (pdata && pdata->open_drain)) {
- err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_PP_OD_ADDR,
- ST_LSM6DSX_REG_PP_OD_MASK,
- FIELD_PREP(ST_LSM6DSX_REG_PP_OD_MASK,
- 1));
- if (err < 0)
- return err;
-
- irq_type |= IRQF_SHARED;
- }
-
- err = devm_request_threaded_irq(hw->dev, hw->irq,
- st_lsm6dsx_handler_irq,
- st_lsm6dsx_handler_thread,
- irq_type | IRQF_ONESHOT,
- "lsm6dsx", hw);
- if (err) {
- dev_err(hw->dev, "failed to request trigger irq %d\n",
- hw->irq);
- return err;
- }
+ int i;
for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
if (!hw->iio_devs[i])
generated by cgit v1.2.3 (git 2.39.0) at 2024-08-17 15:13:51 +0300