linux/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
<<
>>
Prefs
   1// SPDX-License-Identifier: GPL-2.0-or-later
   2/*
   3 * Copyright (C) 2020 Invensense, Inc.
   4 */
   5
   6#include <linux/kernel.h>
   7#include <linux/device.h>
   8#include <linux/mutex.h>
   9#include <linux/pm_runtime.h>
  10#include <linux/regmap.h>
  11#include <linux/delay.h>
  12#include <linux/math64.h>
  13#include <linux/iio/iio.h>
  14#include <linux/iio/buffer.h>
  15#include <linux/iio/kfifo_buf.h>
  16
  17#include "inv_icm42600.h"
  18#include "inv_icm42600_temp.h"
  19#include "inv_icm42600_buffer.h"
  20#include "inv_icm42600_timestamp.h"
  21
  22#define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info)           \
  23        {                                                               \
  24                .type = IIO_ACCEL,                                      \
  25                .modified = 1,                                          \
  26                .channel2 = _modifier,                                  \
  27                .info_mask_separate =                                   \
  28                        BIT(IIO_CHAN_INFO_RAW) |                        \
  29                        BIT(IIO_CHAN_INFO_CALIBBIAS),                   \
  30                .info_mask_shared_by_type =                             \
  31                        BIT(IIO_CHAN_INFO_SCALE),                       \
  32                .info_mask_shared_by_type_available =                   \
  33                        BIT(IIO_CHAN_INFO_SCALE) |                      \
  34                        BIT(IIO_CHAN_INFO_CALIBBIAS),                   \
  35                .info_mask_shared_by_all =                              \
  36                        BIT(IIO_CHAN_INFO_SAMP_FREQ),                   \
  37                .info_mask_shared_by_all_available =                    \
  38                        BIT(IIO_CHAN_INFO_SAMP_FREQ),                   \
  39                .scan_index = _index,                                   \
  40                .scan_type = {                                          \
  41                        .sign = 's',                                    \
  42                        .realbits = 16,                                 \
  43                        .storagebits = 16,                              \
  44                        .endianness = IIO_BE,                           \
  45                },                                                      \
  46                .ext_info = _ext_info,                                  \
  47        }
  48
  49enum inv_icm42600_accel_scan {
  50        INV_ICM42600_ACCEL_SCAN_X,
  51        INV_ICM42600_ACCEL_SCAN_Y,
  52        INV_ICM42600_ACCEL_SCAN_Z,
  53        INV_ICM42600_ACCEL_SCAN_TEMP,
  54        INV_ICM42600_ACCEL_SCAN_TIMESTAMP,
  55};
  56
  57static const struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = {
  58        IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm42600_get_mount_matrix),
  59        {},
  60};
  61
  62static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
  63        INV_ICM42600_ACCEL_CHAN(IIO_MOD_X, INV_ICM42600_ACCEL_SCAN_X,
  64                                inv_icm42600_accel_ext_infos),
  65        INV_ICM42600_ACCEL_CHAN(IIO_MOD_Y, INV_ICM42600_ACCEL_SCAN_Y,
  66                                inv_icm42600_accel_ext_infos),
  67        INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z,
  68                                inv_icm42600_accel_ext_infos),
  69        INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
  70        IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_ACCEL_SCAN_TIMESTAMP),
  71};
  72
  73/*
  74 * IIO buffer data: size must be a power of 2 and timestamp aligned
  75 * 16 bytes: 6 bytes acceleration, 2 bytes temperature, 8 bytes timestamp
  76 */
  77struct inv_icm42600_accel_buffer {
  78        struct inv_icm42600_fifo_sensor_data accel;
  79        int16_t temp;
  80        int64_t timestamp __aligned(8);
  81};
  82
  83#define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS                              \
  84        (BIT(INV_ICM42600_ACCEL_SCAN_X) |                               \
  85        BIT(INV_ICM42600_ACCEL_SCAN_Y) |                                \
  86        BIT(INV_ICM42600_ACCEL_SCAN_Z))
  87
  88#define INV_ICM42600_SCAN_MASK_TEMP     BIT(INV_ICM42600_ACCEL_SCAN_TEMP)
  89
  90static const unsigned long inv_icm42600_accel_scan_masks[] = {
  91        /* 3-axis accel + temperature */
  92        INV_ICM42600_SCAN_MASK_ACCEL_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
  93        0,
  94};
  95
  96/* enable accelerometer sensor and FIFO write */
  97static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
  98                                               const unsigned long *scan_mask)
  99{
 100        struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
 101        struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
 102        struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
 103        unsigned int fifo_en = 0;
 104        unsigned int sleep_temp = 0;
 105        unsigned int sleep_accel = 0;
 106        unsigned int sleep;
 107        int ret;
 108
 109        mutex_lock(&st->lock);
 110
 111        if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
 112                /* enable temp sensor */
 113                ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
 114                if (ret)
 115                        goto out_unlock;
 116                fifo_en |= INV_ICM42600_SENSOR_TEMP;
 117        }
 118
 119        if (*scan_mask & INV_ICM42600_SCAN_MASK_ACCEL_3AXIS) {
 120                /* enable accel sensor */
 121                conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
 122                ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_accel);
 123                if (ret)
 124                        goto out_unlock;
 125                fifo_en |= INV_ICM42600_SENSOR_ACCEL;
 126        }
 127
 128        /* update data FIFO write */
 129        inv_icm42600_timestamp_apply_odr(ts, 0, 0, 0);
 130        ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
 131        if (ret)
 132                goto out_unlock;
 133
 134        ret = inv_icm42600_buffer_update_watermark(st);
 135
 136out_unlock:
 137        mutex_unlock(&st->lock);
 138        /* sleep maximum required time */
 139        if (sleep_accel > sleep_temp)
 140                sleep = sleep_accel;
 141        else
 142                sleep = sleep_temp;
 143        if (sleep)
 144                msleep(sleep);
 145        return ret;
 146}
 147
 148static int inv_icm42600_accel_read_sensor(struct inv_icm42600_state *st,
 149                                          struct iio_chan_spec const *chan,
 150                                          int16_t *val)
 151{
 152        struct device *dev = regmap_get_device(st->map);
 153        struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
 154        unsigned int reg;
 155        __be16 *data;
 156        int ret;
 157
 158        if (chan->type != IIO_ACCEL)
 159                return -EINVAL;
 160
 161        switch (chan->channel2) {
 162        case IIO_MOD_X:
 163                reg = INV_ICM42600_REG_ACCEL_DATA_X;
 164                break;
 165        case IIO_MOD_Y:
 166                reg = INV_ICM42600_REG_ACCEL_DATA_Y;
 167                break;
 168        case IIO_MOD_Z:
 169                reg = INV_ICM42600_REG_ACCEL_DATA_Z;
 170                break;
 171        default:
 172                return -EINVAL;
 173        }
 174
 175        pm_runtime_get_sync(dev);
 176        mutex_lock(&st->lock);
 177
 178        /* enable accel sensor */
 179        conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
 180        ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
 181        if (ret)
 182                goto exit;
 183
 184        /* read accel register data */
 185        data = (__be16 *)&st->buffer[0];
 186        ret = regmap_bulk_read(st->map, reg, data, sizeof(*data));
 187        if (ret)
 188                goto exit;
 189
 190        *val = (int16_t)be16_to_cpup(data);
 191        if (*val == INV_ICM42600_DATA_INVALID)
 192                ret = -EINVAL;
 193exit:
 194        mutex_unlock(&st->lock);
 195        pm_runtime_mark_last_busy(dev);
 196        pm_runtime_put_autosuspend(dev);
 197        return ret;
 198}
 199
 200/* IIO format int + nano */
 201static const int inv_icm42600_accel_scale[] = {
 202        /* +/- 16G => 0.004788403 m/s-2 */
 203        [2 * INV_ICM42600_ACCEL_FS_16G] = 0,
 204        [2 * INV_ICM42600_ACCEL_FS_16G + 1] = 4788403,
 205        /* +/- 8G => 0.002394202 m/s-2 */
 206        [2 * INV_ICM42600_ACCEL_FS_8G] = 0,
 207        [2 * INV_ICM42600_ACCEL_FS_8G + 1] = 2394202,
 208        /* +/- 4G => 0.001197101 m/s-2 */
 209        [2 * INV_ICM42600_ACCEL_FS_4G] = 0,
 210        [2 * INV_ICM42600_ACCEL_FS_4G + 1] = 1197101,
 211        /* +/- 2G => 0.000598550 m/s-2 */
 212        [2 * INV_ICM42600_ACCEL_FS_2G] = 0,
 213        [2 * INV_ICM42600_ACCEL_FS_2G + 1] = 598550,
 214};
 215
 216static int inv_icm42600_accel_read_scale(struct inv_icm42600_state *st,
 217                                         int *val, int *val2)
 218{
 219        unsigned int idx;
 220
 221        idx = st->conf.accel.fs;
 222
 223        *val = inv_icm42600_accel_scale[2 * idx];
 224        *val2 = inv_icm42600_accel_scale[2 * idx + 1];
 225        return IIO_VAL_INT_PLUS_NANO;
 226}
 227
 228static int inv_icm42600_accel_write_scale(struct inv_icm42600_state *st,
 229                                          int val, int val2)
 230{
 231        struct device *dev = regmap_get_device(st->map);
 232        unsigned int idx;
 233        struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
 234        int ret;
 235
 236        for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_scale); idx += 2) {
 237                if (val == inv_icm42600_accel_scale[idx] &&
 238                    val2 == inv_icm42600_accel_scale[idx + 1])
 239                        break;
 240        }
 241        if (idx >= ARRAY_SIZE(inv_icm42600_accel_scale))
 242                return -EINVAL;
 243
 244        conf.fs = idx / 2;
 245
 246        pm_runtime_get_sync(dev);
 247        mutex_lock(&st->lock);
 248
 249        ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
 250
 251        mutex_unlock(&st->lock);
 252        pm_runtime_mark_last_busy(dev);
 253        pm_runtime_put_autosuspend(dev);
 254
 255        return ret;
 256}
 257
 258/* IIO format int + micro */
 259static const int inv_icm42600_accel_odr[] = {
 260        /* 12.5Hz */
 261        12, 500000,
 262        /* 25Hz */
 263        25, 0,
 264        /* 50Hz */
 265        50, 0,
 266        /* 100Hz */
 267        100, 0,
 268        /* 200Hz */
 269        200, 0,
 270        /* 1kHz */
 271        1000, 0,
 272        /* 2kHz */
 273        2000, 0,
 274        /* 4kHz */
 275        4000, 0,
 276};
 277
 278static const int inv_icm42600_accel_odr_conv[] = {
 279        INV_ICM42600_ODR_12_5HZ,
 280        INV_ICM42600_ODR_25HZ,
 281        INV_ICM42600_ODR_50HZ,
 282        INV_ICM42600_ODR_100HZ,
 283        INV_ICM42600_ODR_200HZ,
 284        INV_ICM42600_ODR_1KHZ_LN,
 285        INV_ICM42600_ODR_2KHZ_LN,
 286        INV_ICM42600_ODR_4KHZ_LN,
 287};
 288
 289static int inv_icm42600_accel_read_odr(struct inv_icm42600_state *st,
 290                                       int *val, int *val2)
 291{
 292        unsigned int odr;
 293        unsigned int i;
 294
 295        odr = st->conf.accel.odr;
 296
 297        for (i = 0; i < ARRAY_SIZE(inv_icm42600_accel_odr_conv); ++i) {
 298                if (inv_icm42600_accel_odr_conv[i] == odr)
 299                        break;
 300        }
 301        if (i >= ARRAY_SIZE(inv_icm42600_accel_odr_conv))
 302                return -EINVAL;
 303
 304        *val = inv_icm42600_accel_odr[2 * i];
 305        *val2 = inv_icm42600_accel_odr[2 * i + 1];
 306
 307        return IIO_VAL_INT_PLUS_MICRO;
 308}
 309
 310static int inv_icm42600_accel_write_odr(struct iio_dev *indio_dev,
 311                                        int val, int val2)
 312{
 313        struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
 314        struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
 315        struct device *dev = regmap_get_device(st->map);
 316        unsigned int idx;
 317        struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
 318        int ret;
 319
 320        for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_odr); idx += 2) {
 321                if (val == inv_icm42600_accel_odr[idx] &&
 322                    val2 == inv_icm42600_accel_odr[idx + 1])
 323                        break;
 324        }
 325        if (idx >= ARRAY_SIZE(inv_icm42600_accel_odr))
 326                return -EINVAL;
 327
 328        conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
 329
 330        pm_runtime_get_sync(dev);
 331        mutex_lock(&st->lock);
 332
 333        ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
 334                                                iio_buffer_enabled(indio_dev));
 335        if (ret)
 336                goto out_unlock;
 337
 338        ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
 339        if (ret)
 340                goto out_unlock;
 341        inv_icm42600_buffer_update_fifo_period(st);
 342        inv_icm42600_buffer_update_watermark(st);
 343
 344out_unlock:
 345        mutex_unlock(&st->lock);
 346        pm_runtime_mark_last_busy(dev);
 347        pm_runtime_put_autosuspend(dev);
 348
 349        return ret;
 350}
 351
 352/*
 353 * Calibration bias values, IIO range format int + micro.
 354 * Value is limited to +/-1g coded on 12 bits signed. Step is 0.5mg.
 355 */
 356static int inv_icm42600_accel_calibbias[] = {
 357        -10, 42010,             /* min: -10.042010 m/s² */
 358        0, 4903,                /* step: 0.004903 m/s² */
 359        10, 37106,              /* max: 10.037106 m/s² */
 360};
 361
 362static int inv_icm42600_accel_read_offset(struct inv_icm42600_state *st,
 363                                          struct iio_chan_spec const *chan,
 364                                          int *val, int *val2)
 365{
 366        struct device *dev = regmap_get_device(st->map);
 367        int64_t val64;
 368        int32_t bias;
 369        unsigned int reg;
 370        int16_t offset;
 371        uint8_t data[2];
 372        int ret;
 373
 374        if (chan->type != IIO_ACCEL)
 375                return -EINVAL;
 376
 377        switch (chan->channel2) {
 378        case IIO_MOD_X:
 379                reg = INV_ICM42600_REG_OFFSET_USER4;
 380                break;
 381        case IIO_MOD_Y:
 382                reg = INV_ICM42600_REG_OFFSET_USER6;
 383                break;
 384        case IIO_MOD_Z:
 385                reg = INV_ICM42600_REG_OFFSET_USER7;
 386                break;
 387        default:
 388                return -EINVAL;
 389        }
 390
 391        pm_runtime_get_sync(dev);
 392        mutex_lock(&st->lock);
 393
 394        ret = regmap_bulk_read(st->map, reg, st->buffer, sizeof(data));
 395        memcpy(data, st->buffer, sizeof(data));
 396
 397        mutex_unlock(&st->lock);
 398        pm_runtime_mark_last_busy(dev);
 399        pm_runtime_put_autosuspend(dev);
 400        if (ret)
 401                return ret;
 402
 403        /* 12 bits signed value */
 404        switch (chan->channel2) {
 405        case IIO_MOD_X:
 406                offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
 407                break;
 408        case IIO_MOD_Y:
 409                offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
 410                break;
 411        case IIO_MOD_Z:
 412                offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
 413                break;
 414        default:
 415                return -EINVAL;
 416        }
 417
 418        /*
 419         * convert raw offset to g then to m/s²
 420         * 12 bits signed raw step 0.5mg to g: 5 / 10000
 421         * g to m/s²: 9.806650
 422         * result in micro (1000000)
 423         * (offset * 5 * 9.806650 * 1000000) / 10000
 424         */
 425        val64 = (int64_t)offset * 5LL * 9806650LL;
 426        /* for rounding, add + or - divisor (10000) divided by 2 */
 427        if (val64 >= 0)
 428                val64 += 10000LL / 2LL;
 429        else
 430                val64 -= 10000LL / 2LL;
 431        bias = div_s64(val64, 10000L);
 432        *val = bias / 1000000L;
 433        *val2 = bias % 1000000L;
 434
 435        return IIO_VAL_INT_PLUS_MICRO;
 436}
 437
 438static int inv_icm42600_accel_write_offset(struct inv_icm42600_state *st,
 439                                           struct iio_chan_spec const *chan,
 440                                           int val, int val2)
 441{
 442        struct device *dev = regmap_get_device(st->map);
 443        int64_t val64;
 444        int32_t min, max;
 445        unsigned int reg, regval;
 446        int16_t offset;
 447        int ret;
 448
 449        if (chan->type != IIO_ACCEL)
 450                return -EINVAL;
 451
 452        switch (chan->channel2) {
 453        case IIO_MOD_X:
 454                reg = INV_ICM42600_REG_OFFSET_USER4;
 455                break;
 456        case IIO_MOD_Y:
 457                reg = INV_ICM42600_REG_OFFSET_USER6;
 458                break;
 459        case IIO_MOD_Z:
 460                reg = INV_ICM42600_REG_OFFSET_USER7;
 461                break;
 462        default:
 463                return -EINVAL;
 464        }
 465
 466        /* inv_icm42600_accel_calibbias: min - step - max in micro */
 467        min = inv_icm42600_accel_calibbias[0] * 1000000L +
 468              inv_icm42600_accel_calibbias[1];
 469        max = inv_icm42600_accel_calibbias[4] * 1000000L +
 470              inv_icm42600_accel_calibbias[5];
 471        val64 = (int64_t)val * 1000000LL + (int64_t)val2;
 472        if (val64 < min || val64 > max)
 473                return -EINVAL;
 474
 475        /*
 476         * convert m/s² to g then to raw value
 477         * m/s² to g: 1 / 9.806650
 478         * g to raw 12 bits signed, step 0.5mg: 10000 / 5
 479         * val in micro (1000000)
 480         * val * 10000 / (9.806650 * 1000000 * 5)
 481         */
 482        val64 = val64 * 10000LL;
 483        /* for rounding, add + or - divisor (9806650 * 5) divided by 2 */
 484        if (val64 >= 0)
 485                val64 += 9806650 * 5 / 2;
 486        else
 487                val64 -= 9806650 * 5 / 2;
 488        offset = div_s64(val64, 9806650 * 5);
 489
 490        /* clamp value limited to 12 bits signed */
 491        if (offset < -2048)
 492                offset = -2048;
 493        else if (offset > 2047)
 494                offset = 2047;
 495
 496        pm_runtime_get_sync(dev);
 497        mutex_lock(&st->lock);
 498
 499        switch (chan->channel2) {
 500        case IIO_MOD_X:
 501                /* OFFSET_USER4 register is shared */
 502                ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4,
 503                                  &regval);
 504                if (ret)
 505                        goto out_unlock;
 506                st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
 507                st->buffer[1] = offset & 0xFF;
 508                break;
 509        case IIO_MOD_Y:
 510                /* OFFSET_USER7 register is shared */
 511                ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7,
 512                                  &regval);
 513                if (ret)
 514                        goto out_unlock;
 515                st->buffer[0] = offset & 0xFF;
 516                st->buffer[1] = ((offset & 0xF00) >> 8) | (regval & 0xF0);
 517                break;
 518        case IIO_MOD_Z:
 519                /* OFFSET_USER7 register is shared */
 520                ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7,
 521                                  &regval);
 522                if (ret)
 523                        goto out_unlock;
 524                st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
 525                st->buffer[1] = offset & 0xFF;
 526                break;
 527        default:
 528                ret = -EINVAL;
 529                goto out_unlock;
 530        }
 531
 532        ret = regmap_bulk_write(st->map, reg, st->buffer, 2);
 533
 534out_unlock:
 535        mutex_unlock(&st->lock);
 536        pm_runtime_mark_last_busy(dev);
 537        pm_runtime_put_autosuspend(dev);
 538        return ret;
 539}
 540
 541static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev,
 542                                       struct iio_chan_spec const *chan,
 543                                       int *val, int *val2, long mask)
 544{
 545        struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
 546        int16_t data;
 547        int ret;
 548
 549        switch (chan->type) {
 550        case IIO_ACCEL:
 551                break;
 552        case IIO_TEMP:
 553                return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask);
 554        default:
 555                return -EINVAL;
 556        }
 557
 558        switch (mask) {
 559        case IIO_CHAN_INFO_RAW:
 560                ret = iio_device_claim_direct_mode(indio_dev);
 561                if (ret)
 562                        return ret;
 563                ret = inv_icm42600_accel_read_sensor(st, chan, &data);
 564                iio_device_release_direct_mode(indio_dev);
 565                if (ret)
 566                        return ret;
 567                *val = data;
 568                return IIO_VAL_INT;
 569        case IIO_CHAN_INFO_SCALE:
 570                return inv_icm42600_accel_read_scale(st, val, val2);
 571        case IIO_CHAN_INFO_SAMP_FREQ:
 572                return inv_icm42600_accel_read_odr(st, val, val2);
 573        case IIO_CHAN_INFO_CALIBBIAS:
 574                return inv_icm42600_accel_read_offset(st, chan, val, val2);
 575        default:
 576                return -EINVAL;
 577        }
 578}
 579
 580static int inv_icm42600_accel_read_avail(struct iio_dev *indio_dev,
 581                                         struct iio_chan_spec const *chan,
 582                                         const int **vals,
 583                                         int *type, int *length, long mask)
 584{
 585        if (chan->type != IIO_ACCEL)
 586                return -EINVAL;
 587
 588        switch (mask) {
 589        case IIO_CHAN_INFO_SCALE:
 590                *vals = inv_icm42600_accel_scale;
 591                *type = IIO_VAL_INT_PLUS_NANO;
 592                *length = ARRAY_SIZE(inv_icm42600_accel_scale);
 593                return IIO_AVAIL_LIST;
 594        case IIO_CHAN_INFO_SAMP_FREQ:
 595                *vals = inv_icm42600_accel_odr;
 596                *type = IIO_VAL_INT_PLUS_MICRO;
 597                *length = ARRAY_SIZE(inv_icm42600_accel_odr);
 598                return IIO_AVAIL_LIST;
 599        case IIO_CHAN_INFO_CALIBBIAS:
 600                *vals = inv_icm42600_accel_calibbias;
 601                *type = IIO_VAL_INT_PLUS_MICRO;
 602                return IIO_AVAIL_RANGE;
 603        default:
 604                return -EINVAL;
 605        }
 606}
 607
 608static int inv_icm42600_accel_write_raw(struct iio_dev *indio_dev,
 609                                        struct iio_chan_spec const *chan,
 610                                        int val, int val2, long mask)
 611{
 612        struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
 613        int ret;
 614
 615        if (chan->type != IIO_ACCEL)
 616                return -EINVAL;
 617
 618        switch (mask) {
 619        case IIO_CHAN_INFO_SCALE:
 620                ret = iio_device_claim_direct_mode(indio_dev);
 621                if (ret)
 622                        return ret;
 623                ret = inv_icm42600_accel_write_scale(st, val, val2);
 624                iio_device_release_direct_mode(indio_dev);
 625                return ret;
 626        case IIO_CHAN_INFO_SAMP_FREQ:
 627                return inv_icm42600_accel_write_odr(indio_dev, val, val2);
 628        case IIO_CHAN_INFO_CALIBBIAS:
 629                ret = iio_device_claim_direct_mode(indio_dev);
 630                if (ret)
 631                        return ret;
 632                ret = inv_icm42600_accel_write_offset(st, chan, val, val2);
 633                iio_device_release_direct_mode(indio_dev);
 634                return ret;
 635        default:
 636                return -EINVAL;
 637        }
 638}
 639
 640static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev,
 641                                                struct iio_chan_spec const *chan,
 642                                                long mask)
 643{
 644        if (chan->type != IIO_ACCEL)
 645                return -EINVAL;
 646
 647        switch (mask) {
 648        case IIO_CHAN_INFO_SCALE:
 649                return IIO_VAL_INT_PLUS_NANO;
 650        case IIO_CHAN_INFO_SAMP_FREQ:
 651                return IIO_VAL_INT_PLUS_MICRO;
 652        case IIO_CHAN_INFO_CALIBBIAS:
 653                return IIO_VAL_INT_PLUS_MICRO;
 654        default:
 655                return -EINVAL;
 656        }
 657}
 658
 659static int inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev,
 660                                                   unsigned int val)
 661{
 662        struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
 663        int ret;
 664
 665        mutex_lock(&st->lock);
 666
 667        st->fifo.watermark.accel = val;
 668        ret = inv_icm42600_buffer_update_watermark(st);
 669
 670        mutex_unlock(&st->lock);
 671
 672        return ret;
 673}
 674
 675static int inv_icm42600_accel_hwfifo_flush(struct iio_dev *indio_dev,
 676                                           unsigned int count)
 677{
 678        struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
 679        int ret;
 680
 681        if (count == 0)
 682                return 0;
 683
 684        mutex_lock(&st->lock);
 685
 686        ret = inv_icm42600_buffer_hwfifo_flush(st, count);
 687        if (!ret)
 688                ret = st->fifo.nb.accel;
 689
 690        mutex_unlock(&st->lock);
 691
 692        return ret;
 693}
 694
 695static const struct iio_info inv_icm42600_accel_info = {
 696        .read_raw = inv_icm42600_accel_read_raw,
 697        .read_avail = inv_icm42600_accel_read_avail,
 698        .write_raw = inv_icm42600_accel_write_raw,
 699        .write_raw_get_fmt = inv_icm42600_accel_write_raw_get_fmt,
 700        .debugfs_reg_access = inv_icm42600_debugfs_reg,
 701        .update_scan_mode = inv_icm42600_accel_update_scan_mode,
 702        .hwfifo_set_watermark = inv_icm42600_accel_hwfifo_set_watermark,
 703        .hwfifo_flush_to_buffer = inv_icm42600_accel_hwfifo_flush,
 704};
 705
 706struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
 707{
 708        struct device *dev = regmap_get_device(st->map);
 709        const char *name;
 710        struct inv_icm42600_timestamp *ts;
 711        struct iio_dev *indio_dev;
 712        int ret;
 713
 714        name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
 715        if (!name)
 716                return ERR_PTR(-ENOMEM);
 717
 718        indio_dev = devm_iio_device_alloc(dev, sizeof(*ts));
 719        if (!indio_dev)
 720                return ERR_PTR(-ENOMEM);
 721
 722        ts = iio_priv(indio_dev);
 723        inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.accel.odr));
 724
 725        iio_device_set_drvdata(indio_dev, st);
 726        indio_dev->name = name;
 727        indio_dev->info = &inv_icm42600_accel_info;
 728        indio_dev->modes = INDIO_DIRECT_MODE;
 729        indio_dev->channels = inv_icm42600_accel_channels;
 730        indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels);
 731        indio_dev->available_scan_masks = inv_icm42600_accel_scan_masks;
 732
 733        ret = devm_iio_kfifo_buffer_setup(dev, indio_dev,
 734                                          INDIO_BUFFER_SOFTWARE,
 735                                          &inv_icm42600_buffer_ops);
 736        if (ret)
 737                return ERR_PTR(ret);
 738
 739        ret = devm_iio_device_register(dev, indio_dev);
 740        if (ret)
 741                return ERR_PTR(ret);
 742
 743        return indio_dev;
 744}
 745
 746int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
 747{
 748        struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
 749        struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
 750        ssize_t i, size;
 751        unsigned int no;
 752        const void *accel, *gyro, *timestamp;
 753        const int8_t *temp;
 754        unsigned int odr;
 755        int64_t ts_val;
 756        struct inv_icm42600_accel_buffer buffer;
 757
 758        /* parse all fifo packets */
 759        for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
 760                size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
 761                                &accel, &gyro, &temp, &timestamp, &odr);
 762                /* quit if error or FIFO is empty */
 763                if (size <= 0)
 764                        return size;
 765
 766                /* skip packet if no accel data or data is invalid */
 767                if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel))
 768                        continue;
 769
 770                /* update odr */
 771                if (odr & INV_ICM42600_SENSOR_ACCEL)
 772                        inv_icm42600_timestamp_apply_odr(ts, st->fifo.period,
 773                                                         st->fifo.nb.total, no);
 774
 775                /* buffer is copied to userspace, zeroing it to avoid any data leak */
 776                memset(&buffer, 0, sizeof(buffer));
 777                memcpy(&buffer.accel, accel, sizeof(buffer.accel));
 778                /* convert 8 bits FIFO temperature in high resolution format */
 779                buffer.temp = temp ? (*temp * 64) : 0;
 780                ts_val = inv_icm42600_timestamp_pop(ts);
 781                iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
 782        }
 783
 784        return 0;
 785}
 786