shinys000114/linux
1
0
Fork 0
mirror of https://github.com/hardkernel/linux.git synced 2026-06-07 11:26:02 +09:00
Code Issues Packages Projects Releases Wiki Activity

iio: imu: lsm6dsr: reapply settings at resume

Browse Source 
The ST LSM6DSR does not currently preserve settings after
powered down, it will default to the ST LSM6DSR settings after
powered up, not the settings set by the host.

Reapply all the settings at resume.

Signed-off-by: Jason Zhang <jason.zhang@rock-chips.com>
Change-Id: I702140e4d952ea5aee227d0ef4d2761bf6e6760f
This commit is contained in:
Jason Zhang
2022-08-02 19:21:38 +08:00
committed by Tao Huang
parent c33ad29a3e
commit 6e4237635c
3 changed files with 116 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
drivers/iio/imu/st_lsm6dsr/st_lsm6dsr.h
View File

@@ -446,6 +446,7 @@ struct st_lsm6dsr_hw {
unsigned long state;
u32 enable_mask;
u32 requested_mask;
u32 suspend_mask;
u16 fsm_enable_mask;
u8 embfunc_irq_reg;
@@ -544,6 +545,7 @@ int st_lsm6dsr_shub_set_enable(struct st_lsm6dsr_sensor *sensor, bool enable);
int st_lsm6dsr_shub_probe(struct st_lsm6dsr_hw *hw);
int st_lsm6dsr_sensor_set_enable(struct st_lsm6dsr_sensor *sensor,
bool enable);
int st_lsm6dsr_irq_setup(struct st_lsm6dsr_hw *hw);
int st_lsm6dsr_buffers_setup(struct st_lsm6dsr_hw *hw);
int st_lsm6dsr_deallocate_buffers(struct st_lsm6dsr_hw *hw);
int st_lsm6dsr_get_odr_val(enum st_lsm6dsr_sensor_id id, int odr, int uodr,

37
drivers/iio/imu/st_lsm6dsr/st_lsm6dsr_buffer.c
View File

@@ -915,21 +915,19 @@ static const struct iio_buffer_setup_ops st_lsm6dsr_buffer_ops = {
.postdisable = st_lsm6dsr_buffer_postdisable,
};
/**
* Init IIO buffers and triggers
* Init IRQ
*
* @param hw: ST IMU MEMS hw instance
* @return < 0 if error, 0 otherwise
*/
int st_lsm6dsr_buffers_setup(struct st_lsm6dsr_hw *hw)
int st_lsm6dsr_irq_setup(struct st_lsm6dsr_hw *hw)
{
struct device_node *np = hw->dev->of_node;
struct st_lsm6dsr_sensor *sensor;
struct iio_buffer *buffer;
struct iio_dev *iio_dev;
unsigned long irq_type;
bool irq_active_low;
int i, err;
int err;
irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
if (irq_type == IRQF_TRIGGER_NONE)
@@ -961,10 +959,33 @@ int st_lsm6dsr_buffers_setup(struct st_lsm6dsr_hw *hw)
ST_LSM6DSR_REG_PP_OD_MASK, 1);
if (err < 0)
return err;
irq_type |= IRQF_SHARED;
}
return 0;
}
/**
* Init IIO buffers and triggers
*
* @param hw: ST IMU MEMS hw instance
* @return < 0 if error, 0 otherwise
*/
int st_lsm6dsr_buffers_setup(struct st_lsm6dsr_hw *hw)
{
struct st_lsm6dsr_sensor *sensor;
struct iio_buffer *buffer;
struct iio_dev *iio_dev;
unsigned long irq_type;
int i, err;
err = st_lsm6dsr_irq_setup(hw);
if (err < 0)
return err;
irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
if (irq_type == IRQF_TRIGGER_NONE)
irq_type = IRQF_TRIGGER_HIGH;
err = devm_request_threaded_irq(hw->dev, hw->irq,
st_lsm6dsr_handler_irq,
st_lsm6dsr_handler_thread,

111
drivers/iio/imu/st_lsm6dsr/st_lsm6dsr_core.c
View File

@@ -21,6 +21,9 @@
#include "st_lsm6dsr.h"
#include "flat_roll_inward_x_y.h"
#define ST_LSM6DSR_PM_IMPLEMENTED 1
#define ST_LSM6DSR_PM_REGULATOR_CTL 1
struct st_lsm6dsr_hw *hw_input;
struct delayed_work data_work;
struct input_dev *acc_input;
@@ -1665,6 +1668,26 @@ static int st_lsm6dsr_init_device(struct st_lsm6dsr_hw *hw)
return st_lsm6dsr_fsm_init(hw);
}
static int st_lsm6dsr_init_fsm(struct st_lsm6dsr_hw *hw)
{
int i;
int err;
/* yxsui init config fsm */
mutex_lock(&hw->page_lock);
for (i = 0; i < ARRAY_SIZE(flat_roll_inward_x_y); i++) {
//err = hw->tf->write(hw->dev, addr, sizeof(data), &data);
err = hw->tf->write(hw->dev, flat_roll_inward_x_y[i].address,
sizeof(flat_roll_inward_x_y[i].data),
&(flat_roll_inward_x_y[i].data));
if (err < 0)
dev_info(hw->dev, "yxsui init config fsm fail\n");
}
mutex_unlock(&hw->page_lock);
return 0;
}
/**
* Allocate IIO device
*
@@ -2166,17 +2189,9 @@ int st_lsm6dsr_probe(struct device *dev, int irq,
INIT_DELAYED_WORK(&data_work, data_work_func);
schedule_delayed_work(&data_work, 10);
/* yxsui init config fsm */
mutex_lock(&hw->page_lock);
for (i = 0; i < ARRAY_SIZE(flat_roll_inward_x_y); i++) {
//err = hw->tf->write(hw->dev, addr, sizeof(data), &data);
err = hw->tf->write(hw->dev, flat_roll_inward_x_y[i].address,
sizeof(flat_roll_inward_x_y[i].data),
&(flat_roll_inward_x_y[i].data));
if (err < 0)
dev_info(dev, "yxsui init config fsm fail\n");
}
mutex_unlock(&hw->page_lock);
err = st_lsm6dsr_init_fsm(hw);
if (err < 0)
return err;
hw_input = hw;
dev_info(dev, "Device probed\n");
@@ -2274,19 +2289,31 @@ static int __maybe_unused st_lsm6dsr_suspend(struct device *dev)
if (!(hw->enable_mask & BIT(sensor->id)))
continue;
if (hw->enable_mask & (BIT(ST_LSM6DSR_ID_STEP_COUNTER) |
BIT(ST_LSM6DSR_ID_GYRO) |
BIT(ST_LSM6DSR_ID_ACC) |
BIT(ST_LSM6DSR_ID_EXT0) |
BIT(ST_LSM6DSR_ID_EXT1))) {
err = st_lsm6dsr_suspend_fifo(hw);
if (err < 0)
return err;
}
// power off enabled sensors
err = st_lsm6dsr_set_odr(sensor, 0, 0);
if (err < 0)
return err;
}
if (st_lsm6dsr_is_fifo_enabled(hw)) {
err = st_lsm6dsr_suspend_fifo(hw);
if (sensor->id == ST_LSM6DSR_ID_EXT0 ||
sensor->id == ST_LSM6DSR_ID_EXT1)
err = st_lsm6dsr_shub_set_enable(sensor, false);
else
err = st_lsm6dsr_sensor_set_enable(sensor, false);
if (err < 0)
return err;
hw->suspend_mask |= BIT(sensor->id);
}
#ifndef ST_LSM6DSR_PM_REGULATOR_CTL
err = st_lsm6dsr_bk_regs(hw);
#endif
#ifdef CONFIG_IIO_ST_LSM6DSR_MAY_WAKEUP
if (device_may_wakeup(dev))
@@ -2320,31 +2347,63 @@ static int __maybe_unused st_lsm6dsr_resume(struct device *dev)
disable_irq_wake(hw->irq);
#endif
err = st_lsm6dsr_restore_regs(hw);
#ifdef ST_LSM6DSR_PM_REGULATOR_CTL
err = st_lsm6dsr_reset_device(hw);
if (err < 0)
return err;
err = st_lsm6dsr_init_device(hw);
if (err < 0)
return err;
err = st_lsm6dsr_irq_setup(hw);
if (err < 0)
return err;
err = st_lsm6dsr_init_fsm(hw);
if (err < 0)
return err;
#else
err = st_lsm6dsr_restore_regs(hw);
if (err < 0)
return err;
#endif
// power on suspened sensors
for (i = 0; i < ST_LSM6DSR_ID_MAX; i++) {
if (!hw->iio_devs[i])
continue;
sensor = iio_priv(hw->iio_devs[i]);
if (!(hw->enable_mask & BIT(sensor->id)))
if (!(hw->suspend_mask & BIT(sensor->id)))
continue;
err = st_lsm6dsr_set_odr(sensor, sensor->odr, sensor->uodr);
if (sensor->id == ST_LSM6DSR_ID_EXT0 ||
sensor->id == ST_LSM6DSR_ID_EXT1)
err = st_lsm6dsr_shub_set_enable(sensor, true);
else
err = st_lsm6dsr_sensor_set_enable(sensor, true);
if (err < 0)
return err;
if (sensor->id == ST_LSM6DSR_ID_STEP_COUNTER ||
sensor->id == ST_LSM6DSR_ID_GYRO ||
sensor->id == ST_LSM6DSR_ID_ACC ||
sensor->id == ST_LSM6DSR_ID_EXT0 ||
sensor->id == ST_LSM6DSR_ID_EXT1) {
err = st_lsm6dsr_set_fifo_mode(hw,
ST_LSM6DSR_FIFO_CONT);
if (err < 0)
return err;
}
hw->suspend_mask &= ~BIT(sensor->id);
}
err = st_lsm6dsr_reset_hwts(hw);
if (err < 0)
return err;
if (st_lsm6dsr_is_fifo_enabled(hw))
err = st_lsm6dsr_set_fifo_mode(hw, ST_LSM6DSR_FIFO_CONT);
return err < 0 ? err : 0;
#endif
return 0;