korg/linux
0
0
Fork 0
mirror of https://mirrors.bfsu.edu.cn/git/linux.git synced 2024-11-15 00:04:15 +08:00
Code Issues Packages Projects Releases Wiki Activity

iio: imu: st_lsm6dsx: add motion report function and call from interrupt

Browse Source 
Report iio motion events to iio subsystem and filter motion events.
Wakeup will still be on all channels as it's not possible to do the filtering
in hw.

Signed-off-by: Sean Nyekjaer <sean@geanix.com>
Reviewed-by: Lorenzo Bianconi <lorenzo@kernel.org>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>
This commit is contained in:
Sean Nyekjaer 2019-09-16 15:56:30 +02:00 committed by Jonathan Cameron
parent a3aa17d4ba
commit 1aabad1fb5
2 changed files with 98 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -186,6 +186,11 @@ struct st_lsm6dsx_shub_settings {
struct st_lsm6dsx_event_settings {
struct st_lsm6dsx_reg enable_reg;
struct st_lsm6dsx_reg wakeup_reg;
u8 wakeup_src_reg;
u8 wakeup_src_status_mask;
u8 wakeup_src_z_mask;
u8 wakeup_src_y_mask;
u8 wakeup_src_x_mask;
};
enum st_lsm6dsx_ext_sensor_id {

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

@ -48,6 +48,7 @@
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/delay.h>
#include <linux/iio/events.h>
#include <linux/iio/iio.h>
#include <linux/iio/sysfs.h>
#include <linux/interrupt.h>
@ -287,6 +288,11 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
.addr = 0x5B,
.mask = GENMASK(5, 0),
},
.wakeup_src_reg = 0x1b,
.wakeup_src_status_mask = BIT(3),
.wakeup_src_z_mask = BIT(0),
.wakeup_src_y_mask = BIT(1),
.wakeup_src_x_mask = BIT(2),
},
},
{
@ -412,6 +418,11 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
.addr = 0x5B,
.mask = GENMASK(5, 0),
},
.wakeup_src_reg = 0x1b,
.wakeup_src_status_mask = BIT(3),
.wakeup_src_z_mask = BIT(0),
.wakeup_src_y_mask = BIT(1),
.wakeup_src_x_mask = BIT(2),
},
},
{
@ -550,6 +561,11 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
.addr = 0x5B,
.mask = GENMASK(5, 0),
},
.wakeup_src_reg = 0x1b,
.wakeup_src_status_mask = BIT(3),
.wakeup_src_z_mask = BIT(0),
.wakeup_src_y_mask = BIT(1),
.wakeup_src_x_mask = BIT(2),
},
},
{
@ -816,6 +832,11 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
.addr = 0x5B,
.mask = GENMASK(5, 0),
},
.wakeup_src_reg = 0x1b,
.wakeup_src_status_mask = BIT(3),
.wakeup_src_z_mask = BIT(0),
.wakeup_src_y_mask = BIT(1),
.wakeup_src_x_mask = BIT(2),
},
},
{
@ -970,6 +991,11 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
.addr = 0x5B,
.mask = GENMASK(5, 0),
},
.wakeup_src_reg = 0x1b,
.wakeup_src_status_mask = BIT(3),
.wakeup_src_z_mask = BIT(0),
.wakeup_src_y_mask = BIT(1),
.wakeup_src_x_mask = BIT(2),
}
},
};
@ -1334,7 +1360,7 @@ static int st_lsm6dsx_read_event_config(struct iio_dev *iio_dev,
if (type != IIO_EV_TYPE_THRESH)
return -EINVAL;
return hw->enable_event;
return !!(hw->enable_event & BIT(chan->channel2));
}
static int st_lsm6dsx_write_event_config(struct iio_dev *iio_dev,
@ -1345,13 +1371,28 @@ static int st_lsm6dsx_write_event_config(struct iio_dev *iio_dev,
{
struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
struct st_lsm6dsx_hw *hw = sensor->hw;
u8 enable_event;
int err = 0;
if (type != IIO_EV_TYPE_THRESH)
return -EINVAL;
/* do not enable events if they are already enabled */
if (state && hw->enable_event)
if (state) {
enable_event = hw->enable_event | BIT(chan->channel2);
/* do not enable events if they are already enabled */
if (hw->enable_event)
goto out;
} else {
enable_event = hw->enable_event & ~BIT(chan->channel2);
/* only turn off sensor if no events is enabled */
if (enable_event)
goto out;
}
/* stop here if no changes have been made */
if (hw->enable_event == enable_event)
return 0;
err = st_lsm6dsx_event_setup(hw, state);
@ -1362,7 +1403,8 @@ static int st_lsm6dsx_write_event_config(struct iio_dev *iio_dev,
if (err < 0)
return err;
hw->enable_event = state;
out:
hw->enable_event = enable_event;
return 0;
}
@ -1715,10 +1757,57 @@ static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw,
return iio_dev;
}
static void st_lsm6dsx_report_motion_event(struct st_lsm6dsx_hw *hw, int data)
{
s64 timestamp = iio_get_time_ns(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
if ((data & hw->settings->event_settings.wakeup_src_z_mask) &&
(hw->enable_event & BIT(IIO_MOD_Z)))
iio_push_event(hw->iio_devs[ST_LSM6DSX_ID_ACC],
IIO_MOD_EVENT_CODE(IIO_ACCEL,
0,
IIO_MOD_Z,
IIO_EV_TYPE_THRESH,
IIO_EV_DIR_EITHER),
timestamp);
if ((data & hw->settings->event_settings.wakeup_src_y_mask) &&
(hw->enable_event & BIT(IIO_MOD_Y)))
iio_push_event(hw->iio_devs[ST_LSM6DSX_ID_ACC],
IIO_MOD_EVENT_CODE(IIO_ACCEL,
0,
IIO_MOD_Y,
IIO_EV_TYPE_THRESH,
IIO_EV_DIR_EITHER),
timestamp);
if ((data & hw->settings->event_settings.wakeup_src_x_mask) &&
(hw->enable_event & BIT(IIO_MOD_X)))
iio_push_event(hw->iio_devs[ST_LSM6DSX_ID_ACC],
IIO_MOD_EVENT_CODE(IIO_ACCEL,
0,
IIO_MOD_X,
IIO_EV_TYPE_THRESH,
IIO_EV_DIR_EITHER),
timestamp);
}
static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private)
{
struct st_lsm6dsx_hw *hw = private;
int count;
int data, err;
if (hw->enable_event) {
err = regmap_read(hw->regmap,
hw->settings->event_settings.wakeup_src_reg,
&data);
if (err < 0)
return IRQ_NONE;
if (data & hw->settings->event_settings.wakeup_src_status_mask)
st_lsm6dsx_report_motion_event(hw, data);
}
mutex_lock(&hw->fifo_lock);
count = hw->settings->fifo_ops.read_fifo(hw);