Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * STMicroelectronics st_lsm6dsx FIFO buffer library driver
0004  *
0005  * LSM6DS3/LSM6DS3H/LSM6DSL/LSM6DSM/ISM330DLC/LSM6DS3TR-C:
0006  * The FIFO buffer can be configured to store data from gyroscope and
0007  * accelerometer. Samples are queued without any tag according to a
0008  * specific pattern based on 'FIFO data sets' (6 bytes each):
0009  *  - 1st data set is reserved for gyroscope data
0010  *  - 2nd data set is reserved for accelerometer data
0011  * The FIFO pattern changes depending on the ODRs and decimation factors
0012  * assigned to the FIFO data sets. The first sequence of data stored in FIFO
0013  * buffer contains the data of all the enabled FIFO data sets
0014  * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the
0015  * value of the decimation factor and ODR set for each FIFO data set.
0016  *
0017  * LSM6DSO/LSM6DSOX/ASM330LHH/ASM330LHHX/LSM6DSR/LSM6DSRX/ISM330DHCX/
0018  * LSM6DST/LSM6DSOP:
0019  * The FIFO buffer can be configured to store data from gyroscope and
0020  * accelerometer. Each sample is queued with a tag (1B) indicating data
0021  * source (gyroscope, accelerometer, hw timer).
0022  *
0023  * FIFO supported modes:
0024  *  - BYPASS: FIFO disabled
0025  *  - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index
0026  *    restarts from the beginning and the oldest sample is overwritten
0027  *
0028  * Copyright 2016 STMicroelectronics Inc.
0029  *
0030  * Lorenzo Bianconi <lorenzo.bianconi@st.com>
0031  * Denis Ciocca <denis.ciocca@st.com>
0032  */
0033 #include <linux/module.h>
0034 #include <linux/iio/kfifo_buf.h>
0035 #include <linux/iio/iio.h>
0036 #include <linux/iio/buffer.h>
0037 #include <linux/regmap.h>
0038 #include <linux/bitfield.h>
0039 
0040 #include <linux/platform_data/st_sensors_pdata.h>
0041 
0042 #include "st_lsm6dsx.h"
0043 
0044 #define ST_LSM6DSX_REG_FIFO_MODE_ADDR       0x0a
0045 #define ST_LSM6DSX_FIFO_MODE_MASK       GENMASK(2, 0)
0046 #define ST_LSM6DSX_FIFO_ODR_MASK        GENMASK(6, 3)
0047 #define ST_LSM6DSX_FIFO_EMPTY_MASK      BIT(12)
0048 #define ST_LSM6DSX_REG_FIFO_OUTL_ADDR       0x3e
0049 #define ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR    0x78
0050 #define ST_LSM6DSX_REG_TS_RESET_ADDR        0x42
0051 
0052 #define ST_LSM6DSX_MAX_FIFO_ODR_VAL     0x08
0053 
0054 #define ST_LSM6DSX_TS_RESET_VAL         0xaa
0055 
0056 struct st_lsm6dsx_decimator_entry {
0057     u8 decimator;
0058     u8 val;
0059 };
0060 
0061 enum st_lsm6dsx_fifo_tag {
0062     ST_LSM6DSX_GYRO_TAG = 0x01,
0063     ST_LSM6DSX_ACC_TAG = 0x02,
0064     ST_LSM6DSX_TS_TAG = 0x04,
0065     ST_LSM6DSX_EXT0_TAG = 0x0f,
0066     ST_LSM6DSX_EXT1_TAG = 0x10,
0067     ST_LSM6DSX_EXT2_TAG = 0x11,
0068 };
0069 
0070 static const
0071 struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = {
0072     {  0, 0x0 },
0073     {  1, 0x1 },
0074     {  2, 0x2 },
0075     {  3, 0x3 },
0076     {  4, 0x4 },
0077     {  8, 0x5 },
0078     { 16, 0x6 },
0079     { 32, 0x7 },
0080 };
0081 
0082 static int
0083 st_lsm6dsx_get_decimator_val(struct st_lsm6dsx_sensor *sensor, u32 max_odr)
0084 {
0085     const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
0086     u32 decimator =  max_odr / sensor->odr;
0087     int i;
0088 
0089     if (decimator > 1)
0090         decimator = round_down(decimator, 2);
0091 
0092     for (i = 0; i < max_size; i++) {
0093         if (st_lsm6dsx_decimator_table[i].decimator == decimator)
0094             break;
0095     }
0096 
0097     sensor->decimator = decimator;
0098     return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
0099 }
0100 
0101 static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
0102                        u32 *max_odr, u32 *min_odr)
0103 {
0104     struct st_lsm6dsx_sensor *sensor;
0105     int i;
0106 
0107     *max_odr = 0, *min_odr = ~0;
0108     for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
0109         if (!hw->iio_devs[i])
0110             continue;
0111 
0112         sensor = iio_priv(hw->iio_devs[i]);
0113 
0114         if (!(hw->enable_mask & BIT(sensor->id)))
0115             continue;
0116 
0117         *max_odr = max_t(u32, *max_odr, sensor->odr);
0118         *min_odr = min_t(u32, *min_odr, sensor->odr);
0119     }
0120 }
0121 
0122 static u8 st_lsm6dsx_get_sip(struct st_lsm6dsx_sensor *sensor, u32 min_odr)
0123 {
0124     u8 sip = sensor->odr / min_odr;
0125 
0126     return sip > 1 ? round_down(sip, 2) : sip;
0127 }
0128 
0129 static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
0130 {
0131     const struct st_lsm6dsx_reg *ts_dec_reg;
0132     struct st_lsm6dsx_sensor *sensor;
0133     u16 sip = 0, ts_sip = 0;
0134     u32 max_odr, min_odr;
0135     int err = 0, i;
0136     u8 data;
0137 
0138     st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr);
0139 
0140     for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
0141         const struct st_lsm6dsx_reg *dec_reg;
0142 
0143         if (!hw->iio_devs[i])
0144             continue;
0145 
0146         sensor = iio_priv(hw->iio_devs[i]);
0147         /* update fifo decimators and sample in pattern */
0148         if (hw->enable_mask & BIT(sensor->id)) {
0149             sensor->sip = st_lsm6dsx_get_sip(sensor, min_odr);
0150             data = st_lsm6dsx_get_decimator_val(sensor, max_odr);
0151         } else {
0152             sensor->sip = 0;
0153             data = 0;
0154         }
0155         ts_sip = max_t(u16, ts_sip, sensor->sip);
0156 
0157         dec_reg = &hw->settings->decimator[sensor->id];
0158         if (dec_reg->addr) {
0159             int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask);
0160 
0161             err = st_lsm6dsx_update_bits_locked(hw, dec_reg->addr,
0162                                 dec_reg->mask,
0163                                 val);
0164             if (err < 0)
0165                 return err;
0166         }
0167         sip += sensor->sip;
0168     }
0169     hw->sip = sip + ts_sip;
0170     hw->ts_sip = ts_sip;
0171 
0172     /*
0173      * update hw ts decimator if necessary. Decimator for hw timestamp
0174      * is always 1 or 0 in order to have a ts sample for each data
0175      * sample in FIFO
0176      */
0177     ts_dec_reg = &hw->settings->ts_settings.decimator;
0178     if (ts_dec_reg->addr) {
0179         int val, ts_dec = !!hw->ts_sip;
0180 
0181         val = ST_LSM6DSX_SHIFT_VAL(ts_dec, ts_dec_reg->mask);
0182         err = st_lsm6dsx_update_bits_locked(hw, ts_dec_reg->addr,
0183                             ts_dec_reg->mask, val);
0184     }
0185     return err;
0186 }
0187 
0188 static int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
0189                     enum st_lsm6dsx_fifo_mode fifo_mode)
0190 {
0191     unsigned int data;
0192 
0193     data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode);
0194     return st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
0195                          ST_LSM6DSX_FIFO_MODE_MASK, data);
0196 }
0197 
0198 static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
0199                    bool enable)
0200 {
0201     struct st_lsm6dsx_hw *hw = sensor->hw;
0202     const struct st_lsm6dsx_reg *batch_reg;
0203     u8 data;
0204 
0205     batch_reg = &hw->settings->batch[sensor->id];
0206     if (batch_reg->addr) {
0207         int val;
0208 
0209         if (enable) {
0210             int err;
0211 
0212             err = st_lsm6dsx_check_odr(sensor, sensor->odr,
0213                            &data);
0214             if (err < 0)
0215                 return err;
0216         } else {
0217             data = 0;
0218         }
0219         val = ST_LSM6DSX_SHIFT_VAL(data, batch_reg->mask);
0220         return st_lsm6dsx_update_bits_locked(hw, batch_reg->addr,
0221                              batch_reg->mask, val);
0222     } else {
0223         data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0;
0224         return st_lsm6dsx_update_bits_locked(hw,
0225                     ST_LSM6DSX_REG_FIFO_MODE_ADDR,
0226                     ST_LSM6DSX_FIFO_ODR_MASK,
0227                     FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK,
0228                            data));
0229     }
0230 }
0231 
0232 int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
0233 {
0234     u16 fifo_watermark = ~0, cur_watermark, fifo_th_mask;
0235     struct st_lsm6dsx_hw *hw = sensor->hw;
0236     struct st_lsm6dsx_sensor *cur_sensor;
0237     int i, err, data;
0238     __le16 wdata;
0239 
0240     if (!hw->sip)
0241         return 0;
0242 
0243     for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
0244         if (!hw->iio_devs[i])
0245             continue;
0246 
0247         cur_sensor = iio_priv(hw->iio_devs[i]);
0248 
0249         if (!(hw->enable_mask & BIT(cur_sensor->id)))
0250             continue;
0251 
0252         cur_watermark = (cur_sensor == sensor) ? watermark
0253                                : cur_sensor->watermark;
0254 
0255         fifo_watermark = min_t(u16, fifo_watermark, cur_watermark);
0256     }
0257 
0258     fifo_watermark = max_t(u16, fifo_watermark, hw->sip);
0259     fifo_watermark = (fifo_watermark / hw->sip) * hw->sip;
0260     fifo_watermark = fifo_watermark * hw->settings->fifo_ops.th_wl;
0261 
0262     mutex_lock(&hw->page_lock);
0263     err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1,
0264               &data);
0265     if (err < 0)
0266         goto out;
0267 
0268     fifo_th_mask = hw->settings->fifo_ops.fifo_th.mask;
0269     fifo_watermark = ((data << 8) & ~fifo_th_mask) |
0270              (fifo_watermark & fifo_th_mask);
0271 
0272     wdata = cpu_to_le16(fifo_watermark);
0273     err = regmap_bulk_write(hw->regmap,
0274                 hw->settings->fifo_ops.fifo_th.addr,
0275                 &wdata, sizeof(wdata));
0276 out:
0277     mutex_unlock(&hw->page_lock);
0278     return err;
0279 }
0280 
0281 static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw)
0282 {
0283     struct st_lsm6dsx_sensor *sensor;
0284     int i, err;
0285 
0286     /* reset hw ts counter */
0287     err = st_lsm6dsx_write_locked(hw, ST_LSM6DSX_REG_TS_RESET_ADDR,
0288                       ST_LSM6DSX_TS_RESET_VAL);
0289     if (err < 0)
0290         return err;
0291 
0292     for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
0293         if (!hw->iio_devs[i])
0294             continue;
0295 
0296         sensor = iio_priv(hw->iio_devs[i]);
0297         /*
0298          * store enable buffer timestamp as reference for
0299          * hw timestamp
0300          */
0301         sensor->ts_ref = iio_get_time_ns(hw->iio_devs[i]);
0302     }
0303     return 0;
0304 }
0305 
0306 int st_lsm6dsx_resume_fifo(struct st_lsm6dsx_hw *hw)
0307 {
0308     int err;
0309 
0310     /* reset hw ts counter */
0311     err = st_lsm6dsx_reset_hw_ts(hw);
0312     if (err < 0)
0313         return err;
0314 
0315     return st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
0316 }
0317 
0318 /*
0319  * Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN/ST_LSM6DSX_MAX_TAGGED_WORD_LEN
0320  * in order to avoid a kmalloc for each bus access
0321  */
0322 static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
0323                     u8 *data, unsigned int data_len,
0324                     unsigned int max_word_len)
0325 {
0326     unsigned int word_len, read_len = 0;
0327     int err;
0328 
0329     while (read_len < data_len) {
0330         word_len = min_t(unsigned int, data_len - read_len,
0331                  max_word_len);
0332         err = st_lsm6dsx_read_locked(hw, addr, data + read_len,
0333                          word_len);
0334         if (err < 0)
0335             return err;
0336         read_len += word_len;
0337     }
0338     return 0;
0339 }
0340 
0341 #define ST_LSM6DSX_IIO_BUFF_SIZE    (ALIGN(ST_LSM6DSX_SAMPLE_SIZE, \
0342                            sizeof(s64)) + sizeof(s64))
0343 /**
0344  * st_lsm6dsx_read_fifo() - hw FIFO read routine
0345  * @hw: Pointer to instance of struct st_lsm6dsx_hw.
0346  *
0347  * Read samples from the hw FIFO and push them to IIO buffers.
0348  *
0349  * Return: Number of bytes read from the FIFO
0350  */
0351 int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
0352 {
0353     struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor, *ext_sensor = NULL;
0354     int err, sip, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
0355     u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
0356     u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
0357     bool reset_ts = false;
0358     __le16 fifo_status;
0359     s64 ts = 0;
0360 
0361     err = st_lsm6dsx_read_locked(hw,
0362                      hw->settings->fifo_ops.fifo_diff.addr,
0363                      &fifo_status, sizeof(fifo_status));
0364     if (err < 0) {
0365         dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
0366             err);
0367         return err;
0368     }
0369 
0370     if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
0371         return 0;
0372 
0373     fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
0374            ST_LSM6DSX_CHAN_SIZE;
0375     fifo_len = (fifo_len / pattern_len) * pattern_len;
0376 
0377     acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
0378     gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
0379     if (hw->iio_devs[ST_LSM6DSX_ID_EXT0])
0380         ext_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_EXT0]);
0381 
0382     for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
0383         err = st_lsm6dsx_read_block(hw, ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
0384                         hw->buff, pattern_len,
0385                         ST_LSM6DSX_MAX_WORD_LEN);
0386         if (err < 0) {
0387             dev_err(hw->dev,
0388                 "failed to read pattern from fifo (err=%d)\n",
0389                 err);
0390             return err;
0391         }
0392 
0393         /*
0394          * Data are written to the FIFO with a specific pattern
0395          * depending on the configured ODRs. The first sequence of data
0396          * stored in FIFO contains the data of all enabled sensors
0397          * (e.g. Gx, Gy, Gz, Ax, Ay, Az, Ts), then data are repeated
0398          * depending on the value of the decimation factor set for each
0399          * sensor.
0400          *
0401          * Supposing the FIFO is storing data from gyroscope and
0402          * accelerometer at different ODRs:
0403          *   - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
0404          * Since the gyroscope ODR is twice the accelerometer one, the
0405          * following pattern is repeated every 9 samples:
0406          *   - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, ..
0407          */
0408         ext_sip = ext_sensor ? ext_sensor->sip : 0;
0409         gyro_sip = gyro_sensor->sip;
0410         acc_sip = acc_sensor->sip;
0411         ts_sip = hw->ts_sip;
0412         offset = 0;
0413         sip = 0;
0414 
0415         while (acc_sip > 0 || gyro_sip > 0 || ext_sip > 0) {
0416             if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
0417                 memcpy(hw->scan[ST_LSM6DSX_ID_GYRO].channels,
0418                        &hw->buff[offset],
0419                        sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels));
0420                 offset += sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels);
0421             }
0422             if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
0423                 memcpy(hw->scan[ST_LSM6DSX_ID_ACC].channels,
0424                        &hw->buff[offset],
0425                        sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels));
0426                 offset += sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels);
0427             }
0428             if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
0429                 memcpy(hw->scan[ST_LSM6DSX_ID_EXT0].channels,
0430                        &hw->buff[offset],
0431                        sizeof(hw->scan[ST_LSM6DSX_ID_EXT0].channels));
0432                 offset += sizeof(hw->scan[ST_LSM6DSX_ID_EXT0].channels);
0433             }
0434 
0435             if (ts_sip-- > 0) {
0436                 u8 data[ST_LSM6DSX_SAMPLE_SIZE];
0437 
0438                 memcpy(data, &hw->buff[offset], sizeof(data));
0439                 /*
0440                  * hw timestamp is 3B long and it is stored
0441                  * in FIFO using 6B as 4th FIFO data set
0442                  * according to this schema:
0443                  * B0 = ts[15:8], B1 = ts[23:16], B3 = ts[7:0]
0444                  */
0445                 ts = data[1] << 16 | data[0] << 8 | data[3];
0446                 /*
0447                  * check if hw timestamp engine is going to
0448                  * reset (the sensor generates an interrupt
0449                  * to signal the hw timestamp will reset in
0450                  * 1.638s)
0451                  */
0452                 if (!reset_ts && ts >= 0xff0000)
0453                     reset_ts = true;
0454                 ts *= hw->ts_gain;
0455 
0456                 offset += ST_LSM6DSX_SAMPLE_SIZE;
0457             }
0458 
0459             if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
0460                 iio_push_to_buffers_with_timestamp(
0461                     hw->iio_devs[ST_LSM6DSX_ID_GYRO],
0462                     &hw->scan[ST_LSM6DSX_ID_GYRO],
0463                     gyro_sensor->ts_ref + ts);
0464                 gyro_sip--;
0465             }
0466             if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
0467                 iio_push_to_buffers_with_timestamp(
0468                     hw->iio_devs[ST_LSM6DSX_ID_ACC],
0469                     &hw->scan[ST_LSM6DSX_ID_ACC],
0470                     acc_sensor->ts_ref + ts);
0471                 acc_sip--;
0472             }
0473             if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
0474                 iio_push_to_buffers_with_timestamp(
0475                     hw->iio_devs[ST_LSM6DSX_ID_EXT0],
0476                     &hw->scan[ST_LSM6DSX_ID_EXT0],
0477                     ext_sensor->ts_ref + ts);
0478                 ext_sip--;
0479             }
0480             sip++;
0481         }
0482     }
0483 
0484     if (unlikely(reset_ts)) {
0485         err = st_lsm6dsx_reset_hw_ts(hw);
0486         if (err < 0) {
0487             dev_err(hw->dev, "failed to reset hw ts (err=%d)\n",
0488                 err);
0489             return err;
0490         }
0491     }
0492     return read_len;
0493 }
0494 
0495 #define ST_LSM6DSX_INVALID_SAMPLE   0x7ffd
0496 static int
0497 st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag,
0498                 u8 *data, s64 ts)
0499 {
0500     s16 val = le16_to_cpu(*(__le16 *)data);
0501     struct st_lsm6dsx_sensor *sensor;
0502     struct iio_dev *iio_dev;
0503 
0504     /* invalid sample during bootstrap phase */
0505     if (val >= ST_LSM6DSX_INVALID_SAMPLE)
0506         return -EINVAL;
0507 
0508     /*
0509      * EXT_TAG are managed in FIFO fashion so ST_LSM6DSX_EXT0_TAG
0510      * corresponds to the first enabled channel, ST_LSM6DSX_EXT1_TAG
0511      * to the second one and ST_LSM6DSX_EXT2_TAG to the last enabled
0512      * channel
0513      */
0514     switch (tag) {
0515     case ST_LSM6DSX_GYRO_TAG:
0516         iio_dev = hw->iio_devs[ST_LSM6DSX_ID_GYRO];
0517         break;
0518     case ST_LSM6DSX_ACC_TAG:
0519         iio_dev = hw->iio_devs[ST_LSM6DSX_ID_ACC];
0520         break;
0521     case ST_LSM6DSX_EXT0_TAG:
0522         if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0))
0523             iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT0];
0524         else if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1))
0525             iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
0526         else
0527             iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
0528         break;
0529     case ST_LSM6DSX_EXT1_TAG:
0530         if ((hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) &&
0531             (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1)))
0532             iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
0533         else
0534             iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
0535         break;
0536     case ST_LSM6DSX_EXT2_TAG:
0537         iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
0538         break;
0539     default:
0540         return -EINVAL;
0541     }
0542 
0543     sensor = iio_priv(iio_dev);
0544     iio_push_to_buffers_with_timestamp(iio_dev, data,
0545                        ts + sensor->ts_ref);
0546 
0547     return 0;
0548 }
0549 
0550 /**
0551  * st_lsm6dsx_read_tagged_fifo() - tagged hw FIFO read routine
0552  * @hw: Pointer to instance of struct st_lsm6dsx_hw.
0553  *
0554  * Read samples from the hw FIFO and push them to IIO buffers.
0555  *
0556  * Return: Number of bytes read from the FIFO
0557  */
0558 int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
0559 {
0560     u16 pattern_len = hw->sip * ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
0561     u16 fifo_len, fifo_diff_mask;
0562     /*
0563      * Alignment needed as this can ultimately be passed to a
0564      * call to iio_push_to_buffers_with_timestamp() which
0565      * must be passed a buffer that is aligned to 8 bytes so
0566      * as to allow insertion of a naturally aligned timestamp.
0567      */
0568     u8 iio_buff[ST_LSM6DSX_IIO_BUFF_SIZE] __aligned(8);
0569     u8 tag;
0570     bool reset_ts = false;
0571     int i, err, read_len;
0572     __le16 fifo_status;
0573     s64 ts = 0;
0574 
0575     err = st_lsm6dsx_read_locked(hw,
0576                      hw->settings->fifo_ops.fifo_diff.addr,
0577                      &fifo_status, sizeof(fifo_status));
0578     if (err < 0) {
0579         dev_err(hw->dev, "failed to read fifo status (err=%d)\n",
0580             err);
0581         return err;
0582     }
0583 
0584     fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
0585     fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
0586            ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
0587     if (!fifo_len)
0588         return 0;
0589 
0590     for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
0591         err = st_lsm6dsx_read_block(hw,
0592                         ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR,
0593                         hw->buff, pattern_len,
0594                         ST_LSM6DSX_MAX_TAGGED_WORD_LEN);
0595         if (err < 0) {
0596             dev_err(hw->dev,
0597                 "failed to read pattern from fifo (err=%d)\n",
0598                 err);
0599             return err;
0600         }
0601 
0602         for (i = 0; i < pattern_len;
0603              i += ST_LSM6DSX_TAGGED_SAMPLE_SIZE) {
0604             memcpy(iio_buff, &hw->buff[i + ST_LSM6DSX_TAG_SIZE],
0605                    ST_LSM6DSX_SAMPLE_SIZE);
0606 
0607             tag = hw->buff[i] >> 3;
0608             if (tag == ST_LSM6DSX_TS_TAG) {
0609                 /*
0610                  * hw timestamp is 4B long and it is stored
0611                  * in FIFO according to this schema:
0612                  * B0 = ts[7:0], B1 = ts[15:8], B2 = ts[23:16],
0613                  * B3 = ts[31:24]
0614                  */
0615                 ts = le32_to_cpu(*((__le32 *)iio_buff));
0616                 /*
0617                  * check if hw timestamp engine is going to
0618                  * reset (the sensor generates an interrupt
0619                  * to signal the hw timestamp will reset in
0620                  * 1.638s)
0621                  */
0622                 if (!reset_ts && ts >= 0xffff0000)
0623                     reset_ts = true;
0624                 ts *= hw->ts_gain;
0625             } else {
0626                 st_lsm6dsx_push_tagged_data(hw, tag, iio_buff,
0627                                 ts);
0628             }
0629         }
0630     }
0631 
0632     if (unlikely(reset_ts)) {
0633         err = st_lsm6dsx_reset_hw_ts(hw);
0634         if (err < 0)
0635             return err;
0636     }
0637     return read_len;
0638 }
0639 
0640 int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
0641 {
0642     int err;
0643 
0644     if (!hw->settings->fifo_ops.read_fifo)
0645         return -ENOTSUPP;
0646 
0647     mutex_lock(&hw->fifo_lock);
0648 
0649     hw->settings->fifo_ops.read_fifo(hw);
0650     err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
0651 
0652     mutex_unlock(&hw->fifo_lock);
0653 
0654     return err;
0655 }
0656 
0657 int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
0658 {
0659     struct st_lsm6dsx_hw *hw = sensor->hw;
0660     u8 fifo_mask;
0661     int err;
0662 
0663     mutex_lock(&hw->conf_lock);
0664 
0665     if (enable)
0666         fifo_mask = hw->fifo_mask | BIT(sensor->id);
0667     else
0668         fifo_mask = hw->fifo_mask & ~BIT(sensor->id);
0669 
0670     if (hw->fifo_mask) {
0671         err = st_lsm6dsx_flush_fifo(hw);
0672         if (err < 0)
0673             goto out;
0674     }
0675 
0676     if (sensor->id == ST_LSM6DSX_ID_EXT0 ||
0677         sensor->id == ST_LSM6DSX_ID_EXT1 ||
0678         sensor->id == ST_LSM6DSX_ID_EXT2) {
0679         err = st_lsm6dsx_shub_set_enable(sensor, enable);
0680         if (err < 0)
0681             goto out;
0682     } else {
0683         err = st_lsm6dsx_sensor_set_enable(sensor, enable);
0684         if (err < 0)
0685             goto out;
0686     }
0687 
0688     err = st_lsm6dsx_set_fifo_odr(sensor, enable);
0689     if (err < 0)
0690         goto out;
0691 
0692     err = st_lsm6dsx_update_decimators(hw);
0693     if (err < 0)
0694         goto out;
0695 
0696     err = st_lsm6dsx_update_watermark(sensor, sensor->watermark);
0697     if (err < 0)
0698         goto out;
0699 
0700     if (fifo_mask) {
0701         err = st_lsm6dsx_resume_fifo(hw);
0702         if (err < 0)
0703             goto out;
0704     }
0705 
0706     hw->fifo_mask = fifo_mask;
0707 
0708 out:
0709     mutex_unlock(&hw->conf_lock);
0710 
0711     return err;
0712 }
0713 
0714 static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev)
0715 {
0716     struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
0717     struct st_lsm6dsx_hw *hw = sensor->hw;
0718 
0719     if (!hw->settings->fifo_ops.update_fifo)
0720         return -ENOTSUPP;
0721 
0722     return hw->settings->fifo_ops.update_fifo(sensor, true);
0723 }
0724 
0725 static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev)
0726 {
0727     struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
0728     struct st_lsm6dsx_hw *hw = sensor->hw;
0729 
0730     if (!hw->settings->fifo_ops.update_fifo)
0731         return -ENOTSUPP;
0732 
0733     return hw->settings->fifo_ops.update_fifo(sensor, false);
0734 }
0735 
0736 static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
0737     .preenable = st_lsm6dsx_buffer_preenable,
0738     .postdisable = st_lsm6dsx_buffer_postdisable,
0739 };
0740 
0741 int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
0742 {
0743     int i, ret;
0744 
0745     for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
0746         if (!hw->iio_devs[i])
0747             continue;
0748 
0749         ret = devm_iio_kfifo_buffer_setup(hw->dev, hw->iio_devs[i],
0750                           &st_lsm6dsx_buffer_ops);
0751         if (ret)
0752             return ret;
0753     }
0754 
0755     return 0;
0756 }
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!