/** * 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; }