686 lines
22 KiB
C
686 lines
22 KiB
C
// SPDX-License-Identifier: GPL-2.0-only
|
|
/*
|
|
* STMicroelectronics lsm6dsm buffer driver
|
|
*
|
|
* MEMS Software Solutions Team
|
|
*
|
|
* Copyright 2016 STMicroelectronics Inc.
|
|
*/
|
|
|
|
#include <linux/module.h>
|
|
#include <linux/kernel.h>
|
|
#include <linux/slab.h>
|
|
#include <linux/stat.h>
|
|
#include <linux/interrupt.h>
|
|
#include <linux/i2c.h>
|
|
#include <linux/delay.h>
|
|
#include <linux/iio/iio.h>
|
|
#include <linux/iio/buffer.h>
|
|
#include <linux/iio/trigger_consumer.h>
|
|
#include <linux/iio/triggered_buffer.h>
|
|
#include <linux/version.h>
|
|
|
|
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4,11,0)
|
|
#include <linux/iio/buffer_impl.h>
|
|
#endif /* LINUX_VERSION_CODE */
|
|
|
|
#include "st_lsm6dsm.h"
|
|
|
|
#define ST_LSM6DSM_FIFO_DIFF_L 0x3a
|
|
#define ST_LSM6DSM_FIFO_DIFF_MASK 0x07
|
|
#define ST_LSM6DSM_FIFO_DATA_OUT_L 0x3e
|
|
#define ST_LSM6DSM_FIFO_DATA_OVR 0x40
|
|
#define ST_LSM6DSM_FIFO_DATA_EMPTY 0x10
|
|
#define ST_LSM6DSM_STEP_MASK_64BIT (0xFFFFFFFFFFFF0000)
|
|
|
|
#define MIN_ID(a, b, c, d) (((a) < (b)) ? ((a == 0) ? \
|
|
(d) : (c)) : ((b == 0) ? \
|
|
(c) : (d)))
|
|
|
|
int st_lsm6dsm_push_data_with_timestamp(struct lsm6dsm_data *cdata,
|
|
u8 index, u8 *data, int64_t timestamp)
|
|
{
|
|
int i, n = 0;
|
|
struct iio_chan_spec const *chs = cdata->indio_dev[index]->channels;
|
|
uint16_t bfch, bfchs_out = 0, bfchs_in = 0;
|
|
struct lsm6dsm_sensor_data *sdata = iio_priv(cdata->indio_dev[index]);
|
|
u8 buff[ALIGN(ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE, sizeof(s64)) + sizeof(s64)];
|
|
|
|
if (timestamp <= cdata->fifo_output[index].timestamp_p)
|
|
return -EINVAL;
|
|
|
|
for (i = 0; i < sdata->num_data_channels; i++) {
|
|
bfch = chs[i].scan_type.storagebits >> 3;
|
|
|
|
if (test_bit(i, cdata->indio_dev[index]->active_scan_mask)) {
|
|
memcpy(&buff[bfchs_out], &data[bfchs_in], bfch);
|
|
n++;
|
|
bfchs_out += bfch;
|
|
}
|
|
|
|
bfchs_in += bfch;
|
|
}
|
|
|
|
iio_push_to_buffers_with_timestamp(cdata->indio_dev[index],
|
|
buff, timestamp);
|
|
|
|
cdata->fifo_output[index].timestamp_p = timestamp;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static void st_lsm6dsm_parse_fifo_data(struct lsm6dsm_data *cdata,
|
|
u16 read_len, int64_t time_top, u16 num_pattern)
|
|
{
|
|
int err;
|
|
u16 fifo_offset = 0;
|
|
u8 gyro_sip, accel_sip;
|
|
#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT
|
|
u8 ext0_sip;
|
|
#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
|
|
|
|
while (fifo_offset < read_len) {
|
|
gyro_sip = cdata->fifo_output[ST_MASK_ID_GYRO].sip;
|
|
accel_sip = cdata->fifo_output[ST_MASK_ID_ACCEL].sip;
|
|
#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT
|
|
ext0_sip = cdata->fifo_output[ST_MASK_ID_EXT0].sip;
|
|
#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
|
|
|
|
do {
|
|
if (gyro_sip > 0) {
|
|
if (cdata->fifo_output[ST_MASK_ID_GYRO].timestamp == 0) {
|
|
if (cdata->slower_id == ST_MASK_ID_GYRO)
|
|
cdata->fifo_output[ST_MASK_ID_GYRO].timestamp = time_top -
|
|
(num_pattern * gyro_sip * cdata->fifo_output[ST_MASK_ID_GYRO].deltatime) - 300000;
|
|
else
|
|
cdata->fifo_output[ST_MASK_ID_GYRO].timestamp = time_top -
|
|
(num_pattern * gyro_sip * cdata->fifo_output[ST_MASK_ID_GYRO].deltatime) - 300000 -
|
|
(cdata->fifo_output[cdata->slower_id].deltatime - cdata->fifo_output[ST_MASK_ID_GYRO].deltatime);
|
|
} else
|
|
cdata->fifo_output[ST_MASK_ID_GYRO].timestamp += cdata->fifo_output[ST_MASK_ID_GYRO].deltatime;
|
|
|
|
if (cdata->fifo_output[ST_MASK_ID_GYRO].timestamp > time_top) {
|
|
cdata->fifo_output[ST_MASK_ID_GYRO].timestamp -= cdata->fifo_output[ST_MASK_ID_GYRO].deltatime;
|
|
cdata->samples_to_discard[ST_MASK_ID_GYRO] = 1;
|
|
}
|
|
|
|
if (cdata->samples_to_discard[ST_MASK_ID_GYRO] > 0)
|
|
cdata->samples_to_discard[ST_MASK_ID_GYRO]--;
|
|
else {
|
|
cdata->fifo_output[ST_MASK_ID_GYRO].num_samples++;
|
|
|
|
if (cdata->fifo_output[ST_MASK_ID_GYRO].num_samples >= cdata->fifo_output[ST_MASK_ID_GYRO].decimator) {
|
|
cdata->fifo_output[ST_MASK_ID_GYRO].num_samples = 0;
|
|
|
|
if (cdata->sensors_enabled & BIT(ST_MASK_ID_GYRO)) {
|
|
if (cdata->samples_to_discard_2[ST_MASK_ID_GYRO] == 0) {
|
|
err = st_lsm6dsm_push_data_with_timestamp(
|
|
cdata, ST_MASK_ID_GYRO,
|
|
&cdata->fifo_data[fifo_offset],
|
|
cdata->fifo_output[ST_MASK_ID_GYRO].timestamp);
|
|
|
|
if (err >= 0)
|
|
cdata->fifo_output[ST_MASK_ID_GYRO].initialized = true;
|
|
|
|
memcpy(cdata->gyro_last_push, &cdata->fifo_data[fifo_offset], 6);
|
|
} else {
|
|
cdata->samples_to_discard_2[ST_MASK_ID_GYRO]--;
|
|
|
|
if (cdata->fifo_output[ST_MASK_ID_GYRO].initialized) {
|
|
err = st_lsm6dsm_push_data_with_timestamp(
|
|
cdata, ST_MASK_ID_GYRO,
|
|
cdata->gyro_last_push,
|
|
cdata->fifo_output[ST_MASK_ID_GYRO].timestamp);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
fifo_offset += ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE;
|
|
gyro_sip--;
|
|
}
|
|
|
|
if (accel_sip > 0) {
|
|
if (cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp == 0) {
|
|
if (cdata->slower_id == ST_MASK_ID_ACCEL)
|
|
cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp = time_top -
|
|
(num_pattern * accel_sip * cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime) - 300000;
|
|
else
|
|
cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp = time_top -
|
|
(num_pattern * accel_sip * cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime) - 300000 -
|
|
(cdata->fifo_output[cdata->slower_id].deltatime - cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime);
|
|
} else
|
|
cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp += cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime;
|
|
|
|
if (cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp > time_top) {
|
|
cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp -= cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime;
|
|
cdata->samples_to_discard[ST_MASK_ID_ACCEL] = 1;
|
|
}
|
|
|
|
if (cdata->samples_to_discard[ST_MASK_ID_ACCEL] > 0)
|
|
cdata->samples_to_discard[ST_MASK_ID_ACCEL]--;
|
|
else {
|
|
cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples++;
|
|
|
|
if (cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples >= cdata->fifo_output[ST_MASK_ID_ACCEL].decimator) {
|
|
cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples = 0;
|
|
|
|
if (cdata->sensors_enabled & BIT(ST_MASK_ID_ACCEL)) {
|
|
if (cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] == 0) {
|
|
err = st_lsm6dsm_push_data_with_timestamp(
|
|
cdata, ST_MASK_ID_ACCEL,
|
|
&cdata->fifo_data[fifo_offset],
|
|
cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp);
|
|
|
|
if (err >= 0)
|
|
cdata->fifo_output[ST_MASK_ID_ACCEL].initialized = true;
|
|
|
|
memcpy(cdata->accel_last_push, &cdata->fifo_data[fifo_offset], 6);
|
|
} else {
|
|
cdata->samples_to_discard_2[ST_MASK_ID_ACCEL]--;
|
|
|
|
if (cdata->fifo_output[ST_MASK_ID_ACCEL].initialized) {
|
|
err = st_lsm6dsm_push_data_with_timestamp(
|
|
cdata, ST_MASK_ID_ACCEL,
|
|
cdata->accel_last_push,
|
|
cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
fifo_offset += ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE;
|
|
accel_sip--;
|
|
}
|
|
|
|
#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT
|
|
if (ext0_sip > 0) {
|
|
if (cdata->fifo_output[ST_MASK_ID_EXT0].timestamp == 0) {
|
|
if (cdata->slower_id == ST_MASK_ID_EXT0)
|
|
cdata->fifo_output[ST_MASK_ID_EXT0].timestamp = time_top -
|
|
(num_pattern * ext0_sip * cdata->fifo_output[ST_MASK_ID_EXT0].deltatime) - 300000;
|
|
else
|
|
cdata->fifo_output[ST_MASK_ID_EXT0].timestamp = time_top -
|
|
(num_pattern * ext0_sip * cdata->fifo_output[ST_MASK_ID_EXT0].deltatime) - 300000 -
|
|
(cdata->fifo_output[cdata->slower_id].deltatime - cdata->fifo_output[ST_MASK_ID_EXT0].deltatime);
|
|
} else
|
|
cdata->fifo_output[ST_MASK_ID_EXT0].timestamp += cdata->fifo_output[ST_MASK_ID_EXT0].deltatime;
|
|
|
|
if (cdata->fifo_output[ST_MASK_ID_EXT0].timestamp > time_top) {
|
|
cdata->fifo_output[ST_MASK_ID_EXT0].timestamp -= cdata->fifo_output[ST_MASK_ID_EXT0].deltatime;
|
|
cdata->samples_to_discard[ST_MASK_ID_EXT0] = 1;
|
|
}
|
|
|
|
if (cdata->samples_to_discard[ST_MASK_ID_EXT0] > 0)
|
|
cdata->samples_to_discard[ST_MASK_ID_EXT0]--;
|
|
else {
|
|
cdata->fifo_output[ST_MASK_ID_EXT0].num_samples++;
|
|
|
|
if (cdata->fifo_output[ST_MASK_ID_EXT0].num_samples >= cdata->fifo_output[ST_MASK_ID_EXT0].decimator) {
|
|
cdata->fifo_output[ST_MASK_ID_EXT0].num_samples = 0;
|
|
|
|
if (cdata->sensors_enabled & BIT(ST_MASK_ID_EXT0)) {
|
|
if (cdata->samples_to_discard_2[ST_MASK_ID_EXT0] == 0) {
|
|
err = st_lsm6dsm_push_data_with_timestamp(
|
|
cdata, ST_MASK_ID_EXT0,
|
|
&cdata->fifo_data[fifo_offset],
|
|
cdata->fifo_output[ST_MASK_ID_EXT0].timestamp);
|
|
|
|
if (err >= 0)
|
|
cdata->fifo_output[ST_MASK_ID_EXT0].initialized = true;
|
|
|
|
memcpy(cdata->ext0_last_push, &cdata->fifo_data[fifo_offset], 6);
|
|
} else {
|
|
cdata->samples_to_discard_2[ST_MASK_ID_EXT0]--;
|
|
|
|
if (cdata->fifo_output[ST_MASK_ID_EXT0].initialized) {
|
|
err = st_lsm6dsm_push_data_with_timestamp(
|
|
cdata, ST_MASK_ID_EXT0,
|
|
cdata->ext0_last_push,
|
|
cdata->fifo_output[ST_MASK_ID_EXT0].timestamp);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
fifo_offset += ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE;
|
|
ext0_sip--;
|
|
}
|
|
|
|
} while ((accel_sip > 0) || (gyro_sip > 0) || (ext0_sip > 0));
|
|
#else /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
|
|
} while ((accel_sip > 0) || (gyro_sip > 0));
|
|
#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
|
|
}
|
|
}
|
|
|
|
int st_lsm6dsm_read_fifo(struct lsm6dsm_data *cdata, bool async)
|
|
{
|
|
int err;
|
|
u8 fifo_status[2];
|
|
#if (CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO > 0)
|
|
u16 data_remaining, data_to_read;
|
|
#endif /* CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO */
|
|
u16 read_len = 0, byte_in_pattern, num_pattern;
|
|
int64_t temp_counter = 0, timestamp_diff, slower_deltatime;
|
|
|
|
err = cdata->tf->read(cdata, ST_LSM6DSM_FIFO_DIFF_L,
|
|
2, fifo_status, true);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
timestamp_diff = iio_get_time_ns(cdata->indio_dev[ST_MASK_ID_ACCEL]);
|
|
|
|
if (fifo_status[1] & ST_LSM6DSM_FIFO_DATA_OVR) {
|
|
st_lsm6dsm_set_fifo_mode(cdata, BYPASS);
|
|
st_lsm6dsm_set_fifo_mode(cdata, CONTINUOS);
|
|
dev_err(cdata->dev, "data fifo overrun, failed to read it.\n");
|
|
return -EINVAL;
|
|
}
|
|
|
|
if (fifo_status[1] & ST_LSM6DSM_FIFO_DATA_EMPTY)
|
|
return 0;
|
|
|
|
read_len = ((fifo_status[1] & ST_LSM6DSM_FIFO_DIFF_MASK) << 8) | fifo_status[0];
|
|
read_len *= ST_LSM6DSM_BYTE_FOR_CHANNEL;
|
|
|
|
#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT
|
|
byte_in_pattern = (cdata->fifo_output[ST_MASK_ID_ACCEL].sip +
|
|
cdata->fifo_output[ST_MASK_ID_GYRO].sip +
|
|
cdata->fifo_output[ST_MASK_ID_EXT0].sip) *
|
|
ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE;
|
|
#else /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
|
|
byte_in_pattern = (cdata->fifo_output[ST_MASK_ID_ACCEL].sip +
|
|
cdata->fifo_output[ST_MASK_ID_GYRO].sip) *
|
|
ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE;
|
|
#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
|
|
if (byte_in_pattern == 0)
|
|
return 0;
|
|
|
|
num_pattern = read_len / byte_in_pattern;
|
|
|
|
read_len = (read_len / byte_in_pattern) * byte_in_pattern;
|
|
if (read_len == 0)
|
|
return 0;
|
|
|
|
#if (CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO == 0)
|
|
err = cdata->tf->read(cdata, ST_LSM6DSM_FIFO_DATA_OUT_L,
|
|
read_len, cdata->fifo_data, true);
|
|
if (err < 0)
|
|
return err;
|
|
#else /* CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO */
|
|
data_remaining = read_len;
|
|
|
|
do {
|
|
if (data_remaining > CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO)
|
|
data_to_read = CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO;
|
|
else
|
|
data_to_read = data_remaining;
|
|
|
|
err = cdata->tf->read(cdata, ST_LSM6DSM_FIFO_DATA_OUT_L,
|
|
data_to_read,
|
|
&cdata->fifo_data[read_len - data_remaining], true);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
data_remaining -= data_to_read;
|
|
} while (data_remaining > 0);
|
|
#endif /* CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO */
|
|
|
|
cdata->slower_id = MIN_ID(cdata->fifo_output[ST_MASK_ID_GYRO].sip,
|
|
cdata->fifo_output[ST_MASK_ID_ACCEL].sip,
|
|
ST_MASK_ID_GYRO, ST_MASK_ID_ACCEL);
|
|
#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT
|
|
cdata->slower_id = MIN_ID(cdata->fifo_output[cdata->slower_id].sip,
|
|
cdata->fifo_output[ST_MASK_ID_EXT0].sip,
|
|
cdata->slower_id, ST_MASK_ID_EXT0);
|
|
#endif /* CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO */
|
|
|
|
temp_counter = cdata->slower_counter;
|
|
cdata->slower_counter += (read_len / byte_in_pattern) * cdata->fifo_output[cdata->slower_id].sip;
|
|
|
|
if (async)
|
|
goto parse_fifo;
|
|
|
|
if (temp_counter > 0) {
|
|
slower_deltatime = div64_s64(timestamp_diff - cdata->fifo_enable_timestamp, cdata->slower_counter);
|
|
|
|
switch (cdata->slower_id) {
|
|
case ST_MASK_ID_ACCEL:
|
|
if (cdata->fifo_output[ST_MASK_ID_GYRO].sip != 0)
|
|
cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = div64_s64(slower_deltatime *
|
|
cdata->fifo_output[ST_MASK_ID_ACCEL].sip, cdata->fifo_output[ST_MASK_ID_GYRO].sip);
|
|
|
|
if (cdata->fifo_output[ST_MASK_ID_EXT0].sip != 0)
|
|
cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = div64_s64(slower_deltatime *
|
|
cdata->fifo_output[ST_MASK_ID_ACCEL].sip, cdata->fifo_output[ST_MASK_ID_EXT0].sip);
|
|
|
|
cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = slower_deltatime;
|
|
break;
|
|
|
|
case ST_MASK_ID_GYRO:
|
|
if (cdata->fifo_output[ST_MASK_ID_ACCEL].sip != 0)
|
|
cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = div64_s64(slower_deltatime *
|
|
cdata->fifo_output[ST_MASK_ID_GYRO].sip, cdata->fifo_output[ST_MASK_ID_ACCEL].sip);
|
|
|
|
if (cdata->fifo_output[ST_MASK_ID_EXT0].sip != 0)
|
|
cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = div64_s64(slower_deltatime *
|
|
cdata->fifo_output[ST_MASK_ID_GYRO].sip, cdata->fifo_output[ST_MASK_ID_EXT0].sip);
|
|
|
|
cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = slower_deltatime;
|
|
break;
|
|
|
|
case ST_MASK_ID_EXT0:
|
|
if (cdata->fifo_output[ST_MASK_ID_ACCEL].sip != 0)
|
|
cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = div64_s64(slower_deltatime *
|
|
cdata->fifo_output[ST_MASK_ID_EXT0].sip, cdata->fifo_output[ST_MASK_ID_ACCEL].sip);
|
|
|
|
if (cdata->fifo_output[ST_MASK_ID_GYRO].sip != 0)
|
|
cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = div64_s64(slower_deltatime *
|
|
cdata->fifo_output[ST_MASK_ID_EXT0].sip, cdata->fifo_output[ST_MASK_ID_GYRO].sip);
|
|
|
|
cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = slower_deltatime;
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
} else {
|
|
cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime_default;
|
|
cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = cdata->fifo_output[ST_MASK_ID_GYRO].deltatime_default;
|
|
#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT
|
|
cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = cdata->fifo_output[ST_MASK_ID_EXT0].deltatime_default;
|
|
#endif /* CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO */
|
|
}
|
|
|
|
parse_fifo:
|
|
st_lsm6dsm_parse_fifo_data(cdata, read_len, timestamp_diff, num_pattern);
|
|
|
|
return 0;
|
|
}
|
|
|
|
int lsm6dsm_read_output_data(struct lsm6dsm_data *cdata, int sindex, bool push)
|
|
{
|
|
int err;
|
|
u8 data[6];
|
|
struct iio_dev *indio_dev = cdata->indio_dev[sindex];
|
|
struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev);
|
|
|
|
err = cdata->tf->read(cdata, sdata->data_out_reg,
|
|
ST_LSM6DSM_BYTE_FOR_CHANNEL * 3, data, true);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
if (push)
|
|
st_lsm6dsm_push_data_with_timestamp(cdata, sindex,
|
|
data, cdata->timestamp);
|
|
|
|
return 0;
|
|
}
|
|
EXPORT_SYMBOL(lsm6dsm_read_output_data);
|
|
|
|
static irqreturn_t st_lsm6dsm_outdata_trigger_handler(int irq, void *p)
|
|
{
|
|
struct iio_poll_func *pf = p;
|
|
struct iio_dev *indio_dev = pf->indio_dev;
|
|
|
|
iio_trigger_notify_done(indio_dev->trig);
|
|
return IRQ_HANDLED;
|
|
}
|
|
|
|
static irqreturn_t st_lsm6dsm_step_counter_trigger_handler(int irq, void *p)
|
|
{
|
|
int err;
|
|
u8 steps_data[2];
|
|
int64_t timestamp = 0;
|
|
struct iio_poll_func *pf = p;
|
|
struct iio_dev *indio_dev = pf->indio_dev;
|
|
struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev);
|
|
u8 buff[ALIGN(ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE, sizeof(s64)) + sizeof(s64)];
|
|
|
|
if (!sdata->cdata->reset_steps) {
|
|
err = sdata->cdata->tf->read(sdata->cdata,
|
|
(u8)indio_dev->channels[0].address,
|
|
ST_LSM6DSM_BYTE_FOR_CHANNEL,
|
|
steps_data, true);
|
|
if (err < 0)
|
|
goto st_lsm6dsm_step_counter_done;
|
|
|
|
sdata->cdata->num_steps = (sdata->cdata->num_steps &
|
|
ST_LSM6DSM_STEP_MASK_64BIT) + *((u16 *)steps_data);
|
|
timestamp = sdata->cdata->timestamp;
|
|
} else {
|
|
sdata->cdata->num_steps = 0;
|
|
timestamp = iio_get_time_ns(indio_dev);
|
|
sdata->cdata->reset_steps = false;
|
|
}
|
|
|
|
memcpy(buff, (u8 *)&sdata->cdata->num_steps, sizeof(u64));
|
|
iio_push_to_buffers_with_timestamp(indio_dev, buff, timestamp);
|
|
|
|
st_lsm6dsm_step_counter_done:
|
|
iio_trigger_notify_done(indio_dev->trig);
|
|
return IRQ_HANDLED;
|
|
}
|
|
|
|
static irqreturn_t st_lsm6dsm_wrist_tilt_trigger_handler(int irq, void *p)
|
|
{
|
|
int err;
|
|
u8 wrist_tilt_gesture;
|
|
int64_t timestamp;
|
|
struct iio_poll_func *pf = p;
|
|
struct iio_dev *indio_dev = pf->indio_dev;
|
|
struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev);
|
|
u8 buff[ALIGN(ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE, sizeof(s64)) + sizeof(s64)];
|
|
|
|
err = sdata->cdata->tf->read(sdata->cdata,
|
|
(u8)indio_dev->channels[0].address,
|
|
ST_LSM6DSM_BYTE_FOR_WRIST_TILT,
|
|
&wrist_tilt_gesture, true);
|
|
if (err < 0)
|
|
goto st_lsm6dsm_wrist_tilt_done;
|
|
|
|
buff[0] = wrist_tilt_gesture;
|
|
timestamp = sdata->cdata->timestamp;
|
|
|
|
iio_push_to_buffers_with_timestamp(indio_dev, buff, timestamp);
|
|
|
|
st_lsm6dsm_wrist_tilt_done:
|
|
iio_trigger_notify_done(indio_dev->trig);
|
|
|
|
return IRQ_HANDLED;
|
|
}
|
|
|
|
static inline irqreturn_t st_lsm6dsm_handler_empty(int irq, void *p)
|
|
{
|
|
return IRQ_HANDLED;
|
|
}
|
|
|
|
int st_lsm6dsm_trig_set_state(struct iio_trigger *trig, bool state)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
static int st_lsm6dsm_buffer_preenable(struct iio_dev *indio_dev)
|
|
{
|
|
#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION
|
|
struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev);
|
|
|
|
if (sdata->cdata->injection_mode) {
|
|
switch (sdata->sindex) {
|
|
case ST_MASK_ID_ACCEL:
|
|
case ST_MASK_ID_GYRO:
|
|
return -EBUSY;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int st_lsm6dsm_buffer_postenable(struct iio_dev *indio_dev)
|
|
{
|
|
int err;
|
|
struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev);
|
|
|
|
sdata->cdata->fifo_output[sdata->sindex].initialized = false;
|
|
|
|
if ((sdata->cdata->hwfifo_enabled[sdata->sindex]) &&
|
|
(indio_dev->buffer->length < 2 * ST_LSM6DSM_MAX_FIFO_LENGHT))
|
|
return -EINVAL;
|
|
|
|
mutex_lock(&sdata->cdata->odr_lock);
|
|
|
|
err = st_lsm6dsm_set_enable(sdata, true, true);
|
|
if (err < 0) {
|
|
mutex_unlock(&sdata->cdata->odr_lock);
|
|
return err;
|
|
}
|
|
|
|
mutex_unlock(&sdata->cdata->odr_lock);
|
|
|
|
if (sdata->sindex == ST_MASK_ID_STEP_COUNTER)
|
|
iio_trigger_poll_chained(sdata->cdata->trig[ST_MASK_ID_STEP_COUNTER]);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int st_lsm6dsm_buffer_postdisable(struct iio_dev *indio_dev)
|
|
{
|
|
int err;
|
|
struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev);
|
|
|
|
mutex_lock(&sdata->cdata->odr_lock);
|
|
|
|
err = st_lsm6dsm_set_enable(sdata, false, true);
|
|
|
|
mutex_unlock(&sdata->cdata->odr_lock);
|
|
|
|
return err < 0 ? err : 0;
|
|
}
|
|
|
|
static const struct iio_buffer_setup_ops st_lsm6dsm_buffer_setup_ops = {
|
|
.preenable = &st_lsm6dsm_buffer_preenable,
|
|
.postenable = &st_lsm6dsm_buffer_postenable,
|
|
.postdisable = &st_lsm6dsm_buffer_postdisable,
|
|
};
|
|
|
|
int st_lsm6dsm_allocate_rings(struct lsm6dsm_data *cdata)
|
|
{
|
|
int err;
|
|
struct lsm6dsm_sensor_data *sdata;
|
|
|
|
sdata = iio_priv(cdata->indio_dev[ST_MASK_ID_ACCEL]);
|
|
|
|
err = iio_triggered_buffer_setup(cdata->indio_dev[ST_MASK_ID_ACCEL],
|
|
NULL, &st_lsm6dsm_outdata_trigger_handler,
|
|
&st_lsm6dsm_buffer_setup_ops);
|
|
if (err < 0)
|
|
return err;
|
|
|
|
sdata = iio_priv(cdata->indio_dev[ST_MASK_ID_GYRO]);
|
|
|
|
err = iio_triggered_buffer_setup(cdata->indio_dev[ST_MASK_ID_GYRO],
|
|
NULL, &st_lsm6dsm_outdata_trigger_handler,
|
|
&st_lsm6dsm_buffer_setup_ops);
|
|
if (err < 0)
|
|
goto buffer_cleanup_accel;
|
|
|
|
err = iio_triggered_buffer_setup(
|
|
cdata->indio_dev[ST_MASK_ID_SIGN_MOTION],
|
|
&st_lsm6dsm_handler_empty, NULL,
|
|
&st_lsm6dsm_buffer_setup_ops);
|
|
if (err < 0)
|
|
goto buffer_cleanup_gyro;
|
|
|
|
err = iio_triggered_buffer_setup(
|
|
cdata->indio_dev[ST_MASK_ID_STEP_COUNTER],
|
|
NULL,
|
|
&st_lsm6dsm_step_counter_trigger_handler,
|
|
&st_lsm6dsm_buffer_setup_ops);
|
|
if (err < 0)
|
|
goto buffer_cleanup_sign_motion;
|
|
|
|
err = iio_triggered_buffer_setup(
|
|
cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR],
|
|
&st_lsm6dsm_handler_empty, NULL,
|
|
&st_lsm6dsm_buffer_setup_ops);
|
|
if (err < 0)
|
|
goto buffer_cleanup_step_counter;
|
|
|
|
err = iio_triggered_buffer_setup(
|
|
cdata->indio_dev[ST_MASK_ID_TILT],
|
|
&st_lsm6dsm_handler_empty, NULL,
|
|
&st_lsm6dsm_buffer_setup_ops);
|
|
if (err < 0)
|
|
goto buffer_cleanup_step_detector;
|
|
|
|
err = iio_triggered_buffer_setup(
|
|
cdata->indio_dev[ST_MASK_ID_WTILT],
|
|
NULL,
|
|
&st_lsm6dsm_wrist_tilt_trigger_handler,
|
|
&st_lsm6dsm_buffer_setup_ops);
|
|
if (err < 0)
|
|
goto buffer_cleanup_tilt;
|
|
|
|
err = iio_triggered_buffer_setup(
|
|
cdata->indio_dev[ST_MASK_ID_TAP],
|
|
&st_lsm6dsm_handler_empty, NULL,
|
|
&st_lsm6dsm_buffer_setup_ops);
|
|
if (err < 0)
|
|
goto buffer_cleanup_wtilt;
|
|
|
|
err = iio_triggered_buffer_setup(
|
|
cdata->indio_dev[ST_MASK_ID_TAP_TAP],
|
|
&st_lsm6dsm_handler_empty, NULL,
|
|
&st_lsm6dsm_buffer_setup_ops);
|
|
if (err < 0)
|
|
goto buffer_cleanup_tap;
|
|
|
|
return 0;
|
|
|
|
buffer_cleanup_tap:
|
|
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_TAP]);
|
|
buffer_cleanup_wtilt:
|
|
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_WTILT]);
|
|
buffer_cleanup_tilt:
|
|
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_TILT]);
|
|
buffer_cleanup_step_detector:
|
|
iio_triggered_buffer_cleanup(
|
|
cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]);
|
|
buffer_cleanup_step_counter:
|
|
iio_triggered_buffer_cleanup(
|
|
cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]);
|
|
buffer_cleanup_sign_motion:
|
|
iio_triggered_buffer_cleanup(
|
|
cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]);
|
|
buffer_cleanup_gyro:
|
|
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_GYRO]);
|
|
buffer_cleanup_accel:
|
|
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_ACCEL]);
|
|
|
|
return err;
|
|
}
|
|
|
|
void st_lsm6dsm_deallocate_rings(struct lsm6dsm_data *cdata)
|
|
{
|
|
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_TAP_TAP]);
|
|
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_TAP]);
|
|
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_WTILT]);
|
|
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_TILT]);
|
|
iio_triggered_buffer_cleanup(
|
|
cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]);
|
|
iio_triggered_buffer_cleanup(
|
|
cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]);
|
|
iio_triggered_buffer_cleanup(
|
|
cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]);
|
|
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_ACCEL]);
|
|
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_GYRO]);
|
|
}
|