/**
* cros_ec_accel_legacy_capture() - The trigger handler function
* @irq: The interrupt number.
* @p: Private data - always a pointer to the poll func.
*
* On a trigger event occurring, if the pollfunc is attached then this
* handler is called as a threaded interrupt (and hence may sleep). It
* is responsible for grabbing data from the device and pushing it into
* the associated buffer.
*
* Return: IRQ_HANDLED
*/
static irqreturn_t cros_ec_accel_legacy_capture(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct cros_ec_accel_legacy_state *st = iio_priv(indio_dev);
/* Clear capture data. */
memset(st->capture_data, 0, sizeof(st->capture_data));
/*
* Read data based on which channels are enabled in scan mask. Note
* that on a capture we are always reading the calibrated data.
*/
read_ec_accel_data(st, *indio_dev->active_scan_mask, st->capture_data);
iio_push_to_buffers_with_timestamp(indio_dev, (void *)st->capture_data,
iio_get_time_ns(indio_dev));
/*
* Tell the core we are done with this trigger and ready for the
* next one.
*/
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static irqreturn_t hmc5843_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct hmc5843_data *data = iio_priv(indio_dev);
int ret;
mutex_lock(&data->lock);
ret = hmc5843_wait_measurement(data);
if (ret < 0) {
mutex_unlock(&data->lock);
goto done;
}
ret = regmap_bulk_read(data->regmap, HMC5843_DATA_OUT_MSB_REGS,
data->buffer, 3 * sizeof(__be16));
mutex_unlock(&data->lock);
if (ret < 0)
goto done;
iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,
iio_get_time_ns(indio_dev));
done:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
irqreturn_t adis16400_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct adis16400_state *st = iio_priv(indio_dev);
struct adis *adis = &st->adis;
u32 old_speed_hz = st->adis.spi->max_speed_hz;
int ret;
if (!adis->buffer)
return -ENOMEM;
if (!(st->variant->flags & ADIS16400_NO_BURST) &&
st->adis.spi->max_speed_hz > ADIS16400_SPI_BURST) {
st->adis.spi->max_speed_hz = ADIS16400_SPI_BURST;
spi_setup(st->adis.spi);
}
ret = spi_sync(adis->spi, &adis->msg);
if (ret)
dev_err(&adis->spi->dev, "Failed to read data: %d\n", ret);
if (!(st->variant->flags & ADIS16400_NO_BURST)) {
st->adis.spi->max_speed_hz = old_speed_hz;
spi_setup(st->adis.spi);
}
iio_push_to_buffers_with_timestamp(indio_dev, adis->buffer,
pf->timestamp);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static irqreturn_t mag3110_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct mag3110_data *data = iio_priv(indio_dev);
u8 buffer[16]; /* 3 16-bit channels + 1 byte temp + padding + ts */
int ret;
ret = mag3110_read(data, (__be16 *) buffer);
if (ret < 0)
goto done;
if (test_bit(3, indio_dev->active_scan_mask)) {
ret = i2c_smbus_read_byte_data(data->client,
MAG3110_DIE_TEMP);
if (ret < 0)
goto done;
buffer[6] = ret;
}
iio_push_to_buffers_with_timestamp(indio_dev, buffer,
iio_get_time_ns());
done:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static irqreturn_t mma7455_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct mma7455_data *mma7455 = iio_priv(indio_dev);
u8 buf[16]; /* 3 x 16-bit channels + padding + ts */
int ret;
ret = mma7455_drdy(mma7455);
if (ret)
goto done;
ret = regmap_bulk_read(mma7455->regmap, MMA7455_REG_XOUTL, buf,
sizeof(__le16) * 3);
if (ret)
goto done;
iio_push_to_buffers_with_timestamp(indio_dev, buf,
iio_get_time_ns(indio_dev));
done:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
/**
* iio_simple_dummy_trigger_h() - the trigger handler function
* @irq: the interrupt number
* @p: private data - always a pointer to the poll func.
*
* This is the guts of buffered capture. On a trigger event occurring,
* if the pollfunc is attached then this handler is called as a threaded
* interrupt (and hence may sleep). It is responsible for grabbing data
* from the device and pushing it into the associated buffer.
*/
static irqreturn_t iio_simple_dummy_trigger_h(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
int len = 0;
u16 *data;
data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
if (data == NULL)
goto done;
if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)) {
/*
* Three common options here:
* hardware scans: certain combinations of channels make
* up a fast read. The capture will consist of all of them.
* Hence we just call the grab data function and fill the
* buffer without processing.
* software scans: can be considered to be random access
* so efficient reading is just a case of minimal bus
* transactions.
* software culled hardware scans:
* occasionally a driver may process the nearest hardware
* scan to avoid storing elements that are not desired. This
* is the fiddliest option by far.
* Here let's pretend we have random access. And the values are
* in the constant table fakedata.
*/
int i, j;
for (i = 0, j = 0;
i < bitmap_weight(indio_dev->active_scan_mask,
indio_dev->masklength);
i++, j++) {
j = find_next_bit(indio_dev->active_scan_mask,
indio_dev->masklength, j);
/* random access read from the 'device' */
data[i] = fakedata[j];
len += 2;
}
}
iio_push_to_buffers_with_timestamp(indio_dev, data, iio_get_time_ns());
kfree(data);
done:
/*
* Tell the core we are done with this trigger and ready for the
* next one.
*/
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static void st_hub_step_detector_push_data(struct platform_device *pdev,
u8 *data, int64_t timestamp)
{
struct iio_dev *indio_dev = platform_get_drvdata(pdev);
struct st_hub_sensor_data *sdata = iio_priv(indio_dev);
sdata->buffer[0] = data[0];
sdata->buffer[1] = 0;
iio_push_to_buffers_with_timestamp(indio_dev, sdata->buffer, timestamp);
}
static irqreturn_t ltr501_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct ltr501_data *data = iio_priv(indio_dev);
u16 buf[8];
__le16 als_buf[2];
u8 mask = 0;
int j = 0;
int ret, psdata;
memset(buf, 0, sizeof(buf));
/* figure out which data needs to be ready */
if (test_bit(0, indio_dev->active_scan_mask) ||
test_bit(1, indio_dev->active_scan_mask))
mask |= LTR501_STATUS_ALS_RDY;
if (test_bit(2, indio_dev->active_scan_mask))
mask |= LTR501_STATUS_PS_RDY;
ret = ltr501_drdy(data, mask);
if (ret < 0)
goto done;
if (mask & LTR501_STATUS_ALS_RDY) {
ret = regmap_bulk_read(data->regmap, LTR501_ALS_DATA1,
(u8 *)als_buf, sizeof(als_buf));
if (ret < 0)
return ret;
if (test_bit(0, indio_dev->active_scan_mask))
buf[j++] = le16_to_cpu(als_buf[1]);
if (test_bit(1, indio_dev->active_scan_mask))
buf[j++] = le16_to_cpu(als_buf[0]);
}
if (mask & LTR501_STATUS_PS_RDY) {
ret = regmap_bulk_read(data->regmap, LTR501_PS_DATA,
&psdata, 2);
if (ret < 0)
goto done;
buf[j++] = psdata & LTR501_PS_DATA_MASK;
}
iio_push_to_buffers_with_timestamp(indio_dev, buf,
iio_get_time_ns(indio_dev));
done:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
/* Whilst this makes a lot of calls to iio_sw_ring functions - it is too device
* specific to be rolled into the core.
*/
static irqreturn_t ade7758_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct ade7758_state *st = iio_priv(indio_dev);
s64 dat64[2];
u32 *dat32 = (u32 *)dat64;
if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
if (ade7758_spi_read_burst(indio_dev) >= 0)
*dat32 = get_unaligned_be32(&st->rx_buf[5]) & 0xFFFFFF;
iio_push_to_buffers_with_timestamp(indio_dev, dat64, pf->timestamp);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
/**
* ad7298_trigger_handler() bh of trigger launched polling to ring buffer
*
* Currently there is no option in this driver to disable the saving of
* timestamps within the ring.
**/
static irqreturn_t ad7298_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct ad7298_state *st = iio_priv(indio_dev);
int b_sent;
b_sent = spi_sync(st->spi, &st->ring_msg);
if (b_sent)
goto done;
iio_push_to_buffers_with_timestamp(indio_dev, st->rx_buf,
iio_get_time_ns(indio_dev));
done:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
irqreturn_t st_sensors_trigger_handler(int irq, void *p)
{
int len;
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct st_sensor_data *sdata = iio_priv(indio_dev);
len = st_sensors_get_buffer_element(indio_dev, sdata->buffer_data);
if (len < 0)
goto st_sensors_get_buffer_element_error;
iio_push_to_buffers_with_timestamp(indio_dev, sdata->buffer_data,
pf->timestamp);
st_sensors_get_buffer_element_error:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static irqreturn_t mma8452_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct mma8452_data *data = iio_priv(indio_dev);
u8 buffer[16]; /* 3 16-bit channels + padding + ts */
int ret;
ret = mma8452_read(data, (__be16 *) buffer);
if (ret < 0)
goto done;
iio_push_to_buffers_with_timestamp(indio_dev, buffer,
iio_get_time_ns());
done:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static irqreturn_t mpl3115_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct mpl3115_data *data = iio_priv(indio_dev);
u8 buffer[16]; /* 32-bit channel + 16-bit channel + padding + ts */
int ret, pos = 0;
mutex_lock(&data->lock);
ret = mpl3115_request(data);
if (ret < 0) {
mutex_unlock(&data->lock);
goto done;
}
memset(buffer, 0, sizeof(buffer));
if (test_bit(0, indio_dev->active_scan_mask)) {
ret = i2c_smbus_read_i2c_block_data(data->client,
MPL3115_OUT_PRESS, 3, &buffer[pos]);
if (ret < 0) {
mutex_unlock(&data->lock);
goto done;
}
pos += 4;
}
if (test_bit(1, indio_dev->active_scan_mask)) {
ret = i2c_smbus_read_i2c_block_data(data->client,
MPL3115_OUT_TEMP, 2, &buffer[pos]);
if (ret < 0) {
mutex_unlock(&data->lock);
goto done;
}
}
mutex_unlock(&data->lock);
iio_push_to_buffers_with_timestamp(indio_dev, buffer,
iio_get_time_ns());
done:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
/**
* ad7606_poll_bh_to_ring() bh of trigger launched polling to ring buffer
* @work_s: the work struct through which this was scheduled
*
* Currently there is no option in this driver to disable the saving of
* timestamps within the ring.
* I think the one copy of this at a time was to avoid problems if the
* trigger was set far too high and the reads then locked up the computer.
**/
static void ad7606_poll_bh_to_ring(struct work_struct *work_s)
{
struct ad7606_state *st = container_of(work_s, struct ad7606_state,
poll_work);
struct iio_dev *indio_dev = iio_priv_to_dev(st);
__u8 *buf;
int ret;
buf = kzalloc(indio_dev->scan_bytes, GFP_KERNEL);
if (buf == NULL)
return;
if (gpio_is_valid(st->pdata->gpio_frstdata)) {
ret = st->bops->read_block(st->dev, 1, buf);
if (ret)
goto done;
if (!gpio_get_value(st->pdata->gpio_frstdata)) {
/* This should never happen. However
* some signal glitch caused by bad PCB desgin or
* electrostatic discharge, could cause an extra read
* or clock. This allows recovery.
*/
ad7606_reset(st);
goto done;
}
ret = st->bops->read_block(st->dev,
st->chip_info->num_channels - 1, buf + 2);
if (ret)
goto done;
} else {
ret = st->bops->read_block(st->dev,
st->chip_info->num_channels, buf);
if (ret)
goto done;
}
iio_push_to_buffers_with_timestamp(indio_dev, buf, iio_get_time_ns());
done:
gpio_set_value(st->pdata->gpio_convst, 0);
iio_trigger_notify_done(indio_dev->trig);
kfree(buf);
}
static irqreturn_t vf610_adc_isr(int irq, void *dev_id)
{
struct iio_dev *indio_dev = (struct iio_dev *)dev_id;
struct vf610_adc *info = iio_priv(indio_dev);
int coco;
coco = readl(info->regs + VF610_REG_ADC_HS);
if (coco & VF610_ADC_HS_COCO0) {
info->value = vf610_adc_read_data(info);
if (iio_buffer_enabled(indio_dev)) {
info->buffer[0] = info->value;
iio_push_to_buffers_with_timestamp(indio_dev,
info->buffer, iio_get_time_ns());
iio_trigger_notify_done(indio_dev->trig);
} else
complete(&info->completion);
}
return IRQ_HANDLED;
}
static irqreturn_t adc108s102_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct adc108s102_state *st = iio_priv(indio_dev);
int ret;
ret = spi_sync(st->spi, &st->ring_msg);
if (ret < 0)
goto out_notify;
/* Skip the dummy response in the first slot */
iio_push_to_buffers_with_timestamp(indio_dev,
(u8 *)&st->rx_buf[1],
iio_get_time_ns(indio_dev));
out_notify:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static irqreturn_t kxcjk1013_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct kxcjk1013_data *data = iio_priv(indio_dev);
int ret;
mutex_lock(&data->mutex);
ret = i2c_smbus_read_i2c_block_data_or_emulated(data->client,
KXCJK1013_REG_XOUT_L,
AXIS_MAX * 2,
(u8 *)data->buffer);
mutex_unlock(&data->mutex);
if (ret < 0)
goto err;
iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,
data->timestamp);
err:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static void st_hub_euler_push_data(struct platform_device *pdev,
u8 *data, int64_t timestamp)
{
int i;
u8 *sensor_data = data;
size_t byte_for_channel;
unsigned int init_copy = 0;
struct iio_dev *indio_dev = platform_get_drvdata(pdev);
struct st_hub_sensor_data *edata = iio_priv(indio_dev);
for (i = 0; i < ST_HUB_EULER_NUM_DATA_CH; i++) {
byte_for_channel =
indio_dev->channels[i].scan_type.storagebits >> 3;
if (test_bit(i, indio_dev->active_scan_mask)) {
memcpy(&edata->buffer[init_copy],
sensor_data, byte_for_channel);
init_copy += byte_for_channel;
}
sensor_data += byte_for_channel;
}
iio_push_to_buffers_with_timestamp(indio_dev, edata->buffer, timestamp);
}
static irqreturn_t bma220_trigger_handler(int irq, void *p)
{
int ret;
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct bma220_data *data = iio_priv(indio_dev);
struct spi_device *spi = data->spi_device;
mutex_lock(&data->lock);
data->tx_buf[0] = BMA220_REG_ACCEL_X | BMA220_READ_MASK;
ret = spi_write_then_read(spi, data->tx_buf, 1, data->buffer,
ARRAY_SIZE(bma220_channels) - 1);
if (ret < 0)
goto err;
iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,
pf->timestamp);
err:
mutex_unlock(&data->lock);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
/**
* st_lsm6dsx_read_fifo() - LSM6DS3-LSM6DS3H-LSM6DSL-LSM6DSM read FIFO routine
* @hw: Pointer to instance of struct st_lsm6dsx_hw.
*
* Read samples from the hw FIFO and push them to IIO buffers.
*
* Return: Number of bytes read from the FIFO
*/
static int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
{
u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
int err, acc_sip, gyro_sip, ts_sip, read_len, offset;
struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
u8 gyro_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
u8 acc_buff[ST_LSM6DSX_IIO_BUFF_SIZE];
bool reset_ts = false;
__le16 fifo_status;
s64 ts = 0;
err = regmap_bulk_read(hw->regmap,
hw->settings->fifo_ops.fifo_diff.addr,
&fifo_status, sizeof(fifo_status));
if (err < 0)
return err;
if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
return 0;
fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
ST_LSM6DSX_CHAN_SIZE;
fifo_len = (fifo_len / pattern_len) * pattern_len;
acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
err = st_lsm6dsx_read_block(hw, hw->buff, pattern_len);
if (err < 0)
return err;
/*
* Data are written to the FIFO with a specific pattern
* depending on the configured ODRs. The first sequence of data
* stored in FIFO contains the data of all enabled sensors
* (e.g. Gx, Gy, Gz, Ax, Ay, Az, Ts), then data are repeated
* depending on the value of the decimation factor set for each
* sensor.
*
* Supposing the FIFO is storing data from gyroscope and
* accelerometer at different ODRs:
* - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
* Since the gyroscope ODR is twice the accelerometer one, the
* following pattern is repeated every 9 samples:
* - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, ..
*/
gyro_sip = gyro_sensor->sip;
acc_sip = acc_sensor->sip;
ts_sip = hw->ts_sip;
offset = 0;
while (acc_sip > 0 || gyro_sip > 0) {
if (gyro_sip > 0) {
memcpy(gyro_buff, &hw->buff[offset],
ST_LSM6DSX_SAMPLE_SIZE);
offset += ST_LSM6DSX_SAMPLE_SIZE;
}
if (acc_sip > 0) {
memcpy(acc_buff, &hw->buff[offset],
ST_LSM6DSX_SAMPLE_SIZE);
offset += ST_LSM6DSX_SAMPLE_SIZE;
}
if (ts_sip-- > 0) {
u8 data[ST_LSM6DSX_SAMPLE_SIZE];
memcpy(data, &hw->buff[offset], sizeof(data));
/*
* hw timestamp is 3B long and it is stored
* in FIFO using 6B as 4th FIFO data set
* according to this schema:
* B0 = ts[15:8], B1 = ts[23:16], B3 = ts[7:0]
*/
ts = data[1] << 16 | data[0] << 8 | data[3];
/*
* check if hw timestamp engine is going to
* reset (the sensor generates an interrupt
* to signal the hw timestamp will reset in
* 1.638s)
*/
if (!reset_ts && ts >= 0xff0000)
reset_ts = true;
ts *= ST_LSM6DSX_TS_SENSITIVITY;
offset += ST_LSM6DSX_SAMPLE_SIZE;
}
if (gyro_sip-- > 0)
iio_push_to_buffers_with_timestamp(
hw->iio_devs[ST_LSM6DSX_ID_GYRO],
gyro_buff, gyro_sensor->ts_ref + ts);
if (acc_sip-- > 0)
iio_push_to_buffers_with_timestamp(
hw->iio_devs[ST_LSM6DSX_ID_ACC],
acc_buff, acc_sensor->ts_ref + ts);
}
}
if (unlikely(reset_ts)) {
err = st_lsm6dsx_reset_hw_ts(hw);
if (err < 0)
return err;
}
return read_len;
}
static irqreturn_t mpu3050_trigger_handler(int irq, void *p)
{
const struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct mpu3050 *mpu3050 = iio_priv(indio_dev);
int ret;
/*
* Temperature 1*16 bits
* Three axes 3*16 bits
* Timestamp 64 bits (4*16 bits)
* Sum total 8*16 bits
*/
__be16 hw_values[8];
s64 timestamp;
unsigned int datums_from_fifo = 0;
/*
* If we're using the hardware trigger, get the precise timestamp from
* the top half of the threaded IRQ handler. Otherwise get the
* timestamp here so it will be close in time to the actual values
* read from the registers.
*/
if (iio_trigger_using_own(indio_dev))
timestamp = mpu3050->hw_timestamp;
else
timestamp = iio_get_time_ns(indio_dev);
mutex_lock(&mpu3050->lock);
/* Using the hardware IRQ trigger? Check the buffer then. */
if (mpu3050->hw_irq_trigger) {
__be16 raw_fifocnt;
u16 fifocnt;
/* X, Y, Z + temperature */
unsigned int bytes_per_datum = 8;
bool fifo_overflow = false;
ret = regmap_bulk_read(mpu3050->map,
MPU3050_FIFO_COUNT_H,
&raw_fifocnt,
sizeof(raw_fifocnt));
if (ret)
goto out_trigger_unlock;
fifocnt = be16_to_cpu(raw_fifocnt);
if (fifocnt == 512) {
dev_info(mpu3050->dev,
"FIFO overflow! Emptying and resetting FIFO\n");
fifo_overflow = true;
/* Reset and enable the FIFO */
ret = regmap_update_bits(mpu3050->map,
MPU3050_USR_CTRL,
MPU3050_USR_CTRL_FIFO_EN |
MPU3050_USR_CTRL_FIFO_RST,
MPU3050_USR_CTRL_FIFO_EN |
MPU3050_USR_CTRL_FIFO_RST);
if (ret) {
dev_info(mpu3050->dev, "error resetting FIFO\n");
goto out_trigger_unlock;
}
mpu3050->pending_fifo_footer = false;
}
if (fifocnt)
dev_dbg(mpu3050->dev,
"%d bytes in the FIFO\n",
fifocnt);
while (!fifo_overflow && fifocnt > bytes_per_datum) {
unsigned int toread;
unsigned int offset;
__be16 fifo_values[5];
/*
* If there is a FIFO footer in the pipe, first clear
* that out. This follows the complex algorithm in the
* datasheet that states that you may never leave the
* FIFO empty after the first reading: you have to
* always leave two footer bytes in it. The footer is
* in practice just two zero bytes.
*/
if (mpu3050->pending_fifo_footer) {
toread = bytes_per_datum + 2;
offset = 0;
} else {
toread = bytes_per_datum;
offset = 1;
/* Put in some dummy value */
fifo_values[0] = 0xAAAA;
}
ret = regmap_bulk_read(mpu3050->map,
MPU3050_FIFO_R,
&fifo_values[offset],
toread);
dev_dbg(mpu3050->dev,
"%04x %04x %04x %04x %04x\n",
fifo_values[0],
fifo_values[1],
fifo_values[2],
fifo_values[3],
fifo_values[4]);
/* Index past the footer (fifo_values[0]) and push */
iio_push_to_buffers_with_timestamp(indio_dev,
&fifo_values[1],
timestamp);
fifocnt -= toread;
datums_from_fifo++;
mpu3050->pending_fifo_footer = true;
/*
* If we're emptying the FIFO, just make sure to
* check if something new appeared.
*/
if (fifocnt < bytes_per_datum) {
ret = regmap_bulk_read(mpu3050->map,
MPU3050_FIFO_COUNT_H,
&raw_fifocnt,
sizeof(raw_fifocnt));
if (ret)
goto out_trigger_unlock;
fifocnt = be16_to_cpu(raw_fifocnt);
}
if (fifocnt < bytes_per_datum)
dev_dbg(mpu3050->dev,
"%d bytes left in the FIFO\n",
fifocnt);
/*
* At this point, the timestamp that triggered the
* hardware interrupt is no longer valid for what
* we are reading (the interrupt likely fired for
* the value on the top of the FIFO), so set the
* timestamp to zero and let userspace deal with it.
*/
timestamp = 0;
}
}
/*
* If we picked some datums from the FIFO that's enough, else
* fall through and just read from the current value registers.
* This happens in two cases:
*
* - We are using some other trigger (external, like an HRTimer)
* than the sensor's own sample generator. In this case the
* sensor is just set to the max sampling frequency and we give
* the trigger a copy of the latest value every time we get here.
*
* - The hardware trigger is active but unused and we actually use
* another trigger which calls here with a frequency higher
* than what the device provides data. We will then just read
* duplicate values directly from the hardware registers.
*/
if (datums_from_fifo) {
dev_dbg(mpu3050->dev,
"read %d datums from the FIFO\n",
datums_from_fifo);
goto out_trigger_unlock;
}
ret = regmap_bulk_read(mpu3050->map, MPU3050_TEMP_H, &hw_values,
sizeof(hw_values));
if (ret) {
dev_err(mpu3050->dev,
"error reading axis data\n");
goto out_trigger_unlock;
}
iio_push_to_buffers_with_timestamp(indio_dev, hw_values, timestamp);
out_trigger_unlock:
mutex_unlock(&mpu3050->lock);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
/**
* st_lsm6dsx_read_fifo() - LSM6DS3-LSM6DSM read FIFO routine
* @hw: Pointer to instance of struct st_lsm6dsx_hw.
*
* Read samples from the hw FIFO and push them to IIO buffers.
*
* Return: Number of bytes read from the FIFO
*/
static int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
{
u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
int err, acc_sip, gyro_sip, read_len, samples, offset;
struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
s64 acc_ts, acc_delta_ts, gyro_ts, gyro_delta_ts;
u8 iio_buff[ALIGN(ST_LSM6DSX_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)];
u8 buff[pattern_len];
__le16 fifo_status;
err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_DIFFL_ADDR,
sizeof(fifo_status), (u8 *)&fifo_status);
if (err < 0)
return err;
if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
return 0;
fifo_len = (le16_to_cpu(fifo_status) & ST_LSM6DSX_FIFO_DIFF_MASK) *
ST_LSM6DSX_CHAN_SIZE;
samples = fifo_len / ST_LSM6DSX_SAMPLE_SIZE;
fifo_len = (fifo_len / pattern_len) * pattern_len;
/*
* compute delta timestamp between two consecutive samples
* in order to estimate queueing time of data generated
* by the sensor
*/
acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
acc_ts = acc_sensor->ts - acc_sensor->delta_ts;
acc_delta_ts = div_s64(acc_sensor->delta_ts * acc_sensor->decimator,
samples);
gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
gyro_ts = gyro_sensor->ts - gyro_sensor->delta_ts;
gyro_delta_ts = div_s64(gyro_sensor->delta_ts * gyro_sensor->decimator,
samples);
for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
sizeof(buff), buff);
if (err < 0)
return err;
/*
* Data are written to the FIFO with a specific pattern
* depending on the configured ODRs. The first sequence of data
* stored in FIFO contains the data of all enabled sensors
* (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated
* depending on the value of the decimation factor set for each
* sensor.
*
* Supposing the FIFO is storing data from gyroscope and
* accelerometer at different ODRs:
* - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
* Since the gyroscope ODR is twice the accelerometer one, the
* following pattern is repeated every 9 samples:
* - Gx, Gy, Gz, Ax, Ay, Az, Gx, Gy, Gz
*/
gyro_sip = gyro_sensor->sip;
acc_sip = acc_sensor->sip;
offset = 0;
while (acc_sip > 0 || gyro_sip > 0) {
if (gyro_sip-- > 0) {
memcpy(iio_buff, &buff[offset],
ST_LSM6DSX_SAMPLE_SIZE);
iio_push_to_buffers_with_timestamp(
hw->iio_devs[ST_LSM6DSX_ID_GYRO],
iio_buff, gyro_ts);
offset += ST_LSM6DSX_SAMPLE_SIZE;
gyro_ts += gyro_delta_ts;
}
if (acc_sip-- > 0) {
memcpy(iio_buff, &buff[offset],
ST_LSM6DSX_SAMPLE_SIZE);
iio_push_to_buffers_with_timestamp(
hw->iio_devs[ST_LSM6DSX_ID_ACC],
iio_buff, acc_ts);
offset += ST_LSM6DSX_SAMPLE_SIZE;
acc_ts += acc_delta_ts;
}
}
}
return read_len;
}
/**
* inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
*/
irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct inv_mpu6050_state *st = iio_priv(indio_dev);
size_t bytes_per_datum;
int result;
u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
u16 fifo_count;
s64 timestamp;
mutex_lock(&indio_dev->mlock);
if (!(st->chip_config.accl_fifo_enable |
st->chip_config.gyro_fifo_enable))
goto end_session;
bytes_per_datum = 0;
if (st->chip_config.accl_fifo_enable)
bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
if (st->chip_config.gyro_fifo_enable)
bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
/*
* read fifo_count register to know how many bytes inside FIFO
* right now
*/
result = i2c_smbus_read_i2c_block_data(st->client,
st->reg->fifo_count_h,
INV_MPU6050_FIFO_COUNT_BYTE, data);
if (result != INV_MPU6050_FIFO_COUNT_BYTE)
goto end_session;
fifo_count = be16_to_cpup((__be16 *)(&data[0]));
if (fifo_count < bytes_per_datum)
goto end_session;
/* fifo count can't be odd number, if it is odd, reset fifo*/
if (fifo_count & 1)
goto flush_fifo;
if (fifo_count > INV_MPU6050_FIFO_THRESHOLD)
goto flush_fifo;
/* Timestamp mismatch. */
if (kfifo_len(&st->timestamps) >
fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR)
goto flush_fifo;
while (fifo_count >= bytes_per_datum) {
result = i2c_smbus_read_i2c_block_data(st->client,
st->reg->fifo_r_w,
bytes_per_datum, data);
if (result != bytes_per_datum)
goto flush_fifo;
result = kfifo_out(&st->timestamps, ×tamp, 1);
/* when there is no timestamp, put timestamp as 0 */
if (0 == result)
timestamp = 0;
result = iio_push_to_buffers_with_timestamp(indio_dev, data,
timestamp);
if (result)
goto flush_fifo;
fifo_count -= bytes_per_datum;
}
end_session:
mutex_unlock(&indio_dev->mlock);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
flush_fifo:
/* Flush HW and SW FIFOs. */
inv_reset_fifo(indio_dev);
inv_clear_kfifo(st);
mutex_unlock(&indio_dev->mlock);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
