drivers/iio/imu/inv_mpu/inv530/inv_mpu_setup.c

   1 /*
   2 * Copyright (C) 2012 Invensense, Inc.
   3 *
   4 * This software is licensed under the terms of the GNU General Public
   5 * License version 2, as published by the Free Software Foundation, and
   6 * may be copied, distributed, and modified under those terms.
   7 *
   8 * This program is distributed in the hope that it will be useful,
   9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11 * GNU General Public License for more details.
  12 */
  13 #define pr_fmt(fmt) "inv_mpu: " fmt
  14
  15 #include <linux/module.h>
  16 #include <linux/init.h>
  17 #include <linux/slab.h>
  18 #include <linux/err.h>
  19 #include <linux/delay.h>
  20 #include <linux/sysfs.h>
  21 #include <linux/jiffies.h>
  22 #include <linux/irq.h>
  23 #include <linux/interrupt.h>
  24 #include <linux/kfifo.h>
  25 #include <linux/poll.h>
  26 #include <linux/miscdevice.h>
  27
  28 #include "iio.h"
  29 #include "kfifo_buf.h"
  30 #include "trigger_consumer.h"
  31 #include "sysfs.h"
  32
  33 #include "inv_mpu_iio.h"
  34 struct inv_local_store {
  35         u8 reg_pwr_mgmt_2;
  36         u8 reg_lp_config;
  37         u8 reg_fifo_cfg;
  38         u8 reg_fifo_size_0;
  39         u8 reg_delay_enable;
  40         u8 reg_delay_time;
  41         u8 reg_gyro_smplrt;
  42         u8 reg_accel_smplrt;
  43         bool batchmode_en;
  44         bool d_flag;
  45         bool step_indicator_on;
  46         bool geomag_enable;
  47         bool step_detector_on;
  48         bool accel_cal_enable;
  49         bool gyro_cal_enable;
  50         bool compass_cal_enable;
  51         bool wom_on;
  52         int ped_rate;
  53         int cpass_cal_ind;
  54         int accel_cal_ind;
  55 };
  56
  57 static struct inv_local_store local;
  58
  59 struct inv_compass_cal_params {
  60         int ct;
  61         int RADIUS_3D;
  62 };
  63
  64 static struct inv_compass_cal_params compass_cal_param[] = {
  65         {
  66                 .ct = 200,
  67         },
  68         {
  69                 .ct = 70,
  70                 .RADIUS_3D = 3584,
  71         },
  72         {
  73                 .ct = 35,
  74                 .RADIUS_3D = 3584,
  75         },
  76         {
  77                 .ct = 15,
  78                 .RADIUS_3D = 3584,
  79         },
  80         {
  81                 .ct = 8,
  82                 .RADIUS_3D = 3584,
  83         },
  84         {
  85                 .ct = 4,
  86                 .RADIUS_3D = 3584,
  87         },
  88 };
  89
  90 struct inv_accel_cal_params {
  91         int freq;
  92         int rate;
  93         int gain;
  94         int alpha;
  95         int a;
  96 };
  97
  98 static struct inv_accel_cal_params accel_cal_para[] = {
  99         {
 100                 .freq = 1000,
 101         },
 102         {
 103                 .freq = 225,
 104                 .rate = 3,
 105                 .gain = DEFAULT_ACCEL_GAIN,
 106                 .alpha = 858993459,
 107                 .a     = 214748365,
 108         },
 109         {
 110                 .freq = 102,
 111                 .gain = DEFAULT_ACCEL_GAIN,
 112                 .rate = 1,
 113                 .alpha = 858993459,
 114                 .a     = 214748365,
 115         },
 116         {
 117                 .freq = PEDOMETER_FREQ,
 118                 .gain = PED_ACCEL_GAIN,
 119                 .rate = 0,
 120                 .alpha = 858993459,
 121                 .a     = 214748365,
 122         },
 123         {
 124                 .freq = 15,
 125                 .gain = DEFAULT_ACCEL_GAIN,
 126                 .rate = 0,
 127                 .alpha = 357913941,
 128                 .a     = 715827883,
 129         },
 130         {
 131                 .freq = 5,
 132                 .gain = DEFAULT_ACCEL_GAIN,
 133                 .rate = 0,
 134                 .alpha = 107374182,
 135                 .a     = 966367642,
 136         },
 137 };
 138
 139 static int accel_gyro_rate[] = {5, 6, 7, 8, 9, 10, 11, 12, 13,
 140         14, 15, 17, 18, 22, 25, 28, 32, 37, 45,
 141         51, 75, 102, 225};
 142
 143 static int inv_out_data_cntl(struct inv_mpu_state *st, u16 wd, bool en)
 144 {
 145         return inv_write_cntl(st, wd, en, DATA_OUT_CTL1);
 146 }
 147
 148 static int inv_out_data_cntl_2(struct inv_mpu_state *st, u16 wd, bool en)
 149 {
 150         return inv_write_cntl(st, wd, en, DATA_OUT_CTL2);
 151 }
 152
 153 static int inv_set_batchmode(struct inv_mpu_state *st, bool enable)
 154 {
 155         int result;
 156
 157         if (local.batchmode_en != enable) {
 158                 result = inv_out_data_cntl_2(st, BATCH_MODE_EN, enable);
 159                 if (result)
 160                         return result;
 161                 local.batchmode_en = enable;
 162         }
 163
 164         return 0;
 165 }
 166 static int inv_calc_engine_dur(struct inv_engine_info *ei)
 167 {
 168         if (!ei->running_rate)
 169                 return -EINVAL;
 170
 171         ei->dur = ei->base_time / ei->orig_rate;
 172         ei->dur *= ei->divider;
 173
 174         return 0;
 175 }
 176 static int inv_batchmode_calc(struct inv_mpu_state *st)
 177 {
 178         int b, timeout;
 179         int i, bps, max_rate;
 180         enum INV_ENGINE eng;
 181         int ps, real_rate;
 182         int en_num, en_sen, count;
 183
 184         max_rate = 0;
 185         bps = 0;
 186         ps = 0;
 187         en_num = 0;
 188         en_sen = 0;
 189         count = 0;
 190         for (i = 0; i < SENSOR_NUM_MAX; i++) {
 191                 if (st->sensor[i].on) {
 192                         if (max_rate < st->sensor[i].rate) {
 193                                 max_rate = st->sensor[i].rate;
 194                         }
 195                         /* get actual rate */
 196                         real_rate = (MSEC_PER_SEC * NSEC_PER_MSEC) /
 197                                 (st->sensor[i].dur);
 198                         if (((MSEC_PER_SEC * NSEC_PER_MSEC) %
 199                                 (st->sensor[i].dur)) > (st->sensor[i].dur / 2))
 200                                 real_rate++;
 201                         b = st->sensor[i].sample_size + 2;
 202                         bps += b * real_rate;
 203                         ps = b;
 204                         en_sen = i;
 205                         en_num++;
 206                 }
 207         }
 208         st->batch.max_rate = max_rate;
 209         if (max_rate  && bps) {
 210                 b = st->batch.timeout * bps;
 211                 if ((b > (FIFO_SIZE * MSEC_PER_SEC)) &&
 212                                                 (!st->batch.overflow_on)) {
 213                         if (en_num > 1)
 214                                 timeout = FIFO_SIZE * MSEC_PER_SEC / bps;
 215                         else {
 216                                 count = FIFO_SIZE / ps *
 217                                         st->sensor[en_sen].div;
 218                                 timeout = 0;
 219                         }
 220                 } else
 221                         timeout = st->batch.timeout;
 222
 223                 if (st->chip_config.gyro_enable)
 224                         eng = ENGINE_GYRO;
 225                 else if (st->chip_config.accel_enable)
 226                         eng = ENGINE_ACCEL;
 227                 else
 228                         eng = ENGINE_I2C;
 229         } else {
 230                 if (st->chip_config.step_detector_on ||
 231                                         st->chip_config.step_indicator_on) {
 232                         eng = ENGINE_ACCEL;
 233                         timeout = st->batch.timeout;
 234                 } else {
 235                         return -EINVAL;
 236                 }
 237         }
 238
 239         st->batch.engine_base = eng;
 240         if (timeout) {
 241                 b = st->eng_info[eng].dur / USEC_PER_MSEC;
 242                 count = timeout * USEC_PER_MSEC / b;
 243         }
 244         st->batch.counter = count;
 245
 246         if (st->batch.counter)
 247                 st->batch.on = true;
 248
 249         return 0;
 250 }
 251
 252 static int inv_set_default_batch(struct inv_mpu_state *st)
 253 {
 254         if (st->batch.max_rate > DEFAULT_BATCH_RATE) {
 255                 st->batch.default_on = true;
 256                 st->batch.counter = DEFAULT_BATCH_TIME * NSEC_PER_MSEC /
 257                                         st->eng_info[ENGINE_GYRO].dur;
 258         }
 259
 260         return 0;
 261 }
 262 int inv_batchmode_setup(struct inv_mpu_state *st)
 263 {
 264         int r;
 265         bool on;
 266         s16 mask[ENGINE_NUM_MAX] = {1, 2, 8, 8};
 267
 268         r = write_be32_to_mem(st, 0, BM_BATCH_CNTR);
 269         if (r)
 270                 return r;
 271         st->batch.on = false;
 272         st->batch.default_on = false;
 273         if (st->batch.timeout > 0) {
 274                 r = inv_batchmode_calc(st);
 275                 if (r)
 276                         return r;
 277         } else {
 278                 r = inv_set_default_batch(st);
 279                 if (r)
 280                         return r;
 281         }
 282
 283         on = (st->batch.on || st->batch.default_on);
 284
 285         if (on) {
 286                 r = write_be32_to_mem(st, st->batch.counter, BM_BATCH_THLD);
 287                 if (r)
 288                         return r;
 289                 r = inv_write_2bytes(st, BM_BATCH_MASK,
 290                                                 mask[st->batch.engine_base]);
 291                 if (r)
 292                         return r;
 293         }
 294         r = inv_set_batchmode(st, on);
 295
 296         return r;
 297 }
 298
 299 static int inv_turn_on_fifo(struct inv_mpu_state *st)
 300 {
 301         u8 w, x;
 302         int r;
 303         int i;
 304
 305         /* clear FIFO data */
 306         r = inv_plat_single_write(st, REG_FIFO_RST, MAX_5_BIT_VALUE);
 307         if (r)
 308                 return r;
 309         if (!st->chip_config.dmp_on)
 310                 w = 0;
 311         else
 312                 w = MAX_5_BIT_VALUE - 1;
 313         r = inv_plat_single_write(st, REG_FIFO_RST, w);
 314         if (r)
 315                 return r;
 316         /* turn on FIFO output data  for non-DMP mode */
 317         w = 0;
 318         x = 0;
 319         if (!st->chip_config.dmp_on) {
 320                 if (st->sensor[SENSOR_GYRO].on)
 321                         w |= BITS_GYRO_FIFO_EN;
 322                 if (st->sensor[SENSOR_ACCEL].on)
 323                         w |= BIT_ACCEL_FIFO_EN;
 324                 if (st->sensor[SENSOR_COMPASS].on)
 325                         x |= BIT_SLV_0_FIFO_EN;
 326         }
 327         r = inv_plat_single_write(st, REG_FIFO_EN_2, w);
 328         if (r)
 329                 return r;
 330         r = inv_plat_single_write(st, REG_FIFO_EN, x);
 331         if (r)
 332                 return r;
 333         /* turn on user ctrl register */
 334         if (st->chip_config.dmp_on) {
 335                 w = BIT_DMP_RST;
 336                 r = inv_plat_single_write(st, REG_USER_CTRL, w | st->i2c_dis);
 337                 if (r)
 338                         return r;
 339                 msleep(DMP_RESET_TIME);
 340         }
 341         /* reset counters in dmp */
 342         if (st->chip_config.dmp_on) {
 343                 for (i = 0; i < SENSOR_NUM_MAX; i++) {
 344                         if (st->sensor[i].on) {
 345                                         r = inv_write_2bytes(st,
 346                                                 st->sensor[i].counter_addr,
 347                                                 st->sensor[i].div);
 348                                         if (r)
 349                                                 return r;
 350                         }
 351                 }
 352         }
 353         /* turn on interrupt */
 354         if (st->chip_config.dmp_on) {
 355                 r = inv_plat_single_write(st, REG_INT_ENABLE, BIT_DMP_INT_EN);
 356                 if (r)
 357                         return r;
 358                 r = inv_plat_single_write(st, REG_INT_ENABLE_2,
 359                                                         BIT_FIFO_OVERFLOW_EN_0);
 360         } else {
 361                 w = 0;
 362                 if (st->sensor[SENSOR_GYRO].on && st->sensor[SENSOR_ACCEL].on)
 363                         w = (BIT_DATA_RDY_0_EN | BIT_DATA_RDY_1_EN);
 364                 else
 365                         w = BIT_DATA_RDY_0_EN;
 366                 r = inv_plat_single_write(st, REG_INT_ENABLE_1, w);
 367         }
 368
 369         w = BIT_FIFO_EN;
 370         if (st->chip_config.dmp_on)
 371                 w |= BIT_DMP_EN;
 372         if (st->chip_config.slave_enable)
 373                         w |= BIT_I2C_MST_EN;
 374         r = inv_plat_single_write(st, REG_USER_CTRL, w | st->i2c_dis);
 375         if (r)
 376                 return r;
 377
 378         return r;
 379 }
 380
 381 static int inv_turn_off_fifo(struct inv_mpu_state *st)
 382 {
 383         int r;
 384
 385         if (st->chip_config.dmp_on) {
 386                 r = inv_plat_single_write(st, REG_INT_ENABLE, 0);
 387                 if (r)
 388                         return r;
 389                 r = inv_plat_single_write(st, REG_INT_ENABLE_2, 0);
 390         } else {
 391                 r = inv_plat_single_write(st, REG_INT_ENABLE_1, 0);
 392                 if (r)
 393                         return r;
 394                 r = inv_plat_single_write(st, REG_FIFO_EN_2, 0);
 395         }
 396         if (r)
 397                 return r;
 398
 399         r = inv_plat_single_write(st, REG_FIFO_RST, MAX_5_BIT_VALUE);
 400         if (r)
 401                 return r;
 402         r = inv_plat_single_write(st, REG_FIFO_RST, 0x0);
 403
 404         return r;
 405 }
 406
 407 /*
 408  *  inv_reset_fifo() - Reset FIFO related registers.
 409  */
 410 int inv_reset_fifo(struct iio_dev *indio_dev, bool turn_off)
 411 {
 412         struct inv_mpu_state *st = iio_priv(indio_dev);
 413         int r, i;
 414
 415         if (turn_off) {
 416                 r = inv_turn_off_fifo(st);
 417                 if (r)
 418                         return r;
 419         }
 420         r = inv_turn_on_fifo(st);
 421         if (r)
 422                 return r;
 423         st->last_run_time = get_time_ns();
 424         for (i = 0; i < SENSOR_NUM_MAX; i++) {
 425                 if (st->sensor[i].on) {
 426                         st->sensor[i].ts = st->last_run_time;
 427                         st->sensor[i].time_calib = st->last_run_time;
 428                         st->sensor[i].sample_calib = 0;
 429                         st->sensor[i].calib_flag = 0;
 430                 }
 431         }
 432         r = read_be32_from_mem(st, &st->start_dmp_counter, DMPRATE_CNTR);
 433         if (r)
 434                 return r;
 435         for (i = 0; i < ENGINE_NUM_MAX; i++)
 436                 st->eng_info[i].last_update_time = st->last_run_time;
 437
 438         st->step_detector_base_ts = st->last_run_time;
 439         st->ts_for_calib = st->last_run_time;
 440         st->last_temp_comp_time = st->last_run_time;
 441         st->left_over_size        = 0;
 442
 443         return 0;
 444 }
 445
 446 static int inv_turn_on_engine(struct inv_mpu_state *st)
 447 {
 448         u8 w;
 449         int r;
 450
 451         r = 0;
 452         if (ICM20728 == st->chip_type) {
 453                 if (st->chip_config.gyro_enable |
 454                         st->chip_config.accel_enable |
 455                         st->chip_config.pressure_enable) {
 456                         w = 0;
 457                         if (!st->chip_config.gyro_enable)
 458                                 w |= BIT_PWR_GYRO_STBY;
 459                         if (!st->chip_config.accel_enable)
 460                                 w |= BIT_PWR_ACCEL_STBY;
 461                         if (!st->chip_config.pressure_enable)
 462                                 w |= BIT_PWR_PRESSURE_STBY;
 463                 } else {
 464                         w = (BIT_PWR_GYRO_STBY |
 465                                         BIT_PWR_ACCEL_STBY |
 466                                         BIT_PWR_PRESSURE_STBY);
 467                 }
 468         } else {
 469                 if (st->chip_config.gyro_enable |
 470                                 st->chip_config.accel_enable) {
 471                         w = BIT_PWR_PRESSURE_STBY;
 472                         if (!st->chip_config.gyro_enable)
 473                                 w |= BIT_PWR_GYRO_STBY;
 474                         if (!st->chip_config.accel_enable)
 475                                 w |= BIT_PWR_ACCEL_STBY;
 476                 } else {
 477                         w = (BIT_PWR_GYRO_STBY |
 478                                         BIT_PWR_ACCEL_STBY |
 479                                         BIT_PWR_PRESSURE_STBY);
 480                 }
 481         }
 482         r = inv_plat_single_write(st, REG_PWR_MGMT_2, w);
 483         if (r)
 484                 return r;
 485
 486         if (st->chip_config.gyro_enable)
 487                 msleep(GYRO_ENGINE_UP_TIME);
 488
 489         if (st->chip_config.has_compass) {
 490                 if (st->chip_config.compass_enable)
 491                         r = st->slave_compass->resume(st);
 492                 else
 493                         r = st->slave_compass->suspend(st);
 494                 if (r)
 495                         return r;
 496         }
 497         if (st->chip_config.has_als) {
 498                 if (st->chip_config.als_enable)
 499                         r = st->slave_als->resume(st);
 500                 else
 501                         r = st->slave_als->suspend(st);
 502                 if (r)
 503                         return r;
 504         }
 505         if (st->chip_config.has_pressure &&  (ICM20628 == st->chip_type)) {
 506                 if (st->chip_config.pressure_enable)
 507                         r = st->slave_pressure->resume(st);
 508                 else
 509                         r = st->slave_pressure->suspend(st);
 510                 if (r)
 511                         return r;
 512         }
 513
 514         /* secondary cycle mode should be set all the time */
 515         w = BIT_I2C_MST_CYCLE;
 516         if (st->chip_config.low_power_gyro_on)
 517                 w |= BIT_GYRO_CYCLE;
 518         w |= BIT_ACCEL_CYCLE;
 519         if (w != local.reg_lp_config) {
 520                 r = inv_plat_single_write(st, REG_LP_CONFIG, w);
 521                 local.reg_lp_config = w;
 522         }
 523
 524         return r;
 525 }
 526
 527 static int inv_setup_dmp_rate(struct inv_mpu_state *st)
 528 {
 529         int i, result, tmp;
 530         int div[SENSOR_NUM_MAX];
 531         bool d_flag;
 532
 533         result = 0;
 534         for (i = 0; i < SENSOR_NUM_MAX; i++) {
 535                 if (st->sensor[i].on) {
 536                         if (!st->sensor[i].rate) {
 537                                 pr_err("sensor %d rate is zero\n", i);
 538                                 return -EINVAL;
 539                         }
 540                         div[i] =
 541                         st->eng_info[st->sensor[i].engine_base].running_rate /
 542                                                         st->sensor[i].rate;
 543                         if (!div[i])
 544                                 div[i] = 1;
 545                 }
 546         }
 547     /* make geomag and rv consistent */
 548     if (st->sensor[SENSOR_NINEQ].on &&
 549                                         st->sensor[SENSOR_GEOMAG].on) {
 550                 tmp = min(div[SENSOR_GEOMAG], div[SENSOR_NINEQ]);
 551                 div[SENSOR_GEOMAG] = tmp;
 552                 div[SENSOR_NINEQ] = tmp;
 553         }
 554
 555         /* make compass and calib compass consistent */
 556         if (st->sensor[SENSOR_COMPASS].on &&
 557                                         st->sensor[SENSOR_CALIB_COMPASS].on) {
 558                 tmp = min(div[SENSOR_COMPASS], div[SENSOR_CALIB_COMPASS]);
 559                 div[SENSOR_COMPASS] = tmp;
 560                 div[SENSOR_CALIB_COMPASS] = tmp;
 561         }
 562
 563         /* make gyro and calib gyro consistent */
 564         if (st->sensor[SENSOR_GYRO].on &&
 565                                         st->sensor[SENSOR_CALIB_GYRO].on) {
 566                 tmp = min(div[SENSOR_GYRO], div[SENSOR_CALIB_GYRO]);
 567                 div[SENSOR_GYRO] = tmp;
 568                 div[SENSOR_CALIB_GYRO] = tmp;
 569         }
 570         for (i = 0; i < SENSOR_NUM_MAX; i++) {
 571                 if (st->sensor[i].on) {
 572                         st->sensor[i].dur =
 573                                 st->eng_info[st->sensor[i].engine_base].dur *
 574                                                 div[i];
 575                         if (div[i] != st->sensor[i].div) {
 576                                 st->sensor[i].div = div[i];
 577                                 result = inv_write_2bytes(st,
 578                                         st->sensor[i].odr_addr, div[i] - 1);
 579                                 if (result)
 580                                         return result;
 581                                 result = inv_out_data_cntl(st,
 582                                                 st->sensor[i].output, true);
 583                                 if (result)
 584                                         return result;
 585                         }
 586                 } else if (-1 != st->sensor[i].div) {
 587                         result = inv_out_data_cntl(st,
 588                                         st->sensor[i].output, false);
 589                         if (result)
 590                                 return result;
 591                         st->sensor[i].div = -1;
 592                 }
 593         }
 594
 595         d_flag = 0;
 596         for (i = 0; i < SENSOR_ACCURACY_NUM_MAX; i++) {
 597                 result = inv_out_data_cntl_2(st,
 598                                 st->sensor_accuracy[i].output,
 599                                         st->sensor_accuracy[i].on);
 600                 if (result)
 601                         return result;
 602                 d_flag |= st->sensor_accuracy[i].on;
 603         }
 604
 605         if (d_flag != local.d_flag) {
 606                 result = inv_out_data_cntl(st, HEADER2_SET, d_flag);
 607                 if (result)
 608                         return result;
 609                 local.d_flag = d_flag;
 610         }
 611         if (st->chip_config.step_indicator_on != local.step_indicator_on) {
 612                 result = inv_out_data_cntl(st, PED_STEPIND_SET,
 613                                         st->chip_config.step_indicator_on);
 614                 if (result)
 615                         return result;
 616                 local.step_indicator_on = st->chip_config.step_indicator_on;
 617         }
 618         if (st->chip_config.geomag_enable != local.geomag_enable) {
 619                 result = inv_out_data_cntl_2(st, GEOMAG_EN,
 620                                                 st->chip_config.geomag_enable);
 621                 if (result)
 622                         return result;
 623                 local.geomag_enable = st->chip_config.geomag_enable;
 624         }
 625         if (st->chip_config.step_detector_on != local.step_detector_on) {
 626                 result = inv_out_data_cntl(st, PED_STEPDET_SET,
 627                                         st->chip_config.step_detector_on);
 628                 if (result)
 629                         return result;
 630                 local.step_detector_on = st->chip_config.step_detector_on;
 631         }
 632
 633         if (!st->chip_config.dmp_event_int_on)
 634                 result = inv_batchmode_setup(st);
 635
 636         return result;
 637 }
 638
 639 static int inv_set_pressure_rate(struct inv_mpu_state *st)
 640 {
 641         int r, i;
 642
 643         if ((!st->chip_config.gyro_enable) && (!st->chip_config.accel_enable))
 644                 r = max(st->sensor[SENSOR_PRESSURE].rate,
 645                                         st->chip_config.compass_rate);
 646         else
 647                 r = st->sensor[SENSOR_PRESSURE].rate;
 648         if (r > 0) {
 649                 i = -1;
 650                 while (r < MAX_PRS_RATE) {
 651                         r <<= 1;
 652                         i++;
 653                 }
 654         } else {
 655                 return -EINVAL;
 656         }
 657         if (i < 0)
 658                 i = 0;
 659         st->eng_info[ENGINE_PRESSURE].running_rate = (MAX_PRS_RATE >> i);
 660         st->eng_info[ENGINE_PRESSURE].divider      = (1 << i);
 661         r = inv_calc_engine_dur(&st->eng_info[ENGINE_PRESSURE]);
 662         if (r)
 663                 return r;
 664         r = inv_plat_single_write(st, REG_PRS_ODR_CONFIG, i);
 665
 666         return r;
 667 }
 668
 669 static int inv_set_div(struct inv_mpu_state *st, int a_d, int g_d)
 670 {
 671         int result;
 672
 673         result = inv_set_bank(st, BANK_SEL_2);
 674         if (result)
 675                 return result;
 676
 677         if (local.reg_gyro_smplrt != g_d) {
 678                 result = inv_plat_single_write(st, REG_GYRO_SMPLRT_DIV, g_d);
 679                 if (result)
 680                         return result;
 681                 local.reg_gyro_smplrt = g_d;
 682         }
 683         if (local.reg_accel_smplrt != a_d) {
 684                 result = inv_plat_single_write(st, REG_ACCEL_SMPLRT_DIV_2, a_d);
 685                 if (result)
 686                         return result;
 687                 local.reg_accel_smplrt = a_d;
 688         }
 689         if (st->chip_config.pressure_enable && (ICM20728 == st->chip_type)) {
 690                 result = inv_set_pressure_rate(st);
 691                 if (result)
 692                         return result;
 693         }
 694         result = inv_set_bank(st, BANK_SEL_0);
 695
 696         return result;
 697 }
 698
 699 static int inv_set_rate(struct inv_mpu_state *st)
 700 {
 701         int g_d, a_d, result;
 702
 703         if (st->chip_config.dmp_on) {
 704                 result = inv_setup_dmp_rate(st);
 705                 if (result)
 706                         return result;
 707         } else {
 708                 st->eng_info[ENGINE_GYRO].running_rate =
 709                                                 st->sensor[SENSOR_GYRO].rate;
 710                 st->eng_info[ENGINE_ACCEL].running_rate =
 711                                                 st->sensor[SENSOR_ACCEL].rate;
 712         }
 713
 714         g_d = st->eng_info[ENGINE_GYRO].divider - 1;
 715         a_d = st->eng_info[ENGINE_ACCEL].divider - 1;
 716         result = inv_set_div(st, a_d, g_d);
 717         if (result)
 718                 return result;
 719
 720         return result;
 721 }
 722
 723 static int inv_set_fifo_size(struct inv_mpu_state *st)
 724 {
 725         int result;
 726         u8 cfg, ind;
 727
 728         result = 0;
 729         if (st->chip_config.dmp_on) {
 730                 /* use one FIFO in DMP mode */
 731                 cfg = BIT_SINGLE_FIFO_CFG;
 732         } else {
 733                 ind = 0;
 734                 if (st->sensor[SENSOR_GYRO].on)
 735                         ind++;
 736                 if (st->sensor[SENSOR_ACCEL].on)
 737                         ind++;
 738                 if (st->sensor[SENSOR_COMPASS].on)
 739                         ind++;
 740                 if (ind > 1)
 741                         cfg = BIT_MULTI_FIFO_CFG;
 742                 else
 743                         cfg = BIT_SINGLE_FIFO_CFG;
 744         }
 745         result = inv_plat_single_write(st, REG_FIFO_CFG, cfg);
 746         if (result)
 747                 return result;
 748
 749         return 0;
 750 }
 751
 752 /*
 753  *  inv_set_fake_secondary() - set fake secondary I2C such that
 754  *                           I2C data in the same position.
 755  */
 756 static int inv_set_fake_secondary(struct inv_mpu_state *st)
 757 {
 758         int r;
 759         u8 bytes, ind;
 760
 761         /* may need to saturate the master I2C counter like Scorpion did */
 762         r = inv_set_bank(st, BANK_SEL_3);
 763         if (r)
 764                 return r;
 765         if (st->sec_set.delay_enable != local.reg_delay_enable) {
 766                 r = inv_plat_single_write(st, REG_I2C_MST_DELAY_CTRL,
 767                                                 st->sec_set.delay_enable);
 768                 if (r)
 769                         return r;
 770                 local.reg_delay_enable = st->sec_set.delay_enable;
 771         }
 772         if (st->sec_set.delay_time != local.reg_delay_time) {
 773                 r = inv_plat_single_write(st, REG_I2C_SLV4_CTRL,
 774                                         st->sec_set.delay_time);
 775                 if (r)
 776                         return r;
 777                 local.reg_delay_time = st->sec_set.delay_time;
 778         }
 779         /* odr config is changed during slave setup */
 780         r = inv_plat_single_write(st, REG_I2C_MST_ODR_CONFIG,
 781                                                 st->sec_set.odr_config);
 782         if (r)
 783                 return r;
 784         r = inv_set_bank(st, BANK_SEL_0);
 785         if (r)
 786                 return r;
 787
 788         if (ICM20728 == st->chip_type)
 789                 return 0;
 790         /*111, 110 */
 791         if (st->chip_config.compass_enable && st->chip_config.als_enable)
 792                 return 0;
 793         /* 100 */
 794         if (st->chip_config.compass_enable &&
 795                 (!st->chip_config.als_enable) &&
 796                 (!st->chip_config.pressure_enable))
 797                 return 0;
 798         r = inv_set_bank(st, BANK_SEL_3);
 799         if (r)
 800                 return r;
 801
 802         if (st->chip_config.pressure_enable) {
 803                 /* 001 */
 804                 if ((!st->chip_config.compass_enable) &&
 805                                         (!st->chip_config.als_enable)) {
 806                         r = inv_read_secondary(st, 0,
 807                                                 st->plat_data.aux_i2c_addr,
 808                                                 BMP280_DIG_T1_LSB_REG,
 809                                                 DATA_AKM_99_BYTES_DMP);
 810                         if (r)
 811                                 return r;
 812                         r = inv_read_secondary(st, 2,
 813                                                 st->plat_data.aux_i2c_addr,
 814                                                 BMP280_DIG_T1_LSB_REG,
 815                                                 DATA_ALS_BYTES_DMP);
 816                         r = inv_set_bank(st, BANK_SEL_0);
 817
 818                         return r;
 819                 }
 820
 821                 if (st->chip_config.compass_enable &&
 822                                         (!st->chip_config.als_enable)) {
 823                         /* 101 */
 824                         ind = 2;
 825                         if ((COMPASS_ID_AK09911 ==
 826                                                 st->plat_data.sec_slave_id) ||
 827                                         (COMPASS_ID_AK09912 ==
 828                                                 st->plat_data.sec_slave_id))
 829                                 bytes = DATA_ALS_BYTES_DMP;
 830                         else
 831                                 bytes = DATA_ALS_BYTES_DMP +
 832                                         DATA_AKM_99_BYTES_DMP -
 833                                         DATA_AKM_89_BYTES_DMP;
 834                 } else { /* 011 */
 835                         ind = 0;
 836                         bytes = DATA_AKM_99_BYTES_DMP;
 837                 }
 838                 r = inv_read_secondary(st, ind, st->plat_data.aux_i2c_addr,
 839                                                 BMP280_DIG_T1_LSB_REG, bytes);
 840         } else { /* compass disabled; als enabled, pressure disabled 010 */
 841                 r = inv_read_secondary(st, 0, st->plat_data.read_only_i2c_addr,
 842                                 APDS9900_AILTL_REG, DATA_AKM_99_BYTES_DMP);
 843         }
 844         if (r)
 845                 return r;
 846         r = inv_set_bank(st, BANK_SEL_0);
 847
 848         return r;
 849 }
 850 static int inv_set_ICM20728_secondary(struct inv_mpu_state *st)
 851 {
 852         return 0;
 853 }
 854
 855 static int inv_set_ICM20628_secondary(struct inv_mpu_state *st)
 856 {
 857         int rate, compass_rate, pressure_rate, als_rate, min_rate, base;
 858         int mst_odr_config, d, delay;
 859
 860         if (st->chip_config.compass_enable)
 861                 compass_rate = st->chip_config.compass_rate;
 862         else
 863                 compass_rate = 0;
 864         if (st->chip_config.pressure_enable)
 865                 pressure_rate = st->sensor[SENSOR_PRESSURE].rate;
 866         else
 867                 pressure_rate = 0;
 868         if (st->chip_config.als_enable)
 869                 als_rate = st->sensor[SENSOR_ALS].rate;
 870         else
 871                 als_rate = 0;
 872         if (compass_rate)
 873                 rate = compass_rate;
 874         else
 875                 rate = max(pressure_rate, als_rate);
 876         mst_odr_config = 0;
 877         min_rate = BASE_SAMPLE_RATE;
 878         rate += (rate >> 1);
 879         while (rate < min_rate) {
 880                 mst_odr_config++;
 881                 min_rate >>= 1;
 882         }
 883         if (mst_odr_config < MIN_MST_ODR_CONFIG)
 884                 mst_odr_config = MIN_MST_ODR_CONFIG;
 885         if (compass_rate) {
 886                 if (mst_odr_config > MAX_MST_NON_COMPASS_ODR_CONFIG)
 887                         mst_odr_config = MAX_MST_NON_COMPASS_ODR_CONFIG;
 888         }
 889
 890         base = BASE_SAMPLE_RATE / (1 << mst_odr_config);
 891         if ((!st->chip_config.gyro_enable) &&
 892                         (!st->chip_config.accel_enable)) {
 893                 st->eng_info[ENGINE_I2C].running_rate = base;
 894                 st->eng_info[ENGINE_I2C].divider = (1 << mst_odr_config);
 895         }
 896         inv_calc_engine_dur(&st->eng_info[ENGINE_I2C]);
 897
 898         d = 0;
 899         if (d > 0)
 900                 d -= 1;
 901         if (d > MAX_5_BIT_VALUE)
 902                 d = MAX_5_BIT_VALUE;
 903
 904         /* I2C_MST_DLY is set to slow down secondary I2C */
 905         if (d)
 906                 delay = 0x1F;
 907         else
 908                 delay = 0;
 909
 910         st->sec_set.delay_enable = delay;
 911         st->sec_set.delay_time = d;
 912         st->sec_set.odr_config = mst_odr_config;
 913
 914         return 0;
 915 }
 916
 917 static int inv_set_master_delay(struct inv_mpu_state *st)
 918 {
 919
 920         if (!st->chip_config.slave_enable)
 921                 return 0;
 922         if (ICM20728 == st->chip_type)
 923                 inv_set_ICM20728_secondary(st);
 924         else
 925                 inv_set_ICM20628_secondary(st);
 926
 927         return 0;
 928 }
 929
 930 static int inv_enable_accel_cal_V3(struct inv_mpu_state *st, u8 enable)
 931 {
 932         return inv_write_cntl(st, ACCEL_CAL_EN << 8, enable, MOTION_EVENT_CTL);
 933 }
 934
 935 static int inv_enable_gyro_cal_V3(struct inv_mpu_state *st, u8 enable)
 936 {
 937         return inv_write_cntl(st, GYRO_CAL_EN << 8, enable, MOTION_EVENT_CTL);
 938 }
 939
 940 static int inv_enable_compass_cal_V3(struct inv_mpu_state *st, u8 enable)
 941 {
 942         return inv_write_cntl(st, COMPASS_CAL_EN, enable, MOTION_EVENT_CTL);
 943 }
 944
 945 static int inv_enable_9axes_V3(struct inv_mpu_state *st)
 946 {
 947         bool en;
 948
 949         if (st->sensor[SENSOR_NINEQ].on ||
 950                 st->sensor[SENSOR_GEOMAG].on ||
 951                         st->sensor_accuracy[SENSOR_COMPASS_ACCURACY].on)
 952                 en = true;
 953         else
 954                 en = false;
 955
 956         return inv_write_cntl(st, NINE_AXIS_EN, en, MOTION_EVENT_CTL);
 957 }
 958
 959 static int inv_set_wom(struct inv_mpu_state *st)
 960 {
 961         int result;
 962         u8 d[4] = {0, 0, 0, 0};
 963
 964         if (st->chip_config.wom_on)
 965                 d[3] = 1;
 966
 967         if (local.wom_on != st->chip_config.wom_on) {
 968                 result = mem_w(WOM_ENABLE, ARRAY_SIZE(d), d);
 969                 if (result)
 970                         return result;
 971                 local.wom_on = st->chip_config.wom_on;
 972         }
 973
 974         inv_write_2bytes(st, 0x8a, st->chip_config.gyro_enable |
 975                         (st->chip_config.accel_enable << 1) |
 976                         (st->chip_config.slave_enable << 3));
 977
 978         return 0;
 979 }
 980
 981 static int inv_setup_events(struct inv_mpu_state *st)
 982 {
 983         int result;
 984
 985         result = inv_write_cntl(st, PEDOMETER_EN << 8, st->ped.engine_on,
 986                                                         MOTION_EVENT_CTL);
 987         if (result)
 988                 return result;
 989
 990         result = inv_write_cntl(st, SMD_EN << 8, st->smd.on,
 991                                                         MOTION_EVENT_CTL);
 992
 993         return result;
 994 }
 995
 996 static int inv_setup_sensor_interrupt(struct inv_mpu_state *st)
 997 {
 998         int i, ind, rate;
 999         u16 cntl;
1000
1001         cntl = 0;
1002         ind = -1;
1003         rate = 0;
1004
1005         for (i = 0; i < SENSOR_NUM_MAX; i++) {
1006                 if (st->sensor[i].on) {
1007                         if (st->batch.on) {
1008                                 cntl |= st->sensor[i].output;
1009                         } else {
1010                                 if (st->sensor[i].rate > rate) {
1011                                         ind = i;
1012                                         rate = st->sensor[i].rate;
1013                                 }
1014                         }
1015                 }
1016         }
1017     // Take appropriate output for sensors sharing same configuration address
1018         // on DMP memory
1019         if (ind != -1) {
1020                 for (i = 0; i < SENSOR_NUM_MAX; i++) {
1021                         if ((st->sensor[i].odr_addr == st->sensor[ind].odr_addr) &&
1022                                 st->sensor[i].on) {
1023                                 if (!st->sensor[ind].output) {
1024                                         ind = i;
1025                                 }
1026                         }
1027                 }
1028         }
1029         if (ind != -1)
1030                 cntl |= st->sensor[ind].output;
1031         if (st->chip_config.step_detector_on)
1032                 cntl |= PED_STEPDET_SET;
1033
1034         return inv_write_2bytes(st, DATA_INTR_CTL, cntl);
1035 }
1036
1037 static int inv_setup_dmp(struct inv_mpu_state *st)
1038 {
1039         int result, i, tmp, min_diff, ind;
1040
1041         result = inv_setup_sensor_interrupt(st);
1042         if (result)
1043                 return result;
1044
1045         i = 0;
1046         ind = 0;
1047         min_diff = accel_cal_para[0].freq;
1048         while (i < ARRAY_SIZE(accel_cal_para)) {
1049                 tmp = abs(accel_cal_para[i].freq -
1050                                 st->eng_info[ENGINE_ACCEL].running_rate);
1051                 if (tmp < min_diff) {
1052                         min_diff = tmp;
1053                         ind = i;
1054                 }
1055                 i++;
1056         }
1057         i = ind;
1058         if (i != local.accel_cal_ind) {
1059                 result = inv_write_2bytes(st,
1060                                 ACCEL_CAL_RATE, accel_cal_para[i].rate);
1061                 if (result)
1062                         return result;
1063                 result = write_be32_to_mem(st,
1064                                 accel_cal_para[i].rate, PED_RATE);
1065                 if (result)
1066                         return result;
1067                 result = write_be32_to_mem(st,
1068                                 accel_cal_para[i].alpha, ACCEL_ALPHA_VAR);
1069                 if (result)
1070                         return result;
1071                 result = write_be32_to_mem(st,
1072                                 accel_cal_para[i].a, ACCEL_A_VAR);
1073                 if (result)
1074                         return result;
1075                 result = write_be32_to_mem(st,
1076                                 accel_cal_para[i].gain, ACCEL_ONLY_GAIN);
1077                 if (result)
1078                         return result;
1079                 local.accel_cal_ind = i;
1080         }
1081         i = 0;
1082         min_diff = compass_cal_param[0].ct;
1083         while (i < ARRAY_SIZE(compass_cal_param)) {
1084                 tmp = abs(compass_cal_param[i].ct -
1085                                 st->eng_info[ENGINE_I2C].running_rate);
1086                 if (tmp < min_diff) {
1087                         min_diff = tmp;
1088                         ind = i;
1089                 }
1090                 i++;
1091         }
1092         i = ind;
1093         if ((i != local.cpass_cal_ind) &&
1094                                 (st->eng_info[ENGINE_I2C].running_rate)) {
1095                 result = inv_write_2bytes(st, CPASS_TIME_BUFFER,
1096                                                 compass_cal_param[i].ct);
1097                 if (result)
1098                         return result;
1099                 result = write_be32_to_mem(st, compass_cal_param[i].RADIUS_3D,
1100                                                 CPASS_RADIUS_3D_THRESH_ANOMALY);
1101                 if (result)
1102                         return result;
1103
1104                 local.cpass_cal_ind = i;
1105         }
1106
1107         if (local.accel_cal_enable != st->accel_cal_enable) {
1108                 result = inv_enable_accel_cal_V3(st, st->accel_cal_enable);
1109                 if (result)
1110                         return result;
1111                 local.accel_cal_enable = st->accel_cal_enable;
1112         }
1113         if (local.gyro_cal_enable != st->gyro_cal_enable) {
1114                 result = inv_enable_gyro_cal_V3(st, st->gyro_cal_enable);
1115                 if (result)
1116                         return result;
1117                 local.gyro_cal_enable = st->gyro_cal_enable;
1118         }
1119         if (local.compass_cal_enable != st->compass_cal_enable) {
1120                 result = inv_enable_compass_cal_V3(st, st->compass_cal_enable);
1121                 if (result)
1122                         return result;
1123                 local.compass_cal_enable = st->compass_cal_enable;
1124         }
1125         result = inv_enable_9axes_V3(st);
1126         if (result)
1127                 return result;
1128
1129         if (EVENT_TRIGGER == st->trigger_state) {
1130                 result = inv_setup_events(st);
1131                 if (result)
1132                         return result;
1133         }
1134
1135         result = inv_set_wom(st);
1136
1137         return result;
1138 }
1139
1140 static int inv_get_accel_gyro_rate(int compass_rate)
1141 {
1142         int i;
1143
1144         i = 0;
1145         while ((i < ARRAY_SIZE(accel_gyro_rate)) &&
1146                         compass_rate > accel_gyro_rate[i])
1147                 i++;
1148
1149         return accel_gyro_rate[i];
1150 }
1151
1152 static int inv_determine_engine(struct inv_mpu_state *st)
1153 {
1154         int i;
1155         bool a_en, g_en, c_en, p_en, data_on, ped_on;
1156         int compass_rate, pressure_rate, nineq_rate, accel_rate, gyro_rate;
1157         u32 base_time;
1158
1159 #define NINEQ_MIN_COMPASS_RATE 35
1160 #define GEOMAG_MIN_COMPASS_RATE    70
1161         if (st->chip_config.debug_data_collection_mode_on) {
1162                 st->chip_config.dmp_on = 0;
1163                 st->eng_info[ENGINE_GYRO].divider =
1164                         BASE_SAMPLE_RATE /
1165                         st->chip_config.debug_data_collection_gyro_freq;
1166                 st->eng_info[ENGINE_ACCEL].divider =
1167                         BASE_SAMPLE_RATE /
1168                         st->chip_config.debug_data_collection_accel_freq;
1169                 inv_calc_engine_dur(&st->eng_info[ENGINE_GYRO]);
1170                 inv_calc_engine_dur(&st->eng_info[ENGINE_ACCEL]);
1171                 st->chip_config.gyro_enable = true;
1172                 st->chip_config.accel_enable = true;
1173                 st->sensor[SENSOR_GYRO].on = true;
1174                 st->sensor[SENSOR_ACCEL].on = true;
1175                 st->sensor[SENSOR_GYRO].dur = st->eng_info[ENGINE_GYRO].dur;
1176                 st->sensor[SENSOR_ACCEL].dur = st->eng_info[ENGINE_ACCEL].dur;
1177
1178                 return 0;
1179         }
1180         a_en = false;
1181         g_en = false;
1182         c_en = false;
1183         p_en = false;
1184         ped_on = false;
1185         data_on = false;
1186         compass_rate = 0;
1187         pressure_rate = 0;
1188         nineq_rate = 0;
1189         gyro_rate = MPU_INIT_SENSOR_RATE;
1190         accel_rate = MPU_INIT_SENSOR_RATE;
1191
1192         st->chip_config.geomag_enable = 0;
1193         if (st->sensor[SENSOR_NINEQ].on)
1194                 nineq_rate = max(nineq_rate, NINEQ_MIN_COMPASS_RATE);
1195         else if (st->sensor[SENSOR_GEOMAG].on)
1196                 nineq_rate = max(nineq_rate, GEOMAG_MIN_COMPASS_RATE);
1197
1198         if (st->sensor[SENSOR_NINEQ].on) {
1199                 st->sensor[SENSOR_GEOMAG].output = 0;
1200                 st->sensor[SENSOR_GEOMAG].sample_size = 0;
1201                 st->sensor[SENSOR_NINEQ].output  = QUAT9_SET;
1202                 st->sensor[SENSOR_NINEQ].sample_size = QUAT9_DATA_SZ;
1203         } else if (st->sensor[SENSOR_GEOMAG].on) {
1204                 st->sensor[SENSOR_NINEQ].output = 0;
1205                 st->sensor[SENSOR_NINEQ].sample_size = 0;
1206                 st->sensor[SENSOR_GEOMAG].output  = QUAT9_SET;
1207                 st->sensor[SENSOR_GEOMAG].sample_size = QUAT9_DATA_SZ;
1208                 st->chip_config.geomag_enable = 1;
1209         }
1210
1211         for (i = 0; i < SENSOR_NUM_MAX; i++) {
1212                 if (st->sensor[i].on) {
1213                         data_on = true;
1214                         a_en |= st->sensor[i].a_en;
1215                         g_en |= st->sensor[i].g_en;
1216                         c_en |= st->sensor[i].c_en;
1217                         p_en |= st->sensor[i].p_en;
1218                         if (st->sensor[i].c_en &&
1219                                 (i != SENSOR_NINEQ) &&
1220                                 (i != SENSOR_GEOMAG))
1221                                 compass_rate =
1222                                         max(compass_rate, st->sensor[i].rate);
1223                         if (st->sensor[i].p_en)
1224                                 pressure_rate =
1225                                         max(pressure_rate, st->sensor[i].rate);
1226                 }
1227         }
1228         if (st->chip_config.step_detector_on ||
1229                                         st->chip_config.step_indicator_on) {
1230                 ped_on = true;
1231                 data_on = true;
1232         }
1233         if (st->ped.on || ped_on || st->smd.on)
1234                 st->ped.engine_on = true;
1235         else
1236                 st->ped.engine_on = false;
1237         if (st->sensor[SENSOR_ALS].on)
1238                 st->chip_config.als_enable = true;
1239         else
1240                 st->chip_config.als_enable = false;
1241         if (st->ped.engine_on)
1242                 a_en = true;
1243
1244         if (data_on)
1245                 st->chip_config.dmp_event_int_on = 0;
1246         else
1247                 st->chip_config.dmp_event_int_on = 1;
1248
1249         if (st->chip_config.dmp_event_int_on)
1250                 st->chip_config.wom_on = 1;
1251         else
1252                 st->chip_config.wom_on = 0;
1253
1254         if (compass_rate < nineq_rate)
1255                 compass_rate = nineq_rate;
1256         if (compass_rate > MAX_COMPASS_RATE)
1257                 compass_rate = MAX_COMPASS_RATE;
1258         st->chip_config.compass_rate = compass_rate;
1259         if (st->sensor[SENSOR_NINEQ].on ||
1260                         st->sensor[SENSOR_GEOMAG].on ||
1261                         st->sensor[SENSOR_SIXQ].on ||
1262                         st->sensor[SENSOR_PEDQ].on) {
1263                 /* if 6 Q or 9 Q is on, set gyro/accel to default rate */
1264                 accel_rate = MPU_DEFAULT_DMP_FREQ;
1265                 gyro_rate  = MPU_DEFAULT_DMP_FREQ;
1266         } else {
1267                 if (st->ped.engine_on) {
1268                         if (st->sensor[SENSOR_ACCEL].on) {
1269                                 if (st->sensor[SENSOR_ACCEL].rate <
1270                                                         PEDOMETER_FREQ) {
1271                                         accel_rate = PEDOMETER_FREQ;
1272                                 } else {
1273                                         accel_rate =
1274                                                 st->sensor[SENSOR_ACCEL].rate;
1275                                 }
1276                         } else {
1277                                 accel_rate = PEDOMETER_FREQ;
1278                         }
1279                 } else {
1280                         accel_rate = st->sensor[SENSOR_ACCEL].rate;
1281                 }
1282         }
1283         if (a_en | g_en) {
1284                 gyro_rate = max(gyro_rate, PEDOMETER_FREQ);
1285         accel_rate = max(accel_rate, PEDOMETER_FREQ);
1286     }
1287         if (st->sensor[SENSOR_GYRO].on)
1288                 gyro_rate = max(gyro_rate, st->sensor[SENSOR_GYRO].rate);
1289         if (st->sensor[SENSOR_CALIB_GYRO].on)
1290                 gyro_rate = max(gyro_rate, st->sensor[SENSOR_CALIB_GYRO].rate);
1291
1292         if (g_en) {
1293                 if (a_en)
1294                         gyro_rate = max(gyro_rate, accel_rate);
1295                 if (c_en || p_en) {
1296                         if (gyro_rate < compass_rate)
1297                                 gyro_rate =
1298                                         inv_get_accel_gyro_rate(compass_rate);
1299                 }
1300                 accel_rate = gyro_rate;
1301                 compass_rate = gyro_rate;
1302         } else if (a_en) {
1303                 if (c_en || p_en) {
1304                         if (accel_rate < compass_rate)
1305                                 accel_rate =
1306                                         inv_get_accel_gyro_rate(compass_rate);
1307                 }
1308                 compass_rate = accel_rate;
1309                 gyro_rate = accel_rate;
1310         }
1311
1312         st->eng_info[ENGINE_GYRO].running_rate = gyro_rate;
1313         st->eng_info[ENGINE_ACCEL].running_rate = accel_rate;
1314         st->eng_info[ENGINE_PRESSURE].running_rate = MPU_DEFAULT_DMP_FREQ;
1315         st->eng_info[ENGINE_I2C].running_rate = compass_rate;
1316         /* engine divider for pressure and compass is set later */
1317         st->eng_info[ENGINE_GYRO].divider =
1318                                 (BASE_SAMPLE_RATE / MPU_DEFAULT_DMP_FREQ) *
1319                                 (MPU_DEFAULT_DMP_FREQ /
1320                                 st->eng_info[ENGINE_GYRO].running_rate);
1321         st->eng_info[ENGINE_ACCEL].divider =
1322                                 (BASE_SAMPLE_RATE / MPU_DEFAULT_DMP_FREQ) *
1323                                 (MPU_DEFAULT_DMP_FREQ /
1324                                 st->eng_info[ENGINE_ACCEL].running_rate);
1325         st->eng_info[ENGINE_I2C].divider =
1326                                 (BASE_SAMPLE_RATE / MPU_DEFAULT_DMP_FREQ) *
1327                                 (MPU_DEFAULT_DMP_FREQ /
1328                                 st->eng_info[ENGINE_ACCEL].running_rate);
1329
1330         base_time = NSEC_PER_SEC;
1331
1332         st->eng_info[ENGINE_GYRO].base_time = base_time;
1333         st->eng_info[ENGINE_ACCEL].base_time = base_time;
1334         st->eng_info[ENGINE_I2C].base_time = base_time;
1335
1336         inv_calc_engine_dur(&st->eng_info[ENGINE_GYRO]);
1337         inv_calc_engine_dur(&st->eng_info[ENGINE_ACCEL]);
1338
1339         if (st->debug_determine_engine_on)
1340                 return 0;
1341
1342         st->chip_config.gyro_enable = g_en;
1343         st->chip_config.accel_enable = a_en;
1344         st->chip_config.compass_enable = c_en;
1345         st->chip_config.pressure_enable = p_en;
1346         st->chip_config.dmp_on = 1;
1347         /* if gyro is enabled, geomag become 9 axes */
1348         if (g_en)
1349                 st->chip_config.geomag_enable = 0;
1350         else
1351                 st->chip_config.geomag_enable = 1;
1352         if (c_en || st->chip_config.als_enable ||
1353                                         (p_en && (ICM20628 == st->chip_type)))
1354                 st->chip_config.slave_enable = 1;
1355         else
1356                 st->chip_config.slave_enable = 0;
1357         if (a_en) {
1358                 st->accel_cal_enable = 1;
1359         } else {
1360                 st->accel_cal_enable = 0;
1361         }
1362         if (g_en) {
1363                 st->gyro_cal_enable = 1;
1364         } else {
1365                 st->gyro_cal_enable = 0;
1366         }
1367         if (c_en)
1368                 st->compass_cal_enable = 1;
1369         else
1370                 st->compass_cal_enable = 0;
1371
1372         /* setting up accuracy output */
1373         if (st->sensor[SENSOR_ACCEL].on || st->sensor[SENSOR_NINEQ].on ||
1374             st->sensor[SENSOR_SIXQ].on)
1375                 st->sensor_accuracy[SENSOR_ACCEL_ACCURACY].on = true;
1376         else
1377                 st->sensor_accuracy[SENSOR_ACCEL_ACCURACY].on = false;
1378
1379         if (st->sensor[SENSOR_CALIB_GYRO].on || st->sensor[SENSOR_NINEQ].on ||
1380             st->sensor[SENSOR_SIXQ].on)
1381                 st->sensor_accuracy[SENSOR_GYRO_ACCURACY].on  = true;
1382         else
1383                 st->sensor_accuracy[SENSOR_GYRO_ACCURACY].on = false;
1384
1385         if (st->sensor[SENSOR_CALIB_COMPASS].on || st->sensor[SENSOR_NINEQ].on)
1386                 st->sensor_accuracy[SENSOR_COMPASS_ACCURACY].on = true;
1387         else
1388                 st->sensor_accuracy[SENSOR_COMPASS_ACCURACY].on = false;
1389
1390         inv_set_master_delay(st);
1391
1392         return 0;
1393 }
1394
1395 /*
1396  *  set_inv_enable() - enable function.
1397  */
1398 int set_inv_enable(struct iio_dev *indio_dev)
1399 {
1400         struct inv_mpu_state *st = iio_priv(indio_dev);
1401         int result;
1402
1403         inv_determine_engine(st);
1404         result = inv_switch_power_in_lp(st, true);
1405         if (result)
1406                 return result;
1407
1408         result = inv_stop_dmp(st);
1409         if (result)
1410                 return result;
1411         result = inv_set_rate(st);
1412         if (result) {
1413                 pr_err("inv_set_rate error\n");
1414                 return result;
1415         }
1416         if (st->chip_config.dmp_on) {
1417                 result = inv_setup_dmp(st);
1418                 if (result) {
1419                         pr_err("setup dmp error\n");
1420                         return result;
1421                 }
1422         }
1423         result = inv_turn_on_engine(st);
1424         if (result) {
1425                 pr_err("inv_turn_on_engine error\n");
1426                 return result;
1427         }
1428         result = inv_set_fifo_size(st);
1429         if (result) {
1430                 pr_err("inv_set_fifo_size error\n");
1431                 return result;
1432         }
1433         result = inv_set_fake_secondary(st);
1434         if (result)
1435                 return result;
1436
1437         result = inv_reset_fifo(indio_dev, false);
1438         if (result)
1439                 return result;
1440         result = inv_switch_power_in_lp(st, false);
1441
1442         if ((!st->chip_config.gyro_enable) &&
1443                 (!st->chip_config.accel_enable) &&
1444                 (!st->chip_config.slave_enable) &&
1445                 (!st->chip_config.pressure_enable)) {
1446                 inv_set_power(st, false);
1447                 return 0;
1448         }
1449
1450         return result;
1451 }
1452
1453 void inv_init_sensor_struct(struct inv_mpu_state *st)
1454 {
1455         int i;
1456
1457         for (i = 0; i < SENSOR_NUM_MAX; i++)
1458                 st->sensor[i].rate = MPU_INIT_SENSOR_RATE;
1459
1460         st->sensor[SENSOR_ACCEL].sample_size         = ACCEL_DATA_SZ;
1461         st->sensor[SENSOR_GYRO].sample_size          = GYRO_DATA_SZ;
1462         st->sensor[SENSOR_COMPASS].sample_size       = CPASS_DATA_SZ;
1463         st->sensor[SENSOR_ALS].sample_size           = ALS_DATA_SZ;
1464         st->sensor[SENSOR_PRESSURE].sample_size      = PRESSURE_DATA_SZ;
1465         st->sensor[SENSOR_SIXQ].sample_size          = QUAT6_DATA_SZ;
1466         st->sensor[SENSOR_PEDQ].sample_size          = PQUAT6_DATA_SZ;
1467         st->sensor[SENSOR_CALIB_GYRO].sample_size    = GYRO_CALIBR_DATA_SZ;
1468         st->sensor[SENSOR_CALIB_COMPASS].sample_size = CPASS_CALIBR_DATA_SZ;
1469         st->sensor[SENSOR_NINEQ].sample_size         = QUAT9_DATA_SZ;
1470         st->sensor[SENSOR_GEOMAG].sample_size        = QUAT9_DATA_SZ;
1471
1472         st->sensor[SENSOR_ACCEL].odr_addr         = ODR_ACCEL;
1473         st->sensor[SENSOR_GYRO].odr_addr          = ODR_GYRO;
1474         st->sensor[SENSOR_COMPASS].odr_addr       = ODR_CPASS;
1475         st->sensor[SENSOR_ALS].odr_addr           = ODR_ALS;
1476         st->sensor[SENSOR_PRESSURE].odr_addr      = ODR_PRESSURE;
1477         st->sensor[SENSOR_SIXQ].odr_addr          = ODR_QUAT6;
1478         st->sensor[SENSOR_PEDQ].odr_addr          = ODR_PQUAT6;
1479         st->sensor[SENSOR_CALIB_GYRO].odr_addr    = ODR_GYRO_CALIBR;
1480         st->sensor[SENSOR_CALIB_COMPASS].odr_addr = ODR_CPASS_CALIBR;
1481         st->sensor[SENSOR_NINEQ].odr_addr         = ODR_QUAT9;
1482         st->sensor[SENSOR_GEOMAG].odr_addr         = ODR_QUAT9;
1483
1484         st->sensor[SENSOR_ACCEL].counter_addr         = ODR_CNTR_ACCEL;
1485         st->sensor[SENSOR_GYRO].counter_addr          = ODR_CNTR_GYRO;
1486         st->sensor[SENSOR_COMPASS].counter_addr       = ODR_CNTR_CPASS;
1487         st->sensor[SENSOR_ALS].counter_addr           = ODR_CNTR_ALS;
1488         st->sensor[SENSOR_PRESSURE].counter_addr      = ODR_CNTR_PRESSURE;
1489         st->sensor[SENSOR_SIXQ].counter_addr          = ODR_CNTR_QUAT6;
1490         st->sensor[SENSOR_PEDQ].counter_addr          = ODR_CNTR_PQUAT6;
1491         st->sensor[SENSOR_CALIB_GYRO].counter_addr    = ODR_CNTR_GYRO_CALIBR;
1492         st->sensor[SENSOR_CALIB_COMPASS].counter_addr = ODR_CNTR_CPASS_CALIBR;
1493         st->sensor[SENSOR_NINEQ].counter_addr         = ODR_CNTR_QUAT9;
1494         st->sensor[SENSOR_GEOMAG].counter_addr        = ODR_CNTR_QUAT9;
1495
1496         st->sensor[SENSOR_ACCEL].output         = ACCEL_SET;
1497         st->sensor[SENSOR_GYRO].output          = GYRO_SET;
1498         st->sensor[SENSOR_COMPASS].output       = CPASS_SET;
1499         st->sensor[SENSOR_ALS].output           = ALS_SET;
1500         st->sensor[SENSOR_PRESSURE].output      = PRESSURE_SET;
1501         st->sensor[SENSOR_SIXQ].output          = QUAT6_SET;
1502         st->sensor[SENSOR_PEDQ].output          = PQUAT6_SET;
1503         st->sensor[SENSOR_CALIB_GYRO].output    = GYRO_CALIBR_SET;
1504         st->sensor[SENSOR_CALIB_COMPASS].output = CPASS_CALIBR_SET;
1505         st->sensor[SENSOR_NINEQ].output         = QUAT9_SET;
1506         st->sensor[SENSOR_GEOMAG].output        = QUAT9_SET;
1507
1508         st->sensor[SENSOR_ACCEL].header         = ACCEL_HDR;
1509         st->sensor[SENSOR_GYRO].header          = GYRO_HDR;
1510         st->sensor[SENSOR_COMPASS].header       = COMPASS_HDR;
1511         st->sensor[SENSOR_ALS].header           = ALS_HDR;
1512         st->sensor[SENSOR_PRESSURE].header      = PRESSURE_HDR;
1513         st->sensor[SENSOR_SIXQ].header          = SIXQUAT_HDR;
1514         st->sensor[SENSOR_PEDQ].header          = PEDQUAT_HDR;
1515         st->sensor[SENSOR_CALIB_GYRO].header    = GYRO_CALIB_HDR;
1516         st->sensor[SENSOR_CALIB_COMPASS].header = COMPASS_CALIB_HDR;
1517         st->sensor[SENSOR_NINEQ].header         = NINEQUAT_HDR;
1518         st->sensor[SENSOR_GEOMAG].header         = NINEQUAT_HDR;
1519
1520         st->sensor[SENSOR_ACCEL].a_en           = true;
1521         st->sensor[SENSOR_GYRO].a_en            = false;
1522         st->sensor[SENSOR_COMPASS].a_en         = false;
1523         st->sensor[SENSOR_ALS].a_en             = false;
1524         st->sensor[SENSOR_PRESSURE].a_en        = false;
1525         st->sensor[SENSOR_SIXQ].a_en            = true;
1526         st->sensor[SENSOR_PEDQ].a_en            = true;
1527         st->sensor[SENSOR_CALIB_GYRO].a_en      = false;
1528         st->sensor[SENSOR_CALIB_COMPASS].a_en   = false;
1529         st->sensor[SENSOR_NINEQ].a_en           = true;
1530         st->sensor[SENSOR_GEOMAG].a_en           = true;
1531
1532         st->sensor[SENSOR_ACCEL].g_en         = false;
1533         st->sensor[SENSOR_GYRO].g_en          = true;
1534         st->sensor[SENSOR_COMPASS].g_en       = false;
1535         st->sensor[SENSOR_ALS].g_en           = false;
1536         st->sensor[SENSOR_PRESSURE].g_en      = false;
1537         st->sensor[SENSOR_SIXQ].g_en          = true;
1538         st->sensor[SENSOR_PEDQ].g_en          = true;
1539         st->sensor[SENSOR_CALIB_GYRO].g_en    = true;
1540         st->sensor[SENSOR_CALIB_COMPASS].g_en = false;
1541         st->sensor[SENSOR_NINEQ].g_en         = true;
1542         st->sensor[SENSOR_GEOMAG].g_en        = false;
1543
1544         st->sensor[SENSOR_ACCEL].c_en         = false;
1545         st->sensor[SENSOR_GYRO].c_en          = false;
1546         st->sensor[SENSOR_COMPASS].c_en       = true;
1547         st->sensor[SENSOR_ALS].c_en           = false;
1548         st->sensor[SENSOR_PRESSURE].c_en      = false;
1549         st->sensor[SENSOR_SIXQ].c_en          = false;
1550         st->sensor[SENSOR_PEDQ].c_en          = false;
1551         st->sensor[SENSOR_CALIB_GYRO].c_en    = false;
1552         st->sensor[SENSOR_CALIB_COMPASS].c_en = true;
1553         st->sensor[SENSOR_NINEQ].c_en         = true;
1554         st->sensor[SENSOR_GEOMAG].c_en        = true;
1555
1556         st->sensor[SENSOR_ACCEL].p_en         = false;
1557         st->sensor[SENSOR_GYRO].p_en          = false;
1558         st->sensor[SENSOR_COMPASS].p_en       = false;
1559         st->sensor[SENSOR_ALS].p_en           = false;
1560         st->sensor[SENSOR_PRESSURE].p_en      = true;
1561         st->sensor[SENSOR_SIXQ].p_en          = false;
1562         st->sensor[SENSOR_PEDQ].p_en          = false;
1563         st->sensor[SENSOR_CALIB_GYRO].p_en    = false;
1564         st->sensor[SENSOR_CALIB_COMPASS].p_en = false;
1565         st->sensor[SENSOR_NINEQ].p_en         = false;
1566         st->sensor[SENSOR_GEOMAG].p_en        = false;
1567
1568         st->sensor[SENSOR_ACCEL].engine_base         = ENGINE_ACCEL;
1569         st->sensor[SENSOR_GYRO].engine_base          = ENGINE_GYRO;
1570         st->sensor[SENSOR_COMPASS].engine_base       = ENGINE_I2C;
1571         st->sensor[SENSOR_ALS].engine_base           = ENGINE_I2C;
1572         st->sensor[SENSOR_PRESSURE].engine_base      = ENGINE_I2C;
1573         st->sensor[SENSOR_SIXQ].engine_base          = ENGINE_GYRO;
1574         st->sensor[SENSOR_PEDQ].engine_base          = ENGINE_GYRO;
1575         st->sensor[SENSOR_CALIB_GYRO].engine_base    = ENGINE_GYRO;
1576         st->sensor[SENSOR_CALIB_COMPASS].engine_base = ENGINE_I2C;
1577         st->sensor[SENSOR_NINEQ].engine_base         = ENGINE_GYRO;
1578         st->sensor[SENSOR_GEOMAG].engine_base        = ENGINE_ACCEL;
1579
1580         st->sensor_accuracy[SENSOR_ACCEL_ACCURACY].sample_size =
1581                                                         ACCEL_ACCURACY_SZ;
1582         st->sensor_accuracy[SENSOR_GYRO_ACCURACY].sample_size  =
1583                                                         GYRO_ACCURACY_SZ;
1584         st->sensor_accuracy[SENSOR_COMPASS_ACCURACY].sample_size =
1585                                                         CPASS_ACCURACY_SZ;
1586
1587         st->sensor_accuracy[SENSOR_ACCEL_ACCURACY].output =
1588                                                         ACCEL_ACCURACY_SET;
1589         st->sensor_accuracy[SENSOR_GYRO_ACCURACY].output =
1590                                                         GYRO_ACCURACY_SET;
1591         st->sensor_accuracy[SENSOR_COMPASS_ACCURACY].output =
1592                                                         CPASS_ACCURACY_SET;
1593
1594         st->sensor_accuracy[SENSOR_ACCEL_ACCURACY].header =
1595                                                         ACCEL_ACCURACY_HDR;
1596         st->sensor_accuracy[SENSOR_GYRO_ACCURACY].header =
1597                                                         GYRO_ACCURACY_HDR;
1598         st->sensor_accuracy[SENSOR_COMPASS_ACCURACY].header =
1599                                                         CPASS_ACCURACY_HDR;
1600
1601 }