nathan/CoolPi-Armbian-Rockchip-RK3588-Upstream-Linux-6.1-LTS-Kernel
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
CoolPi-Armbian-Rockchip-RK3.../drivers/iio/imu/st_lsm6dsr/st_lsm6dsr_buffer.c

1065 lines
26 KiB
C
Raw Blame History

// SPDX-License-Identifier: GPL-2.0+
/*
* STMicroelectronics st_lsm6dsr FIFO buffer library driver
*
* Copyright 2020 STMicroelectronics Inc.
*
* Lorenzo Bianconi <lorenzo.bianconi@st.com>
*/
#include <linux/module.h>
#include <linux/interrupt.h>
#include <linux/irq.h>
#include <linux/iio/iio.h>
#include <linux/iio/kfifo_buf.h>
#include <linux/iio/events.h>
#include <asm/unaligned.h>
#include <linux/iio/trigger_consumer.h>
#include <linux/iio/triggered_buffer.h>
#include <linux/iio/trigger.h>
#include <linux/iio/buffer.h>
#include <linux/of.h>
#include "st_lsm6dsr.h"
#define ST_LSM6DSR_REG_EMB_FUNC_STATUS_MAINPAGE 0x35
#define ST_LSM6DSR_REG_INT_STEP_DET_MASK BIT(3)
#define ST_LSM6DSR_REG_INT_TILT_MASK BIT(4)
#define ST_LSM6DSR_REG_INT_SIGMOT_MASK BIT(5)
#define ST_LSM6DSR_REG_INT_GLANCE_MASK BIT(0)
#define ST_LSM6DSR_REG_INT_MOTION_MASK BIT(1)
#define ST_LSM6DSR_REG_INT_NO_MOTION_MASK BIT(2)
#define ST_LSM6DSR_REG_INT_WAKEUP_MASK BIT(3)
#define ST_LSM6DSR_REG_INT_PICKUP_MASK BIT(4)
#define ST_LSM6DSR_REG_INT_ORIENTATION_MASK BIT(5)
#define ST_LSM6DSR_REG_INT_WRIST_MASK BIT(6)
#define ST_LSM6DSR_SAMPLE_DISCHARD 0x7ffd
#define ST_LSM6DSR_EWMA_LEVEL 120
#define ST_LSM6DSR_EWMA_DIV 128
enum {
ST_LSM6DSR_GYRO_TAG = 0x01,
ST_LSM6DSR_ACC_TAG = 0x02,
ST_LSM6DSR_TEMP_TAG = 0x03,
ST_LSM6DSR_TS_TAG = 0x04,
ST_LSM6DSR_EXT0_TAG = 0x0f,
ST_LSM6DSR_EXT1_TAG = 0x10,
ST_LSM6DSR_SC_TAG = 0x12,
};
/**
* Get Linux timestamp (SW)
*
* @return timestamp in ns
*/
static inline s64 st_lsm6dsr_get_time_ns(void)
{
return ktime_to_ns(ktime_get_boottime());
}
/**
* Timestamp low pass filter
*
* @param old: ST IMU MEMS hw instance
* @param new: ST IMU MEMS hw instance
* @param weight: ST IMU MEMS hw instance
* @return estimation of the timestamp average
*/
static inline s64 st_lsm6dsr_ewma(s64 old, s64 new, int weight)
{
s64 diff, incr;
diff = new - old;
incr = div_s64((ST_LSM6DSR_EWMA_DIV - weight) * diff,
ST_LSM6DSR_EWMA_DIV);
return old + incr;
}
/**
* Reset HW Timestamp counter and clear timestamp data structure
*
* @param hw: ST IMU MEMS hw instance
* @return < 0 if error, 0 otherwise
*/
inline int st_lsm6dsr_reset_hwts(struct st_lsm6dsr_hw *hw)
{
u8 data = 0xaa;
hw->ts = st_lsm6dsr_get_time_ns();
hw->ts_offset = hw->ts;
hw->val_ts_old = 0;
hw->hw_ts_high = 0;
hw->tsample = 0ull;
return st_lsm6dsr_write_atomic(hw, ST_LSM6DSR_REG_TIMESTAMP2_ADDR,
sizeof(data), &data);
}
/**
* Setting FIFO mode
*
* @param hw: ST IMU MEMS hw instance
* @param fifo_mode: ST_LSM6DSR_FIFO_BYPASS or ST_LSM6DSR_FIFO_CONT
* @return 0 FIFO configured accordingly, non zero otherwise
*/
int st_lsm6dsr_set_fifo_mode(struct st_lsm6dsr_hw *hw,
enum st_lsm6dsr_fifo_mode fifo_mode)
{
int err;
err = st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_REG_FIFO_CTRL4_ADDR,
ST_LSM6DSR_REG_FIFO_MODE_MASK,
fifo_mode);
if (err < 0)
return err;
hw->fifo_mode = fifo_mode;
return 0;
}
/**
* Setting sensor ODR in batching mode
*
* @param sensor: ST IMU sensor instance
* @param enable: enable or disable batching mode
* @return 0 FIFO configured accordingly, non zero otherwise
*/
int __st_lsm6dsr_set_sensor_batching_odr(struct st_lsm6dsr_sensor *sensor,
bool enable)
{
struct st_lsm6dsr_hw *hw = sensor->hw;
u8 data = 0;
int err;
int podr, puodr;
if (enable) {
err = st_lsm6dsr_get_odr_val(sensor->id, sensor->odr,
sensor->uodr, &podr, &puodr,
&data);
if (err < 0)
return err;
}
err = __st_lsm6dsr_write_with_mask(hw, sensor->batch_reg.addr,
sensor->batch_reg.mask, data);
return err < 0 ? err : 0;
}
/**
* Setting timestamp ODR in batching mode
*
* @param hw: ST IMU MEMS hw instance
* @return Timestamp ODR
*/
static int st_lsm6dsr_ts_odr(struct st_lsm6dsr_hw *hw)
{
struct st_lsm6dsr_sensor *sensor;
int odr = 0;
u8 i;
for (i = ST_LSM6DSR_ID_GYRO; i <= ST_LSM6DSR_ID_EXT1; i++) {
if (!hw->iio_devs[i])
continue;
sensor = iio_priv(hw->iio_devs[i]);
if (hw->enable_mask & BIT(sensor->id))
odr = max_t(int, odr, sensor->odr);
}
return odr;
}
/**
* Setting sensor ODR in batching mode
*
* @param sensor: ST IMU sensor instance
* @param enable: enable or disable batching mode
* @return 0 FIFO configured accordingly, non zero otherwise
*/
static inline int
st_lsm6dsr_set_sensor_batching_odr(struct st_lsm6dsr_sensor *sensor,
bool enable)
{
struct st_lsm6dsr_hw *hw = sensor->hw;
int err;
mutex_lock(&hw->page_lock);
err = __st_lsm6dsr_set_sensor_batching_odr(sensor, enable);
mutex_unlock(&hw->page_lock);
return err;
}
/**
* Update watermark level in FIFO
*
* @param sensor: ST IMU sensor instance
* @param watermark: New watermark level
* @return 0 if FIFO configured, non zero for error
*/
int st_lsm6dsr_update_watermark(struct st_lsm6dsr_sensor *sensor,
u16 watermark)
{
u16 fifo_watermark = ST_LSM6DSR_MAX_FIFO_DEPTH, cur_watermark = 0;
struct st_lsm6dsr_hw *hw = sensor->hw;
struct st_lsm6dsr_sensor *cur_sensor;
__le16 wdata;
int i, err;
u8 data;
for (i = ST_LSM6DSR_ID_GYRO; i <= ST_LSM6DSR_ID_STEP_COUNTER; i++) {
if (!hw->iio_devs[i])
continue;
cur_sensor = iio_priv(hw->iio_devs[i]);
if (!(hw->enable_mask & BIT(cur_sensor->id)))
continue;
cur_watermark = (cur_sensor == sensor) ? watermark :
cur_sensor->watermark;
fifo_watermark = min_t(u16, fifo_watermark, cur_watermark);
}
fifo_watermark = max_t(u16, fifo_watermark, 2);
err = st_lsm6dsr_read_atomic(hw, ST_LSM6DSR_REG_FIFO_CTRL1_ADDR + 1,
sizeof(data), &data);
if (err < 0)
goto out;
fifo_watermark = ((data << 8) & ~ST_LSM6DSR_REG_FIFO_WTM_MASK) |
(fifo_watermark & ST_LSM6DSR_REG_FIFO_WTM_MASK);
wdata = cpu_to_le16(fifo_watermark);
err = st_lsm6dsr_write_atomic(hw, ST_LSM6DSR_REG_FIFO_CTRL1_ADDR,
sizeof(wdata), (u8 *)&wdata);
out:
return err < 0 ? err : 0;
}
/**
* Timestamp correlation finction
*
* @param hw: ST IMU MEMS hw instance
* @param ts: New timestamp
*/
static inline void st_lsm6dsr_sync_hw_ts(struct st_lsm6dsr_hw *hw, s64 ts)
{
s64 delta = ts - hw->hw_ts;
hw->ts_offset = st_lsm6dsr_ewma(hw->ts_offset, delta,
ST_LSM6DSR_EWMA_LEVEL);
}
/**
* Return the iio device structure based on FIFO TAG ID
*
* @param hw: ST IMU MEMS hw instance
* @param tag: FIFO sample TAG ID
* @return 0 if FIFO configured, non zero for error
*/
static struct
iio_dev *st_lsm6dsr_get_iiodev_from_tag(struct st_lsm6dsr_hw *hw,
u8 tag)
{
struct iio_dev *iio_dev;
switch (tag) {
case ST_LSM6DSR_GYRO_TAG:
iio_dev = hw->iio_devs[ST_LSM6DSR_ID_GYRO];
break;
case ST_LSM6DSR_ACC_TAG:
iio_dev = hw->iio_devs[ST_LSM6DSR_ID_ACC];
break;
case ST_LSM6DSR_TEMP_TAG:
iio_dev = hw->iio_devs[ST_LSM6DSR_ID_TEMP];
break;
case ST_LSM6DSR_EXT0_TAG:
if (hw->enable_mask & BIT(ST_LSM6DSR_ID_EXT0))
iio_dev = hw->iio_devs[ST_LSM6DSR_ID_EXT0];
else
iio_dev = hw->iio_devs[ST_LSM6DSR_ID_EXT1];
break;
case ST_LSM6DSR_EXT1_TAG:
iio_dev = hw->iio_devs[ST_LSM6DSR_ID_EXT1];
break;
case ST_LSM6DSR_SC_TAG:
iio_dev = hw->iio_devs[ST_LSM6DSR_ID_STEP_COUNTER];
break;
default:
iio_dev = NULL;
break;
}
return iio_dev;
}
/**
* Read all FIFO data stored after WTM FIFO irq fired interrupt
*
* @param hw: ST IMU MEMS hw instance
* @return Number of read bytes in FIFO or error if negative
*/
static int st_lsm6dsr_read_fifo(struct st_lsm6dsr_hw *hw)
{
u8 iio_buf[ALIGN(ST_LSM6DSR_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)];
/* acc + gyro + 2 ext + ts + sc */
u8 buf[6 * ST_LSM6DSR_FIFO_SAMPLE_SIZE], tag, *ptr;
int i, err, word_len, fifo_len, read_len;
struct st_lsm6dsr_sensor *sensor;
struct iio_dev *iio_dev;
s64 ts_irq, hw_ts_old;
__le16 fifo_status;
u16 fifo_depth;
s16 drdymask;
u32 val;
ts_irq = hw->ts - hw->delta_ts;
err = st_lsm6dsr_read_atomic(hw, ST_LSM6DSR_REG_FIFO_STATUS1_ADDR,
sizeof(fifo_status), (u8 *)&fifo_status);
if (err < 0)
return err;
fifo_depth = le16_to_cpu(fifo_status) & ST_LSM6DSR_REG_FIFO_STATUS_DIFF;
if (!fifo_depth)
return 0;
fifo_len = fifo_depth * ST_LSM6DSR_FIFO_SAMPLE_SIZE;
read_len = 0;
while (read_len < fifo_len) {
word_len = min_t(int, fifo_len - read_len, sizeof(buf));
err = st_lsm6dsr_read_atomic(hw,
ST_LSM6DSR_REG_FIFO_DATA_OUT_TAG_ADDR,
word_len, buf);
if (err < 0)
return err;
for (i = 0; i < word_len; i += ST_LSM6DSR_FIFO_SAMPLE_SIZE) {
ptr = &buf[i + ST_LSM6DSR_TAG_SIZE];
tag = buf[i] >> 3;
if (tag == ST_LSM6DSR_TS_TAG) {
val = get_unaligned_le32(ptr);
if (hw->val_ts_old > val)
hw->hw_ts_high++;
hw_ts_old = hw->hw_ts;
/* check hw rollover */
hw->val_ts_old = val;
hw->hw_ts = (val +
((s64)hw->hw_ts_high << 32)) *
hw->ts_delta_ns;
hw->ts_offset = st_lsm6dsr_ewma(hw->ts_offset,
ts_irq - hw->hw_ts,
ST_LSM6DSR_EWMA_LEVEL);
if (!test_bit(ST_LSM6DSR_HW_FLUSH, &hw->state))
/* sync ap timestamp and sensor one */
st_lsm6dsr_sync_hw_ts(hw, ts_irq);
ts_irq += hw->hw_ts;
if (!hw->tsample)
hw->tsample = hw->ts_offset + hw->hw_ts;
else
hw->tsample = hw->tsample +
hw->hw_ts - hw_ts_old;
} else {
iio_dev =
st_lsm6dsr_get_iiodev_from_tag(hw, tag);
if (!iio_dev)
continue;
sensor = iio_priv(iio_dev);
/* skip samples if not ready */
drdymask =
(s16)le16_to_cpu(get_unaligned_le16(ptr));
if (unlikely(drdymask >=
ST_LSM6DSR_SAMPLE_DISCHARD)) {
continue;
}
/*
* hw ts in not queued in FIFO if only step
* counter enabled
*/
if (sensor->id == ST_LSM6DSR_ID_STEP_COUNTER) {
val = get_unaligned_le32(ptr + 2);
hw->tsample = (val +
((s64)hw->hw_ts_high << 32)) *
hw->ts_delta_ns;
/* avoid samples in the future */
hw->tsample = min_t(s64,
st_lsm6dsr_get_time_ns(),
hw->tsample);
} else {
hw->tsample = st_lsm6dsr_get_time_ns();
}
memcpy(iio_buf, ptr, ST_LSM6DSR_SAMPLE_SIZE);
sensor->last_fifo_timestamp = hw->tsample;
/* support decimation for ODR < 12.5 Hz */
if (sensor->dec_counter > 0) {
sensor->dec_counter--;
} else {
sensor->dec_counter = sensor->decimator;
iio_push_to_buffers_with_timestamp(iio_dev,
iio_buf,
hw->tsample);
}
}
}
read_len += word_len;
}
return read_len;
}
/**
* Report events after WTM FIFO irq fired interrupt
*
* @param hw: ST IMU MEMS hw instance
* @return 0 if OK, non zero for error
*/
static int st_lsm6dsr_report_events(struct st_lsm6dsr_hw *hw)
{
struct iio_dev *iio_dev;
u8 status[3];
s64 event;
int err;
if (hw->enable_mask & (BIT(ST_LSM6DSR_ID_STEP_DETECTOR) |
BIT(ST_LSM6DSR_ID_SIGN_MOTION) |
BIT(ST_LSM6DSR_ID_TILT) |
BIT(ST_LSM6DSR_ID_MOTION) |
BIT(ST_LSM6DSR_ID_NO_MOTION) |
BIT(ST_LSM6DSR_ID_WAKEUP) |
BIT(ST_LSM6DSR_ID_PICKUP) |
BIT(ST_LSM6DSR_ID_ORIENTATION) |
BIT(ST_LSM6DSR_ID_WRIST_TILT) |
BIT(ST_LSM6DSR_ID_GLANCE))) {
err = hw->tf->read(hw->dev,
ST_LSM6DSR_REG_EMB_FUNC_STATUS_MAINPAGE,
sizeof(status), status);
if (err < 0)
return err;
/* embedded function sensors */
if (status[0] & ST_LSM6DSR_REG_INT_STEP_DET_MASK) {
iio_dev = hw->iio_devs[ST_LSM6DSR_ID_STEP_DETECTOR];
event = IIO_UNMOD_EVENT_CODE(IIO_STEP_DETECTOR, -1,
IIO_EV_TYPE_THRESH,
IIO_EV_DIR_RISING);
iio_push_event(iio_dev, event,
st_lsm6dsr_get_time_ns());
}
if (status[0] & ST_LSM6DSR_REG_INT_SIGMOT_MASK) {
iio_dev = hw->iio_devs[ST_LSM6DSR_ID_SIGN_MOTION];
event = IIO_UNMOD_EVENT_CODE(IIO_SIGN_MOTION, -1,
IIO_EV_TYPE_THRESH,
IIO_EV_DIR_RISING);
iio_push_event(iio_dev, event,
st_lsm6dsr_get_time_ns());
}
if (status[0] & ST_LSM6DSR_REG_INT_TILT_MASK) {
iio_dev = hw->iio_devs[ST_LSM6DSR_ID_TILT];
event = IIO_UNMOD_EVENT_CODE(IIO_TILT, -1,
IIO_EV_TYPE_THRESH,
IIO_EV_DIR_RISING);
iio_push_event(iio_dev, event,
st_lsm6dsr_get_time_ns());
}
/* fsm sensors */
if (status[1] & ST_LSM6DSR_REG_INT_GLANCE_MASK) {
iio_dev = hw->iio_devs[ST_LSM6DSR_ID_GLANCE];
event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1,
IIO_EV_TYPE_THRESH,
IIO_EV_DIR_RISING);
iio_push_event(iio_dev, event,
st_lsm6dsr_get_time_ns());
}
if (status[1] & ST_LSM6DSR_REG_INT_MOTION_MASK) {
iio_dev = hw->iio_devs[ST_LSM6DSR_ID_MOTION];
event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1,
IIO_EV_TYPE_THRESH,
IIO_EV_DIR_RISING);
iio_push_event(iio_dev, event,
st_lsm6dsr_get_time_ns());
}
if (status[1] & ST_LSM6DSR_REG_INT_NO_MOTION_MASK) {
iio_dev = hw->iio_devs[ST_LSM6DSR_ID_NO_MOTION];
event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1,
IIO_EV_TYPE_THRESH,
IIO_EV_DIR_RISING);
iio_push_event(iio_dev, event,
st_lsm6dsr_get_time_ns());
}
if (status[1] & ST_LSM6DSR_REG_INT_WAKEUP_MASK) {
iio_dev = hw->iio_devs[ST_LSM6DSR_ID_WAKEUP];
event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1,
IIO_EV_TYPE_THRESH,
IIO_EV_DIR_RISING);
iio_push_event(iio_dev, event,
st_lsm6dsr_get_time_ns());
}
if (status[1] & ST_LSM6DSR_REG_INT_PICKUP_MASK) {
iio_dev = hw->iio_devs[ST_LSM6DSR_ID_PICKUP];
event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1,
IIO_EV_TYPE_THRESH,
IIO_EV_DIR_RISING);
iio_push_event(iio_dev, event,
st_lsm6dsr_get_time_ns());
}
if (status[1] & ST_LSM6DSR_REG_INT_ORIENTATION_MASK) {
struct st_lsm6dsr_sensor *sensor;
iio_dev = hw->iio_devs[ST_LSM6DSR_ID_ORIENTATION];
sensor = iio_priv(iio_dev);
iio_trigger_poll_chained(sensor->trig);
}
if (status[1] & ST_LSM6DSR_REG_INT_WRIST_MASK) {
iio_dev = hw->iio_devs[ST_LSM6DSR_ID_WRIST_TILT];
event = IIO_UNMOD_EVENT_CODE(IIO_GESTURE, -1,
IIO_EV_TYPE_THRESH,
IIO_EV_DIR_RISING);
iio_push_event(iio_dev, event,
st_lsm6dsr_get_time_ns());
}
}
return 0;
}
/**
* Return the max FIFO watermark level accepted
*
* @param dev: Linux Device
* @param attr: Device Attribute
* @param buf: User Buffer
* @return Number of chars printed into the buffer
*/
ssize_t st_lsm6dsr_get_max_watermark(struct device *dev,
struct device_attribute *attr,
char *buf)
{
struct iio_dev *iio_dev = dev_get_drvdata(dev);
struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
return sprintf(buf, "%d\n", sensor->max_watermark);
}
/**
* Return the FIFO watermark level
*
* @param dev: Linux Device
* @param attr: Device Attribute
* @param buf: User Buffer
* @return Number of chars printed into the buffer
*/
ssize_t st_lsm6dsr_get_watermark(struct device *dev,
struct device_attribute *attr,
char *buf)
{
struct iio_dev *iio_dev = dev_get_drvdata(dev);
struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
return sprintf(buf, "%d\n", sensor->watermark);
}
/**
* Set the FIFO watermark level
*
* @param dev: Linux Device
* @param attr: Device Attribute
* @param buf: User Buffer
* @param size: New FIFO watermark level
* @return Watermark level if >= 0, error otherwise
*/
ssize_t st_lsm6dsr_set_watermark(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t size)
{
struct iio_dev *iio_dev = dev_get_drvdata(dev);
struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
int err, val;
mutex_lock(&iio_dev->mlock);
err = kstrtoint(buf, 10, &val);
if (err < 0)
goto out;
err = st_lsm6dsr_update_watermark(sensor, val);
if (err < 0)
goto out;
sensor->watermark = val;
out:
mutex_unlock(&iio_dev->mlock);
return err < 0 ? err : size;
}
/**
* Flush internal HW FIFO
*
* @param dev: Linux Device
* @param attr: Device Attribute
* @param buf: User Buffer
* @param size: unused
* @return Watermark level if >= 0, error otherwise
*/
ssize_t st_lsm6dsr_flush_fifo(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t size)
{
struct iio_dev *iio_dev = dev_get_drvdata(dev);
struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
struct st_lsm6dsr_hw *hw = sensor->hw;
int count;
s64 ts;
#ifdef CONFIG_NO_GKI
s64 type;
s64 event;
s64 fts;
#endif
mutex_lock(&hw->fifo_lock);
ts = st_lsm6dsr_get_time_ns();
hw->delta_ts = ts - hw->ts;
hw->ts = ts;
set_bit(ST_LSM6DSR_HW_FLUSH, &hw->state);
count = st_lsm6dsr_read_fifo(hw);
sensor->dec_counter = 0;
mutex_unlock(&hw->fifo_lock);
#ifdef CONFIG_NO_GKI
if (count > 0)
fts = sensor->last_fifo_timestamp;
else
fts = ts;
type = count > 0 ? IIO_EV_DIR_FIFO_DATA : IIO_EV_DIR_FIFO_EMPTY;
event = IIO_UNMOD_EVENT_CODE(iio_dev->channels[0].type, -1,
IIO_EV_TYPE_FIFO_FLUSH, type);
iio_push_event(iio_dev, event, fts);
#endif
return size;
}
/**
* Empty FIFO and set HW FIFO in Bypass mode
*
* @param hw: ST IMU MEMS hw instance
* @return Watermark level if >= 0, error otherwise
*/
int st_lsm6dsr_suspend_fifo(struct st_lsm6dsr_hw *hw)
{
int err;
mutex_lock(&hw->fifo_lock);
st_lsm6dsr_read_fifo(hw);
err = st_lsm6dsr_set_fifo_mode(hw, ST_LSM6DSR_FIFO_BYPASS);
mutex_unlock(&hw->fifo_lock);
return err;
}
/**
* Update ODR batching in FIFO and Timestamp
*
* @param iio_dev: Linux IIO device
* @param enable: enable/disable batcing in FIFO
* @return < 0 if error, 0 otherwise
*/
int st_lsm6dsr_update_batching(struct iio_dev *iio_dev, bool enable)
{
struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
struct st_lsm6dsr_hw *hw = sensor->hw;
int err;
disable_irq(hw->irq);
err = st_lsm6dsr_set_sensor_batching_odr(sensor, enable);
if (err < 0)
goto out;
/* Calc TS ODR */
hw->odr = st_lsm6dsr_ts_odr(hw);
out:
enable_irq(hw->irq);
return err;
}
/**
* Update FIFO watermark value based to the enabled sensors
*
* @param iio_dev: Linux IIO device
* @param enable: enable/disable batcing in FIFO
* @return < 0 if error, 0 otherwise
*/
static int st_lsm6dsr_update_fifo(struct iio_dev *iio_dev, bool enable)
{
struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
struct st_lsm6dsr_hw *hw = sensor->hw;
int err;
int podr, puodr;
disable_irq(hw->irq);
if (sensor->id == ST_LSM6DSR_ID_EXT0 ||
sensor->id == ST_LSM6DSR_ID_EXT1) {
err = st_lsm6dsr_shub_set_enable(sensor, enable);
if (err < 0)
goto out;
} else {
if (sensor->id == ST_LSM6DSR_ID_STEP_COUNTER) {
err = st_lsm6dsr_step_counter_set_enable(sensor,
enable);
if (err < 0)
goto out;
} else {
err = st_lsm6dsr_sensor_set_enable(sensor, enable);
if (err < 0)
goto out;
err = st_lsm6dsr_set_sensor_batching_odr(sensor,
enable);
if (err < 0)
goto out;
}
}
/*
* this is an auxiliary sensor, it need to get batched
* together at least with a primary sensor (Acc/Gyro)
*/
if (sensor->id == ST_LSM6DSR_ID_TEMP) {
if (!(hw->enable_mask & (BIT(ST_LSM6DSR_ID_ACC) |
BIT(ST_LSM6DSR_ID_GYRO)))) {
struct st_lsm6dsr_sensor *acc_sensor;
u8 data = 0;
acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSR_ID_ACC]);
if (enable) {
err = st_lsm6dsr_get_odr_val(ST_LSM6DSR_ID_ACC,
sensor->odr, sensor->uodr,
&podr, &puodr, &data);
if (err < 0)
goto out;
}
err = st_lsm6dsr_write_with_mask(hw,
acc_sensor->batch_reg.addr,
acc_sensor->batch_reg.mask,
data);
if (err < 0)
goto out;
}
}
err = st_lsm6dsr_update_watermark(sensor, sensor->watermark);
if (err < 0)
goto out;
/* Calc TS ODR */
hw->odr = st_lsm6dsr_ts_odr(hw);
if (enable && hw->fifo_mode == ST_LSM6DSR_FIFO_BYPASS) {
st_lsm6dsr_reset_hwts(hw);
err = st_lsm6dsr_set_fifo_mode(hw, ST_LSM6DSR_FIFO_CONT);
} else if (!hw->enable_mask) {
err = st_lsm6dsr_set_fifo_mode(hw, ST_LSM6DSR_FIFO_BYPASS);
}
out:
enable_irq(hw->irq);
return err;
}
/**
* Bottom handler for FSM Orientation sensor event generation
*
* @param irq: IIO trigger irq number
* @param p: iio poll function environment
* @return IRQ_HANDLED or < 0 for error
*/
static irqreturn_t st_lsm6dsr_buffer_handler_thread(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *iio_dev = pf->indio_dev;
struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
u8 buffer[sizeof(u8) + sizeof(s64)];
int err;
err = st_lsm6dsr_fsm_get_orientation(sensor->hw, buffer);
if (err < 0)
goto out;
iio_push_to_buffers_with_timestamp(iio_dev, buffer,
st_lsm6dsr_get_time_ns());
out:
iio_trigger_notify_done(sensor->trig);
return IRQ_HANDLED;
}
/**
* Top handler for sensor event generation + FIFO management
*
* @param irq: IIO trigger irq number
* @param private: iio poll function environment
* @return IRQ_HANDLED or < 0 for error
*/
static irqreturn_t st_lsm6dsr_handler_irq(int irq, void *private)
{
struct st_lsm6dsr_hw *hw = (struct st_lsm6dsr_hw *)private;
s64 ts = st_lsm6dsr_get_time_ns();
hw->delta_ts = ts - hw->ts;
hw->ts = ts;
return IRQ_WAKE_THREAD;
}
/**
* Bottom handler for sensor event generation + FIFO management
*
* @param irq: irq line number
* @param private: device private environment pointer
* @return IRQ_HANDLED or < 0 for error
*/
static irqreturn_t st_lsm6dsr_handler_thread(int irq, void *private)
{
struct st_lsm6dsr_hw *hw = (struct st_lsm6dsr_hw *)private;
mutex_lock(&hw->fifo_lock);
st_lsm6dsr_read_fifo(hw);
clear_bit(ST_LSM6DSR_HW_FLUSH, &hw->state);
mutex_unlock(&hw->fifo_lock);
st_lsm6dsr_report_events(hw);
return IRQ_HANDLED;
}
/**
* IIO fifo pre enabled callback function
*
* @param iio_dev: IIO device
* @return < 0 if error, 0 otherwise
*/
static int st_lsm6dsr_fifo_preenable(struct iio_dev *iio_dev)
{
return st_lsm6dsr_update_fifo(iio_dev, true);
}
/**
* IIO fifo post disable callback function
*
* @param iio_dev: IIO device
* @return < 0 if error, 0 otherwise
*/
static int st_lsm6dsr_fifo_postdisable(struct iio_dev *iio_dev)
{
return st_lsm6dsr_update_fifo(iio_dev, false);
}
/**
* IIO fifo callback registruction structure
*/
static const struct iio_buffer_setup_ops st_lsm6dsr_fifo_ops = {
.preenable = st_lsm6dsr_fifo_preenable,
.postdisable = st_lsm6dsr_fifo_postdisable,
};
/**
* Enable HW FIFO
*
* @param hw: ST IMU MEMS hw instance
* @return < 0 if error, 0 otherwise
*/
static int st_lsm6dsr_fifo_init(struct st_lsm6dsr_hw *hw)
{
return st_lsm6dsr_write_with_mask(hw,
ST_LSM6DSR_REG_FIFO_CTRL4_ADDR,
ST_LSM6DSR_REG_DEC_TS_MASK, 1);
}
static const struct iio_trigger_ops st_lsm6dsr_trigger_ops;
static int st_lsm6dsr_buffer_preenable(struct iio_dev *iio_dev)
{
return st_lsm6dsr_embfunc_sensor_set_enable(iio_priv(iio_dev), true);
}
static int st_lsm6dsr_buffer_postdisable(struct iio_dev *iio_dev)
{
return st_lsm6dsr_embfunc_sensor_set_enable(iio_priv(iio_dev), false);
}
static const struct iio_buffer_setup_ops st_lsm6dsr_buffer_ops = {
.preenable = st_lsm6dsr_buffer_preenable,
.postenable = NULL,
.predisable = NULL,
.postdisable = st_lsm6dsr_buffer_postdisable,
};
/**
* Init IRQ
*
* @param hw: ST IMU MEMS hw instance
* @return < 0 if error, 0 otherwise
*/
int st_lsm6dsr_irq_setup(struct st_lsm6dsr_hw *hw)
{
unsigned long irq_type;
bool irq_active_low;
int err;
irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
if (irq_type == IRQF_TRIGGER_NONE)
irq_type = IRQF_TRIGGER_HIGH;
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 = st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_REG_CTRL3_C_ADDR,
ST_LSM6DSR_REG_H_LACTIVE_MASK,
irq_active_low);
if (err < 0)
return err;
err = st_lsm6dsr_write_with_mask(hw, ST_LSM6DSR_REG_INT1_CTRL_ADDR,
ST_LSM6DSR_REG_INT_FIFO_TH_MASK, 1);
if (err < 0)
return err;
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 device_node *np = hw->dev->of_node;
struct st_lsm6dsr_sensor *sensor;
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;
if (np && of_property_read_bool(np, "drive-open-drain")) {
err = st_lsm6dsr_write_with_mask(hw,
ST_LSM6DSR_REG_CTRL3_C_ADDR,
ST_LSM6DSR_REG_PP_OD_MASK, 1);
if (err < 0)
return err;
irq_type |= IRQF_SHARED;
}
err = devm_request_threaded_irq(hw->dev, hw->irq,
st_lsm6dsr_handler_irq,
st_lsm6dsr_handler_thread,
irq_type | IRQF_ONESHOT,
"lsm6dsr", hw);
if (err) {
dev_err(hw->dev, "failed to request trigger irq %d\n",
hw->irq);
return err;
}
for (i = ST_LSM6DSR_ID_GYRO; i <= ST_LSM6DSR_ID_SIGN_MOTION; i++) {
if (!hw->iio_devs[i])
continue;
err = devm_iio_kfifo_buffer_setup(hw->dev, hw->iio_devs[i],
&st_lsm6dsr_fifo_ops);
if (err)
return err;
}
err = st_lsm6dsr_fifo_init(hw);
if (err < 0)
return err;
iio_dev = hw->iio_devs[ST_LSM6DSR_ID_ORIENTATION];
sensor = iio_priv(iio_dev);
err = iio_triggered_buffer_setup(iio_dev,
NULL, st_lsm6dsr_buffer_handler_thread,
&st_lsm6dsr_buffer_ops);
if (err < 0)
return err;
sensor->trig = devm_iio_trigger_alloc(hw->dev, "%s-trigger",
iio_dev->name);
if (!sensor->trig)
return -ENOMEM;
iio_trigger_set_drvdata(sensor->trig, iio_dev);
sensor->trig->ops = &st_lsm6dsr_trigger_ops;
sensor->trig->dev.parent = hw->dev;
sensor->trig->owner = THIS_MODULE;
iio_dev->trig = iio_trigger_get(sensor->trig);
err = iio_trigger_register(sensor->trig);
if (err < 0)
iio_triggered_buffer_cleanup(iio_dev);
return err;
}
int st_lsm6dsr_deallocate_buffers(struct st_lsm6dsr_hw *hw)
{
struct iio_dev *iio_dev = hw->iio_devs[ST_LSM6DSR_ID_ORIENTATION];
struct st_lsm6dsr_sensor *sensor = iio_priv(iio_dev);
iio_trigger_unregister(sensor->trig);
iio_triggered_buffer_cleanup(iio_dev);
return 0;
}
Reference in New Issue View Git Blame Copy Permalink