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 #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
  49 enum 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
  57 static 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
  62 static 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  */
  77 struct 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
  90 static 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 */
  97 static 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
 136 out_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
 148 static 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;
 193 exit:
 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 */
 201 static 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
 216 static 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
 228 static 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 */
 259 static 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
 278 static 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
 289 static 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
 310 static 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
 344 out_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  */
 356 static 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
 362 static 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
 438 static 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
 534 out_unlock:
 535         mutex_unlock(&st->lock);
 536         pm_runtime_mark_last_busy(dev);
 537         pm_runtime_put_autosuspend(dev);
 538         return ret;
 539 }
 540
 541 static 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
 580 static 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
 608 static 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
 640 static 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
 659 static 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
 675 static 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
 695 static 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
 706 struct 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                                           &inv_icm42600_buffer_ops);
 735         if (ret)
 736                 return ERR_PTR(ret);
 737
 738         ret = devm_iio_device_register(dev, indio_dev);
 739         if (ret)
 740                 return ERR_PTR(ret);
 741
 742         return indio_dev;
 743 }
 744
 745 int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
 746 {
 747         struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
 748         struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
 749         ssize_t i, size;
 750         unsigned int no;
 751         const void *accel, *gyro, *timestamp;
 752         const int8_t *temp;
 753         unsigned int odr;
 754         int64_t ts_val;
 755         struct inv_icm42600_accel_buffer buffer;
 756
 757         /* parse all fifo packets */
 758         for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
 759                 size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
 760                                 &accel, &gyro, &temp, &timestamp, &odr);
 761                 /* quit if error or FIFO is empty */
 762                 if (size <= 0)
 763                         return size;
 764
 765                 /* skip packet if no accel data or data is invalid */
 766                 if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel))
 767                         continue;
 768
 769                 /* update odr */
 770                 if (odr & INV_ICM42600_SENSOR_ACCEL)
 771                         inv_icm42600_timestamp_apply_odr(ts, st->fifo.period,
 772                                                          st->fifo.nb.total, no);
 773
 774                 /* buffer is copied to userspace, zeroing it to avoid any data leak */
 775                 memset(&buffer, 0, sizeof(buffer));
 776                 memcpy(&buffer.accel, accel, sizeof(buffer.accel));
 777                 /* convert 8 bits FIFO temperature in high resolution format */
 778                 buffer.temp = temp ? (*temp * 64) : 0;
 779                 ts_val = inv_icm42600_timestamp_pop(ts);
 780                 iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
 781         }
 782
 783         return 0;
 784 }