drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c

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