drivers/staging/iio/imu/inv_mpu/inv_mpu_ring.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 */
  14
  15 /**
  16  *  @addtogroup  DRIVERS
  17  *  @brief       Hardware drivers.
  18  *
  19  *  @{
  20  *      @file    inv_mpu_ring.c
  21  *      @brief   A sysfs device driver for Invensense gyroscopes.
  22  *      @details This file is part of inv mpu iio driver code
  23  */
  24
  25 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
  26
  27 #include <linux/module.h>
  28 #include <linux/init.h>
  29 #include <linux/slab.h>
  30 #include <linux/i2c.h>
  31 #include <linux/err.h>
  32 #include <linux/delay.h>
  33 #include <linux/sysfs.h>
  34 #include <linux/jiffies.h>
  35 #include <linux/irq.h>
  36 #include <linux/interrupt.h>
  37 #include <linux/kfifo.h>
  38 #include <linux/poll.h>
  39 #include <linux/miscdevice.h>
  40 #ifdef INV_KERNEL_3_10
  41 #include <linux/iio/iio.h>
  42 #include <linux/iio/kfifo_buf.h>
  43 #include <linux/iio/trigger_consumer.h>
  44 #include <linux/iio/sysfs.h>
  45 #else
  46 #include "iio.h"
  47 #include "kfifo_buf.h"
  48 #include "trigger_consumer.h"
  49 #include "sysfs.h"
  50 #endif
  51
  52 #include "inv_mpu_iio.h"
  53
  54 /**
  55  *  reset_fifo_mpu3050() - Reset FIFO related registers
  56  *  @st:        Device driver instance.
  57  */
  58 static int reset_fifo_mpu3050(struct iio_dev *indio_dev)
  59 {
  60         struct inv_reg_map_s *reg;
  61         int result;
  62         u8 val, user_ctrl;
  63         struct inv_mpu_iio_s  *st = iio_priv(indio_dev);
  64         reg = &st->reg;
  65
  66         /* disable interrupt */
  67         result = inv_plat_single_write(st, reg->int_enable,
  68                                 st->plat_data.int_config);
  69         if (result)
  70                 return result;
  71         /* disable the sensor output to FIFO */
  72         result = inv_plat_single_write(st, reg->fifo_en, 0);
  73         if (result)
  74                 goto reset_fifo_fail;
  75         result = inv_plat_read(st, reg->user_ctrl, 1, &user_ctrl);
  76         if (result)
  77                 goto reset_fifo_fail;
  78         /* disable fifo reading */
  79         user_ctrl &= ~BIT_FIFO_EN;
  80         st->chip_config.has_footer = 0;
  81         /* reset fifo */
  82         val = (BIT_3050_FIFO_RST | user_ctrl);
  83         result = inv_plat_single_write(st, reg->user_ctrl, val | st->i2c_dis);
  84         if (result)
  85                 goto reset_fifo_fail;
  86         st->last_isr_time = get_time_ns();
  87         if (st->chip_config.dmp_on) {
  88                 /* enable interrupt when DMP is done */
  89                 result = inv_plat_single_write(st, reg->int_enable,
  90                                 st->plat_data.int_config | BIT_DMP_INT_EN);
  91                 if (result)
  92                         return result;
  93
  94                 result = inv_plat_single_write(st, reg->user_ctrl,
  95                         BIT_FIFO_EN | user_ctrl | st->i2c_dis);
  96                 if (result)
  97                         return result;
  98         } else {
  99                 /* enable interrupt */
 100                 if (st->chip_config.accl_fifo_enable ||
 101                     st->chip_config.gyro_fifo_enable) {
 102                         result = inv_plat_single_write(st, reg->int_enable,
 103                                 st->plat_data.int_config | BIT_DATA_RDY_EN);
 104                         if (result)
 105                                 return result;
 106                 }
 107                 /* enable FIFO reading and I2C master interface*/
 108                 result = inv_plat_single_write(st, reg->user_ctrl,
 109                         BIT_FIFO_EN | user_ctrl | st->i2c_dis);
 110                 if (result)
 111                         return result;
 112                 /* enable sensor output to FIFO and FIFO footer*/
 113                 val = 1;
 114                 if (st->chip_config.accl_fifo_enable)
 115                         val |= BITS_3050_ACCL_OUT;
 116                 if (st->chip_config.gyro_fifo_enable)
 117                         val |= BITS_GYRO_OUT;
 118                 result = inv_plat_single_write(st, reg->fifo_en, val);
 119                 if (result)
 120                         return result;
 121         }
 122
 123         return 0;
 124 reset_fifo_fail:
 125         if (st->chip_config.dmp_on)
 126                 val = BIT_DMP_INT_EN;
 127         else
 128                 val = BIT_DATA_RDY_EN;
 129         inv_plat_single_write(st, reg->int_enable,
 130                              st->plat_data.int_config | val);
 131         pr_err("reset fifo failed\n");
 132
 133         return result;
 134 }
 135
 136 /**
 137  *  inv_set_lpf() - set low pass filer based on fifo rate.
 138  */
 139 static int inv_set_lpf(struct inv_mpu_iio_s *st, int rate)
 140 {
 141         const short hz[] = {188, 98, 42, 20, 10, 5};
 142         const int   d[] = {INV_FILTER_188HZ, INV_FILTER_98HZ,
 143                         INV_FILTER_42HZ, INV_FILTER_20HZ,
 144                         INV_FILTER_10HZ, INV_FILTER_5HZ};
 145         int i, h, data, result;
 146         struct inv_reg_map_s *reg;
 147         reg = &st->reg;
 148         h = (rate >> 1);
 149         i = 0;
 150         while ((h < hz[i]) && (i < ARRAY_SIZE(d) - 1))
 151                 i++;
 152         data = d[i];
 153         if (INV_MPU3050 == st->chip_type) {
 154                 if (st->mpu_slave != NULL) {
 155                         result = st->mpu_slave->set_lpf(st, rate);
 156                         if (result)
 157                                 return result;
 158                 }
 159                 result = inv_plat_single_write(st, reg->lpf, data |
 160                         (st->chip_config.fsr << GYRO_CONFIG_FSR_SHIFT));
 161         } else {
 162                 result = inv_plat_single_write(st, reg->lpf, data);
 163         }
 164         if (result)
 165                 return result;
 166         st->chip_config.lpf = data;
 167
 168         return 0;
 169 }
 170
 171 /**
 172  *  set_fifo_rate_reg() - Set fifo rate in hardware register
 173  */
 174 static int set_fifo_rate_reg(struct inv_mpu_iio_s *st)
 175 {
 176         u8 data;
 177         u16 fifo_rate;
 178         int result;
 179         struct inv_reg_map_s *reg;
 180
 181         reg = &st->reg;
 182         fifo_rate = st->chip_config.new_fifo_rate;
 183         data = ONE_K_HZ / fifo_rate - 1;
 184         result = inv_plat_single_write(st, reg->sample_rate_div, data);
 185         if (result)
 186                 return result;
 187         result = inv_set_lpf(st, fifo_rate);
 188         if (result)
 189                 return result;
 190         st->chip_config.fifo_rate = fifo_rate;
 191
 192         return 0;
 193 }
 194
 195 /**
 196  *  inv_lpa_mode() - store current low power mode settings
 197  */
 198 static int inv_lpa_mode(struct inv_mpu_iio_s *st, int lpa_mode)
 199 {
 200         unsigned long result;
 201         u8 d;
 202         struct inv_reg_map_s *reg;
 203
 204         reg = &st->reg;
 205         result = inv_plat_read(st, reg->pwr_mgmt_1, 1, &d);
 206         if (result)
 207                 return result;
 208         if (lpa_mode)
 209                 d |= BIT_CYCLE;
 210         else
 211                 d &= ~BIT_CYCLE;
 212
 213         result = inv_plat_single_write(st, reg->pwr_mgmt_1, d);
 214         if (result)
 215                 return result;
 216         if (INV_MPU6500 == st->chip_type) {
 217                 if (lpa_mode)
 218                         d = BIT_ACCEL_FCHOCIE_B;
 219                 else
 220                         d = 0;
 221                 result = inv_plat_single_write(st, REG_6500_ACCEL_CONFIG2, d);
 222                 if (result)
 223                         return result;
 224         }
 225
 226         return 0;
 227 }
 228
 229 /**
 230  *  reset_fifo_itg() - Reset FIFO related registers.
 231  *  @st:        Device driver instance.
 232  */
 233 static int reset_fifo_itg(struct iio_dev *indio_dev)
 234 {
 235         struct inv_reg_map_s *reg;
 236         int result, data;
 237         u8 val, int_word;
 238         struct inv_mpu_iio_s  *st = iio_priv(indio_dev);
 239         reg = &st->reg;
 240
 241         if (st->chip_config.lpa_mode) {
 242                 result = inv_lpa_mode(st, 0);
 243                 if (result) {
 244                         pr_err("reset lpa mode failed\n");
 245                         return result;
 246                 }
 247         }
 248         /* disable interrupt */
 249         result = inv_plat_single_write(st, reg->int_enable, 0);
 250         if (result) {
 251                 pr_err("int_enable write failed\n");
 252                 return result;
 253         }
 254         /* disable the sensor output to FIFO */
 255         result = inv_plat_single_write(st, reg->fifo_en, 0);
 256         if (result)
 257                 goto reset_fifo_fail;
 258         /* disable fifo reading */
 259         result = inv_plat_single_write(st, reg->user_ctrl, st->i2c_dis);
 260         if (result)
 261                 goto reset_fifo_fail;
 262         int_word = 0;
 263
 264         /* MPU6500's BIT_6500_WOM_EN is the same as BIT_MOT_EN */
 265         if (st->mot_int.mot_on)
 266                 int_word |= BIT_MOT_EN;
 267
 268         if (st->chip_config.dmp_on) {
 269                 val = (BIT_FIFO_RST | BIT_DMP_RST);
 270                 result = inv_plat_single_write(st, reg->user_ctrl, val | st->i2c_dis);
 271                 if (result)
 272                         goto reset_fifo_fail;
 273                 st->last_isr_time = get_time_ns();
 274                 if (st->chip_config.dmp_int_on) {
 275                         int_word |= BIT_DMP_INT_EN;
 276                         result = inv_plat_single_write(st, reg->int_enable,
 277                                                         int_word);
 278                         if (result)
 279                                 return result;
 280                 }
 281                 val = (BIT_DMP_EN | BIT_FIFO_EN);
 282                 if (st->chip_config.compass_enable &
 283                         (!st->chip_config.dmp_event_int_on))
 284                         val |= BIT_I2C_MST_EN;
 285                 result = inv_plat_single_write(st, reg->user_ctrl, val | st->i2c_dis);
 286                 if (result)
 287                         goto reset_fifo_fail;
 288
 289                 if (st->chip_config.compass_enable) {
 290                         /* I2C_MST_DLY is set according to sample rate,
 291                            slow down the power*/
 292                         data = max(COMPASS_RATE_SCALE *
 293                                 st->chip_config.new_fifo_rate / ONE_K_HZ,
 294                                 st->chip_config.new_fifo_rate /
 295                                 st->chip_config.dmp_output_rate);
 296                         if (data > 0)
 297                                 data -= 1;
 298                         result = inv_plat_single_write(st, REG_I2C_SLV4_CTRL,
 299                                                         data);
 300                         if (result)
 301                                 return result;
 302                 }
 303                 val = 0;
 304                 if (st->chip_config.accl_fifo_enable)
 305                         val |= INV_ACCL_MASK;
 306                 if (st->chip_config.gyro_fifo_enable)
 307                         val |= INV_GYRO_MASK;
 308                 result = inv_send_sensor_data(st, val);
 309                 if (result)
 310                         return result;
 311                 if (st->chip_config.display_orient_on || st->chip_config.tap_on)
 312                         result = inv_send_interrupt_word(st, true);
 313                 else
 314                         result = inv_send_interrupt_word(st, false);
 315         } else {
 316                 /* reset FIFO and possibly reset I2C*/
 317                 val = BIT_FIFO_RST;
 318                 result = inv_plat_single_write(st, reg->user_ctrl, val | st->i2c_dis);
 319                 if (result)
 320                         goto reset_fifo_fail;
 321                 st->last_isr_time = get_time_ns();
 322                 /* enable interrupt */
 323                 if (st->chip_config.accl_fifo_enable ||
 324                     st->chip_config.gyro_fifo_enable ||
 325                     st->chip_config.compass_enable) {
 326                         int_word |= BIT_DATA_RDY_EN;
 327                 }
 328                 result = inv_plat_single_write(st, reg->int_enable, int_word);
 329                 if (result)
 330                         return result;
 331                 /* enable FIFO reading and I2C master interface*/
 332                 val = BIT_FIFO_EN;
 333                 if (st->chip_config.compass_enable)
 334                         val |= BIT_I2C_MST_EN;
 335                 result = inv_plat_single_write(st, reg->user_ctrl, val | st->i2c_dis);
 336                 if (result)
 337                         goto reset_fifo_fail;
 338                 if (st->chip_config.compass_enable) {
 339                         /* I2C_MST_DLY is set according to sample rate,
 340                            slow down the power*/
 341                         data = COMPASS_RATE_SCALE *
 342                                 st->chip_config.new_fifo_rate / ONE_K_HZ;
 343                         if (data > 0)
 344                                 data -= 1;
 345                         result = inv_plat_single_write(st, REG_I2C_SLV4_CTRL,
 346                                                         data);
 347                         if (result)
 348                                 return result;
 349                 }
 350                 /* enable sensor output to FIFO */
 351                 val = 0;
 352                 if (st->chip_config.gyro_fifo_enable)
 353                         val |= BITS_GYRO_OUT;
 354                 if (st->chip_config.accl_fifo_enable)
 355                         val |= BIT_ACCEL_OUT;
 356                 result = inv_plat_single_write(st, reg->fifo_en, val);
 357                 if (result)
 358                         goto reset_fifo_fail;
 359         }
 360         st->chip_config.normal_compass_measure = 0;
 361         result = inv_lpa_mode(st, st->chip_config.lpa_mode);
 362         if (result)
 363                 goto reset_fifo_fail;
 364
 365         return 0;
 366
 367 reset_fifo_fail:
 368         if (st->chip_config.dmp_on)
 369                 val = BIT_DMP_INT_EN;
 370         else
 371                 val = BIT_DATA_RDY_EN;
 372         inv_plat_single_write(st, reg->int_enable, val);
 373         pr_err("reset fifo failed\n");
 374
 375         return result;
 376 }
 377
 378 /**
 379  *  inv_clear_kfifo() - clear time stamp fifo
 380  *  @st:        Device driver instance.
 381  */
 382 static void inv_clear_kfifo(struct inv_mpu_iio_s *st)
 383 {
 384         unsigned long flags;
 385
 386         spin_lock_irqsave(&st->time_stamp_lock, flags);
 387         kfifo_reset(&st->timestamps);
 388         spin_unlock_irqrestore(&st->time_stamp_lock, flags);
 389 }
 390
 391 /**
 392  *  inv_reset_fifo() - Reset FIFO related registers.
 393  *  @st:        Device driver instance.
 394  */
 395 static int inv_reset_fifo(struct iio_dev *indio_dev)
 396 {
 397         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
 398
 399         inv_clear_kfifo(st);
 400         if (INV_MPU3050 == st->chip_type)
 401                 return reset_fifo_mpu3050(indio_dev);
 402         else
 403                 return reset_fifo_itg(indio_dev);
 404 }
 405
 406 static int inv_set_dmp_sysfs(struct inv_mpu_iio_s *st)
 407 {
 408         int result;
 409
 410         result = inv_set_fifo_rate(st, st->chip_config.dmp_output_rate);
 411         if (result)
 412                 return result;
 413         result = inv_set_interrupt_on_gesture_event(st,
 414                                 st->chip_config.dmp_event_int_on);
 415
 416         return result;
 417 }
 418
 419 /**
 420  *  set_inv_enable() - Reset FIFO related registers.
 421  *                      This also powers on the chip if needed.
 422  *  @st:        Device driver instance.
 423  *  @fifo_enable: enable/disable
 424  */
 425 static int set_inv_enable(struct iio_dev *indio_dev,
 426                         bool enable) {
 427         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
 428         struct inv_reg_map_s *reg;
 429         int result;
 430
 431         reg = &st->reg;
 432         if (enable) {
 433                 if (st->chip_config.new_fifo_rate !=
 434                                 st->chip_config.fifo_rate) {
 435                         result = set_fifo_rate_reg(st);
 436                         if (result)
 437                                 return result;
 438                 }
 439                 if (st->chip_config.dmp_on) {
 440                         result = inv_set_dmp_sysfs(st);
 441                         if (result)
 442                                 return result;
 443                 }
 444
 445                 if (st->chip_config.gyro_enable) {
 446                         result = st->switch_gyro_engine(st, true);
 447                         if (result)
 448                                 return result;
 449                 }
 450                 if (st->chip_config.accl_enable) {
 451                         result = st->switch_accl_engine(st, true);
 452                         if (result)
 453                                 return result;
 454                 }
 455
 456                 result = inv_reset_fifo(indio_dev);
 457                 if (result)
 458                         return result;
 459         } else {
 460                 if ((INV_MPU3050 != st->chip_type)
 461                         && st->chip_config.lpa_mode) {
 462                         /* if the chip is in low power mode,
 463                                 register write/read could fail */
 464                         result = inv_lpa_mode(st, 0);
 465                         if (result)
 466                                 return result;
 467                 }
 468                 result = inv_plat_single_write(st, reg->fifo_en, 0);
 469                 if (result)
 470                         return result;
 471                 /* disable fifo reading */
 472                 if (INV_MPU3050 != st->chip_type) {
 473                         result = inv_plat_single_write(st, reg->int_enable, 0);
 474                         if (result)
 475                                 return result;
 476                         result = inv_plat_single_write(st, reg->user_ctrl, st->i2c_dis);
 477                 } else {
 478                         result = inv_plat_single_write(st, reg->int_enable,
 479                                 st->plat_data.int_config);
 480                 }
 481                 if (result)
 482                         return result;
 483                 /* turn off the gyro/accl engine during disable phase */
 484                 result = st->switch_gyro_engine(st, false);
 485                 if (result)
 486                         return result;
 487                 result = st->switch_accl_engine(st, false);
 488                 if (result)
 489                         return result;
 490                 result = st->set_power_state(st, false);
 491                 if (result)
 492                         return result;
 493         }
 494         st->chip_config.enable = enable;
 495
 496         return 0;
 497 }
 498
 499 /* #define _RATE_DEBUG */
 500
 501 #ifdef _RATE_DEBUG
 502 static s64 tm_irq_min = -1;
 503 static s64 tm_irq_max = -1;
 504 static s64 tm_irq_sum = -1;
 505 static s64 tm_irq_last_print = -1;
 506 static s64 tm_irq_count = -1;
 507 #endif
 508
 509 /**
 510  *  inv_irq_handler() - Cache a timestamp at each data ready interrupt.
 511  */
 512 static irqreturn_t inv_irq_handler(int irq, void *dev_id)
 513 {
 514         struct inv_mpu_iio_s *st;
 515         u64 timestamp;
 516         int catch_up;
 517         u64 time_since_last_irq;
 518
 519         st = (struct inv_mpu_iio_s *)dev_id;
 520         timestamp = get_time_ns();
 521         time_since_last_irq = timestamp - st->last_isr_time;
 522         spin_lock(&st->time_stamp_lock);
 523
 524 #ifdef _RATE_DEBUG
 525         if (tm_irq_min <= 0 && tm_irq_max <= 0) {
 526                 tm_irq_min = time_since_last_irq;
 527                 tm_irq_max = time_since_last_irq;
 528                 tm_irq_sum = 0;
 529                 tm_irq_count = 0;
 530         } else if (time_since_last_irq < tm_irq_min) {
 531                 tm_irq_min = time_since_last_irq;
 532         } else if (time_since_last_irq > tm_irq_max) {
 533                 tm_irq_max = time_since_last_irq;
 534         }
 535         tm_irq_sum += time_since_last_irq;
 536         tm_irq_count++;
 537
 538         if (unlikely((timestamp - tm_irq_last_print) >= 1000000000)) {
 539                 pr_info("MPU Interrupt: %lld,%d,%lld\n", tm_irq_min, (u32)tm_irq_sum / (u32)tm_irq_count, tm_irq_max);
 540                 tm_irq_last_print = timestamp;
 541                 tm_irq_min = 0;
 542                 tm_irq_max = 0;
 543                 tm_irq_count = 0;
 544                 tm_irq_sum = 0;
 545         }
 546 #endif
 547
 548         catch_up = 0;
 549         while ((time_since_last_irq > st->irq_dur_ns * 2) &&
 550                (catch_up < MAX_CATCH_UP) &&
 551                (!st->chip_config.lpa_mode) &&
 552                (!st->chip_config.dmp_on)) {
 553                 st->last_isr_time += st->irq_dur_ns;
 554                 kfifo_in(&st->timestamps,
 555                          &st->last_isr_time, 1);
 556                 time_since_last_irq = timestamp - st->last_isr_time;
 557                 catch_up++;
 558         }
 559         kfifo_in(&st->timestamps, &timestamp, 1);
 560         st->last_isr_time = timestamp;
 561         spin_unlock(&st->time_stamp_lock);
 562
 563         return IRQ_WAKE_THREAD;
 564 }
 565
 566 static int put_scan_to_buf(struct iio_dev *indio_dev, u8 *d,
 567                                 short *s, int scan_index, int d_ind) {
 568         struct iio_buffer *ring = indio_dev->buffer;
 569         int st;
 570         int i;
 571         for (i = 0; i < 3; i++) {
 572                 st = iio_scan_mask_query(indio_dev, ring, scan_index + i);
 573                 if (st) {
 574                         memcpy(&d[d_ind], &s[i], sizeof(s[i]));
 575                         d_ind += sizeof(s[i]);
 576                 }
 577         }
 578         return d_ind;
 579 }
 580
 581 static void inv_report_data_3050(struct iio_dev *indio_dev, s64 t,
 582                         int has_footer, u8 *data)
 583 {
 584         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
 585         struct iio_buffer *ring = indio_dev->buffer;
 586         int ind, i, d_ind;
 587         struct inv_chip_config_s *conf;
 588         short g[THREE_AXIS], a[THREE_AXIS];
 589         s64 buf[8];
 590         u8 *tmp;
 591         int bytes_per_datum, scan_count;
 592         conf = &st->chip_config;
 593
 594         scan_count = bitmap_weight(indio_dev->active_scan_mask,
 595                                        indio_dev->masklength);
 596         bytes_per_datum = scan_count * 2;
 597
 598         ind = 0;
 599         if (has_footer)
 600                 ind += 2;
 601         tmp = (u8 *)buf;
 602         d_ind = 0;
 603
 604         if (conf->gyro_fifo_enable) {
 605                 for (i = 0; i < ARRAY_SIZE(g); i++) {
 606                         g[i] = be16_to_cpup((__be16 *)(&data[ind + i * 2]));
 607                         st->raw_gyro[i] = g[i];
 608                 }
 609                 ind += BYTES_PER_SENSOR;
 610         }
 611         if (conf->accl_fifo_enable) {
 612                 st->mpu_slave->combine_data(&data[ind], a);
 613                 for (i = 0; i < ARRAY_SIZE(a); i++)
 614                         st->raw_accel[i] = a[i];
 615
 616                 ind += BYTES_PER_SENSOR;
 617                 d_ind = put_scan_to_buf(indio_dev, tmp, a,
 618                         INV_MPU_SCAN_ACCL_X, d_ind);
 619         }
 620         if (conf->gyro_fifo_enable)
 621                 d_ind = put_scan_to_buf(indio_dev, tmp, g,
 622                         INV_MPU_SCAN_GYRO_X, d_ind);
 623
 624         i = (bytes_per_datum + 7) / 8;
 625         if (ring->scan_timestamp)
 626                 buf[i] = t;
 627 #ifdef INV_KERNEL_3_10
 628         ring->access->store_to(indio_dev->buffer, (u8 *)buf);
 629 #else
 630         ring->access->store_to(indio_dev->buffer, (u8 *)buf, t);
 631 #endif
 632 }
 633
 634 /**
 635  *  inv_read_fifo_mpu3050() - Transfer data from FIFO to ring buffer for
 636  *                            mpu3050.
 637  */
 638 irqreturn_t inv_read_fifo_mpu3050(int irq, void *dev_id)
 639 {
 640
 641         struct inv_mpu_iio_s *st = (struct inv_mpu_iio_s *)dev_id;
 642         struct iio_dev *indio_dev = iio_priv_to_dev(st);
 643         int bytes_per_datum;
 644         u8 data[64];
 645         int result;
 646         short fifo_count, byte_read;
 647         u32 copied;
 648         s64 timestamp;
 649         struct inv_reg_map_s *reg;
 650         reg = &st->reg;
 651         /* It is impossible that chip is asleep or enable is
 652         zero when interrupt is on
 653         *  because interrupt is now connected with enable */
 654         if (st->chip_config.dmp_on)
 655                 bytes_per_datum = BYTES_FOR_DMP;
 656         else
 657                 bytes_per_datum = (st->chip_config.accl_fifo_enable +
 658                         st->chip_config.gyro_fifo_enable)*BYTES_PER_SENSOR;
 659         if (st->chip_config.has_footer)
 660                 byte_read = bytes_per_datum + MPU3050_FOOTER_SIZE;
 661         else
 662                 byte_read = bytes_per_datum;
 663
 664         fifo_count = 0;
 665         if (byte_read != 0) {
 666                 result = inv_plat_read(st, reg->fifo_count_h,
 667                                 FIFO_COUNT_BYTE, data);
 668                 if (result)
 669                         goto end_session;
 670                 fifo_count = be16_to_cpup((__be16 *)(&data[0]));
 671                 if (fifo_count < byte_read)
 672                         goto end_session;
 673                 if (fifo_count & 1)
 674                         goto flush_fifo;
 675                 if (fifo_count > FIFO_THRESHOLD)
 676                         goto flush_fifo;
 677                 /* Timestamp mismatch. */
 678                 if (kfifo_len(&st->timestamps) <
 679                         fifo_count / byte_read)
 680                         goto flush_fifo;
 681                 if (kfifo_len(&st->timestamps) >
 682                         fifo_count / byte_read + TIME_STAMP_TOR) {
 683                         if (st->chip_config.dmp_on) {
 684                                 result = kfifo_to_user(&st->timestamps,
 685                                 &timestamp, sizeof(timestamp), &copied);
 686                                 if (result)
 687                                         goto flush_fifo;
 688                         } else {
 689                                 goto flush_fifo;
 690                         }
 691                 }
 692         }
 693         while ((bytes_per_datum != 0) && (fifo_count >= byte_read)) {
 694                 result = inv_plat_read(st, reg->fifo_r_w, byte_read, data);
 695                 if (result)
 696                         goto flush_fifo;
 697
 698                 result = kfifo_to_user(&st->timestamps,
 699                         &timestamp, sizeof(timestamp), &copied);
 700                 if (result)
 701                         goto flush_fifo;
 702                 inv_report_data_3050(indio_dev, timestamp,
 703                                      st->chip_config.has_footer, data);
 704                 fifo_count -= byte_read;
 705                 if (st->chip_config.has_footer == 0) {
 706                         st->chip_config.has_footer = 1;
 707                         byte_read = bytes_per_datum + MPU3050_FOOTER_SIZE;
 708                 }
 709         }
 710
 711 end_session:
 712         return IRQ_HANDLED;
 713
 714 flush_fifo:
 715         /* Flush HW and SW FIFOs. */
 716         inv_reset_fifo(indio_dev);
 717         inv_clear_kfifo(st);
 718         return IRQ_HANDLED;
 719 }
 720
 721 #ifdef _RATE_DEBUG
 722 static s64 tm_min = -1;
 723 static s64 tm_max = -1;
 724 static s64 tm_sum = -1;
 725 static s64 tm_last_print = -1;
 726 static s64 tm_count = -1;
 727 #endif
 728
 729 static int inv_report_gyro_accl_compass(struct iio_dev *indio_dev,
 730                                         u8 *data, s64 t)
 731 {
 732         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
 733         short g[THREE_AXIS], a[THREE_AXIS], c[THREE_AXIS];
 734         int q[4];
 735         int result, ind;
 736         u32 word;
 737         u8 d[8], compass_divider;
 738         u8 buf[64];
 739         u64 *tmp;
 740         int source, i;
 741         struct inv_chip_config_s *conf;
 742
 743         conf = &st->chip_config;
 744         ind = 0;
 745
 746         if (conf->quaternion_on & conf->dmp_on) {
 747                 for (i = 0; i < ARRAY_SIZE(q); i++) {
 748                         q[i] = be32_to_cpup((__be32 *)(&data[ind + i * 4]));
 749                         st->raw_quaternion[i] = q[i];
 750                         memcpy(&buf[ind + i * sizeof(q[i])], &q[i],
 751                                                 sizeof(q[i]));
 752                 }
 753                 ind += QUATERNION_BYTES;
 754         }
 755
 756         if (conf->accl_fifo_enable) {
 757                 for (i = 0; i < ARRAY_SIZE(a); i++) {
 758                         a[i] = be16_to_cpup((__be16 *)(&data[ind + i * 2]));
 759                         memcpy(&buf[ind + i * sizeof(a[i])], &a[i],
 760                                                 sizeof(a[i]));
 761                 }
 762                 ind += BYTES_PER_SENSOR;
 763         }
 764
 765         if (conf->gyro_fifo_enable) {
 766                 for (i = 0; i < ARRAY_SIZE(g); i++) {
 767                         g[i] = be16_to_cpup((__be16 *)(&data[ind + i * 2]));
 768                         memcpy(&buf[ind + i * sizeof(g[i])], &g[i],
 769                                                 sizeof(g[i]));
 770                 }
 771                 ind += BYTES_PER_SENSOR;
 772         }
 773
 774         if (conf->dmp_on && (conf->tap_on || conf->display_orient_on)) {
 775                 word = (u32)(be32_to_cpup((u32 *)&data[ind]));
 776                 source = ((word >> 16) & 0xff);
 777                 if (source) {
 778                         st->tap_data = (DMP_MASK_TAP & (word & 0xff));
 779                         st->display_orient_data =
 780                         ((DMP_MASK_DIS_ORIEN & (word & 0xff)) >>
 781                           DMP_DIS_ORIEN_SHIFT);
 782                 }
 783
 784                 /* report tap information */
 785                 if (source & INT_SRC_TAP)
 786                         sysfs_notify(&indio_dev->dev.kobj, NULL, "event_tap");
 787                 /* report orientation information */
 788                 if (source & INT_SRC_DISPLAY_ORIENT)
 789                         sysfs_notify(&indio_dev->dev.kobj, NULL,
 790                                      "event_display_orientation");
 791         }
 792         /*divider and counter is used to decrease the speed of read in
 793                 high frequency sample rate*/
 794         if (conf->compass_fifo_enable) {
 795                 c[0] = 0;
 796                 c[1] = 0;
 797                 c[2] = 0;
 798                 if (conf->dmp_on)
 799                         compass_divider = st->compass_dmp_divider;
 800                 else
 801                         compass_divider = st->compass_divider;
 802                 if (compass_divider <= st->compass_counter) {
 803                         /*read from external sensor data register */
 804                         result = inv_plat_read(st, REG_EXT_SENS_DATA_00,
 805                                               NUM_BYTES_COMPASS_SLAVE, d);
 806                         /* d[7] is status 2 register */
 807                         /*for AKM8975, bit 2 and 3 should be all be zero*/
 808                         /* for AMK8963, bit 3 should be zero*/
 809                         if ((DATA_AKM_DRDY == d[0]) &&
 810                             (0 == (d[7] & DATA_AKM_STAT_MASK)) &&
 811                             (!result)) {
 812                                 u8 *sens;
 813                                 sens = st->chip_info.compass_sens;
 814                                 c[0] = (short)((d[2] << 8) | d[1]);
 815                                 c[1] = (short)((d[4] << 8) | d[3]);
 816                                 c[2] = (short)((d[6] << 8) | d[5]);
 817                                 c[0] = (short)(((int)c[0] *
 818                                                (sens[0] + 128)) >> 8);
 819                                 c[1] = (short)(((int)c[1] *
 820                                                (sens[1] + 128)) >> 8);
 821                                 c[2] = (short)(((int)c[2] *
 822                                                (sens[2] + 128)) >> 8);
 823                                 st->raw_compass[0] = c[0];
 824                                 st->raw_compass[1] = c[1];
 825                                 st->raw_compass[2] = c[2];
 826                         }
 827                         st->compass_counter = 0;
 828                 } else if (compass_divider != 0) {
 829                         st->compass_counter++;
 830                 }
 831                 if (!conf->normal_compass_measure) {
 832                         c[0] = 0;
 833                         c[1] = 0;
 834                         c[2] = 0;
 835                         conf->normal_compass_measure = 1;
 836                 }
 837                 for (i = 0; i < 3; i++)
 838                         memcpy(&buf[ind + i * sizeof(c[i])], &c[i],
 839                                                 sizeof(c[i]));
 840                 ind += BYTES_PER_SENSOR;
 841         }
 842         tmp = (u64 *)buf;
 843         tmp[DIV_ROUND_UP(ind, 8)] = t;
 844
 845         if (ind > 0) {
 846 #ifdef _RATE_DEBUG
 847                 s64 tm_cur = get_time_ns();
 848                 s64 tm_delta = tm_cur - t;
 849
 850                 if (tm_min <= 0 && tm_max <= 0) {
 851                         tm_min = tm_delta;
 852                         tm_max = tm_delta;
 853                         tm_sum = 0;
 854                         tm_count = 0;
 855                 } else if (tm_delta < tm_min) {
 856                         tm_min = tm_delta;
 857                 } else if (tm_delta > tm_max) {
 858                         tm_max = tm_delta;
 859                 }
 860                 tm_sum += tm_delta;
 861                 tm_count++;
 862
 863                 if (unlikely((tm_cur - tm_last_print) >= 1000000000)) {
 864                         pr_info("MPU6050 report size: %d rate: %lld\n", ind, tm_count);
 865                         pr_info("MPU KRNL report: [%lld] %lld,%d,%lld\n", t, tm_min, (u32)tm_sum / (u32)tm_count, tm_max);
 866                         tm_last_print = tm_cur;
 867                         tm_min = 0;
 868                         tm_max = 0;
 869                         tm_count = 0;
 870                         tm_sum = 0;
 871                 }
 872 #endif
 873
 874 #ifdef INV_KERNEL_3_10
 875                 iio_push_to_buffers(indio_dev, buf);
 876 #else
 877                 iio_push_to_buffer(indio_dev->buffer, buf, t);
 878 #endif
 879         }
 880
 881         return 0;
 882 }
 883
 884 static void inv_process_motion(struct inv_mpu_iio_s *st)
 885 {
 886         struct iio_dev *indio_dev = iio_priv_to_dev(st);
 887         s32 diff, true_motion;
 888         s64 timestamp;
 889         int result;
 890         u8 data[1];
 891
 892         /* motion interrupt */
 893         result = inv_plat_read(st, REG_INT_STATUS, 1, data);
 894         if (result)
 895                 return;
 896
 897         if (data[0] & BIT_MOT_INT) {
 898                 timestamp = get_time_ns();
 899                 diff = (int)(((timestamp - st->mpu6500_last_motion_time) >>
 900                         NS_PER_MS_SHIFT));
 901                 if (diff > st->mot_int.mot_dur) {
 902                         st->mpu6500_last_motion_time = timestamp;
 903                         true_motion = 1;
 904                 } else {
 905                         true_motion = 0;
 906                 }
 907                 if (true_motion)
 908                         sysfs_notify(&indio_dev->dev.kobj, NULL,
 909                                         "event_accel_motion");
 910         }
 911 }
 912
 913 static int get_bytes_per_datum(struct inv_mpu_iio_s *st)
 914 {
 915         int bytes_per_datum;
 916
 917         bytes_per_datum = 0;
 918         if (st->chip_config.dmp_on) {
 919                 if (st->chip_config.quaternion_on)
 920                         bytes_per_datum += QUATERNION_BYTES;
 921                 if (st->chip_config.tap_on ||
 922                         st->chip_config.display_orient_on)
 923                         bytes_per_datum += BYTES_FOR_EVENTS;
 924         }
 925         if (st->chip_config.accl_fifo_enable)
 926                 bytes_per_datum += BYTES_PER_SENSOR;
 927         if (st->chip_config.gyro_fifo_enable)
 928                 bytes_per_datum += BYTES_PER_SENSOR;
 929
 930         return bytes_per_datum;
 931 }
 932
 933 /**
 934  *  inv_read_fifo() - Transfer data from FIFO to ring buffer.
 935  */
 936 irqreturn_t inv_read_fifo(int irq, void *dev_id)
 937 {
 938
 939         struct inv_mpu_iio_s *st = (struct inv_mpu_iio_s *)dev_id;
 940         struct iio_dev *indio_dev = iio_priv_to_dev(st);
 941         size_t bytes_per_datum;
 942         int result;
 943         u8 data[BYTES_FOR_DMP + QUATERNION_BYTES];
 944         u16 fifo_count;
 945         u32 copied;
 946         s64 timestamp;
 947         struct inv_reg_map_s *reg;
 948         s64 buf[8];
 949         s8 *tmp;
 950
 951 #ifdef _RATE_DEBUG
 952         s64 tm_i2c_sum = 0;
 953         s64 tm = 0;
 954         s64 tm_begin, tm_end;
 955
 956         tm_begin = get_time_ns();
 957 #endif
 958
 959         mutex_lock(&indio_dev->mlock);
 960         if (!(iio_buffer_enabled(indio_dev)))
 961                 goto end_session;
 962
 963         reg = &st->reg;
 964         if (!(st->chip_config.accl_fifo_enable |
 965                 st->chip_config.gyro_fifo_enable |
 966                 st->chip_config.dmp_on |
 967                 st->chip_config.compass_fifo_enable |
 968                 st->mot_int.mot_on))
 969                 goto end_session;
 970         if (st->mot_int.mot_on)
 971                 inv_process_motion(st);
 972         if (st->chip_config.dmp_on && st->chip_config.smd_enable) {
 973                 /* dmp interrupt status */
 974                 result = inv_plat_read(st, REG_DMP_INT_STATUS, 1, data);
 975                 if (!result)
 976                         if (data[0] & SMD_INT_ON) {
 977                                 sysfs_notify(&indio_dev->dev.kobj, NULL,
 978                                                 "event_smd");
 979                                 st->chip_config.smd_enable = 0;
 980                         }
 981         }
 982         if (st->chip_config.lpa_mode) {
 983                 result = inv_plat_read(st, reg->raw_accl,
 984                                       BYTES_PER_SENSOR, data);
 985                 if (result)
 986                         goto end_session;
 987                 inv_report_gyro_accl_compass(indio_dev, data,
 988                                              get_time_ns());
 989                 goto end_session;
 990         }
 991         bytes_per_datum = get_bytes_per_datum(st);
 992         fifo_count = 0;
 993         if (bytes_per_datum != 0) {
 994 #ifdef _RATE_DEBUG
 995                 tm = get_time_ns();
 996 #endif
 997                 result = inv_plat_read(st, reg->fifo_count_h,
 998                                 FIFO_COUNT_BYTE, data);
 999 #ifdef _RATE_DEBUG
1000                 tm_i2c_sum += get_time_ns() - tm;
1001 #endif
1002                 if (result)
1003                         goto end_session;
1004                 fifo_count = be16_to_cpup((__be16 *)(&data[0]));
1005                 if (fifo_count == 0)
1006                         goto flush_fifo;
1007                 if (fifo_count < bytes_per_datum)
1008                         goto end_session;
1009                 /* fifo count can't be odd number */
1010                 if (fifo_count & 1)
1011                         goto flush_fifo;
1012                 if (fifo_count >  FIFO_THRESHOLD)
1013                         goto flush_fifo;
1014                 /* timestamp mismatch. */
1015                 if (kfifo_len(&st->timestamps) <
1016                         fifo_count / bytes_per_datum)
1017                         goto flush_fifo;
1018                 if (kfifo_len(&st->timestamps) >
1019                         fifo_count / bytes_per_datum + TIME_STAMP_TOR) {
1020                         if (st->chip_config.dmp_on) {
1021                                 result = kfifo_to_user(&st->timestamps,
1022                                 &timestamp, sizeof(timestamp), &copied);
1023                                 if (result)
1024                                         goto flush_fifo;
1025                         } else {
1026                                 goto flush_fifo;
1027                         }
1028                 }
1029         } else {
1030                 result = kfifo_to_user(&st->timestamps,
1031                         &timestamp, sizeof(timestamp), &copied);
1032                 if (result)
1033                         goto flush_fifo;
1034         }
1035         tmp = (s8 *)buf;
1036         while ((bytes_per_datum != 0) && (fifo_count >= bytes_per_datum)) {
1037 #ifdef _RATE_DEBUG
1038                 tm = get_time_ns();
1039 #endif
1040                 result = inv_plat_read(st, reg->fifo_r_w, bytes_per_datum,
1041                         data);
1042 #ifdef _RATE_DEBUG
1043                 tm_i2c_sum += get_time_ns() - tm;
1044 #endif
1045                 if (result)
1046                         goto flush_fifo;
1047
1048                 result = kfifo_to_user(&st->timestamps,
1049                         &timestamp, sizeof(timestamp), &copied);
1050                 if (result)
1051                         goto flush_fifo;
1052                 inv_report_gyro_accl_compass(indio_dev, data, timestamp);
1053                 fifo_count -= bytes_per_datum;
1054         }
1055         if (bytes_per_datum == 0 && st->chip_config.compass_fifo_enable)
1056                 inv_report_gyro_accl_compass(indio_dev, data, timestamp);
1057
1058 end_session:
1059         mutex_unlock(&indio_dev->mlock);
1060
1061 #ifdef _RATE_DEBUG
1062         tm_end = get_time_ns();
1063         if (tm_end - tm_begin > 700000)
1064                 pr_info("%s: [%lld] %lld %lld %lld\n", __func__, timestamp, tm_begin - timestamp, tm_i2c_sum, tm_end - tm_begin);
1065 #endif
1066
1067         return IRQ_HANDLED;
1068
1069 flush_fifo:
1070         /* Flush HW and SW FIFOs. */
1071         inv_reset_fifo(indio_dev);
1072         inv_clear_kfifo(st);
1073         mutex_unlock(&indio_dev->mlock);
1074
1075         return IRQ_HANDLED;
1076 }
1077
1078 void inv_mpu_unconfigure_ring(struct iio_dev *indio_dev)
1079 {
1080         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1081         free_irq(st->irq, st);
1082         iio_kfifo_free(indio_dev->buffer);
1083 };
1084
1085 static int inv_postenable(struct iio_dev *indio_dev)
1086 {
1087         return set_inv_enable(indio_dev, true);
1088 }
1089
1090 static int inv_predisable(struct iio_dev *indio_dev)
1091 {
1092         return set_inv_enable(indio_dev, false);
1093 }
1094
1095 static void inv_scan_query(struct iio_dev *indio_dev)
1096 {
1097         struct inv_mpu_iio_s  *st = iio_priv(indio_dev);
1098         struct iio_buffer *ring = indio_dev->buffer;
1099         int result;
1100
1101         if (iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_GYRO_X) ||
1102             iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_GYRO_Y) ||
1103             iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_GYRO_Z))
1104                 st->chip_config.gyro_fifo_enable = 1;
1105         else
1106                 st->chip_config.gyro_fifo_enable = 0;
1107
1108         if (iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_ACCL_X) ||
1109             iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_ACCL_Y) ||
1110             iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_ACCL_Z))
1111                 st->chip_config.accl_fifo_enable = 1;
1112         else
1113                 st->chip_config.accl_fifo_enable = 0;
1114
1115         if (iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_MAGN_X) ||
1116             iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_MAGN_Y) ||
1117             iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_MAGN_Z))
1118                 st->chip_config.compass_fifo_enable = 1;
1119         else
1120                 st->chip_config.compass_fifo_enable = 0;
1121
1122         /* check to make sure engine is turned on if fifo is turned on */
1123         if (st->chip_config.gyro_fifo_enable &&
1124                 (!st->chip_config.gyro_enable)) {
1125                 result = st->switch_gyro_engine(st, true);
1126                 if (result)
1127                         return;
1128                 st->chip_config.gyro_enable = true;
1129         }
1130         if (st->chip_config.accl_fifo_enable &&
1131                 (!st->chip_config.accl_enable)) {
1132                 result = st->switch_accl_engine(st, true);
1133                 if (result)
1134                         return;
1135                 st->chip_config.accl_enable = true;
1136         }
1137 }
1138
1139 static int inv_check_quaternion(struct iio_dev *indio_dev)
1140 {
1141         struct inv_mpu_iio_s  *st = iio_priv(indio_dev);
1142         struct iio_buffer *ring = indio_dev->buffer;
1143         int result;
1144
1145         if (st->chip_config.dmp_on) {
1146                 if (
1147                   iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_R) ||
1148                   iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_X) ||
1149                   iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_Y) ||
1150                   iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_Z))
1151                         st->chip_config.quaternion_on = 1;
1152                 else
1153                         st->chip_config.quaternion_on = 0;
1154
1155                 result = inv_send_quaternion(st,
1156                                 st->chip_config.quaternion_on);
1157                 if (result)
1158                         return result;
1159         } else {
1160                 st->chip_config.quaternion_on = 0;
1161                 clear_bit(INV_MPU_SCAN_QUAT_R, ring->scan_mask);
1162                 clear_bit(INV_MPU_SCAN_QUAT_X, ring->scan_mask);
1163                 clear_bit(INV_MPU_SCAN_QUAT_Y, ring->scan_mask);
1164                 clear_bit(INV_MPU_SCAN_QUAT_Z, ring->scan_mask);
1165         }
1166
1167         return 0;
1168 }
1169
1170 static int inv_check_conflict_sysfs(struct iio_dev *indio_dev)
1171 {
1172         struct inv_mpu_iio_s  *st = iio_priv(indio_dev);
1173         struct iio_buffer *ring = indio_dev->buffer;
1174         int result;
1175
1176         if (st->chip_config.lpa_mode) {
1177                 /* dmp cannot run with low power mode on */
1178                 st->chip_config.dmp_on = 0;
1179                 result = st->gyro_en(st, ring, false);
1180                 if (result)
1181                         return result;
1182                 result = st->compass_en(st, ring, false);
1183                 if (result)
1184                         return result;
1185                 result = st->quaternion_en(st, ring, false);
1186                 if (result)
1187                         return result;
1188
1189                 result = st->accl_en(st, ring, true);
1190                 if (result)
1191                         return result;
1192         }
1193         result = inv_check_quaternion(indio_dev);
1194         if (result)
1195                 return result;
1196
1197         return result;
1198 }
1199
1200 static int inv_preenable(struct iio_dev *indio_dev)
1201 {
1202         int result;
1203         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1204
1205         result = st->set_power_state(st, true);
1206         if (result)
1207                 return result;
1208
1209         result = inv_check_conflict_sysfs(indio_dev);
1210         if (result)
1211                 return result;
1212         inv_scan_query(indio_dev);
1213
1214         return result;
1215 }
1216
1217 static const struct iio_buffer_setup_ops inv_mpu_ring_setup_ops = {
1218         .preenable = &inv_preenable,
1219         .postenable = &inv_postenable,
1220         .predisable = &inv_predisable,
1221 };
1222
1223 int inv_mpu_configure_ring(struct iio_dev *indio_dev)
1224 {
1225         int ret;
1226         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1227         struct iio_buffer *ring;
1228
1229         ring = iio_kfifo_allocate();
1230         if (!ring)
1231                 return -ENOMEM;
1232         indio_dev->buffer = ring;
1233         /* setup ring buffer */
1234         ring->scan_timestamp = true;
1235         indio_dev->setup_ops = &inv_mpu_ring_setup_ops;
1236         /*scan count double count timestamp. should subtract 1. but
1237         number of channels still includes timestamp*/
1238         if (INV_MPU3050 == st->chip_type)
1239                 ret = request_threaded_irq(st->irq, inv_irq_handler,
1240                         inv_read_fifo_mpu3050,
1241                         IRQF_TRIGGER_RISING | IRQF_SHARED, "inv_irq", st);
1242         else
1243                 ret = request_threaded_irq(st->irq, inv_irq_handler,
1244                         inv_read_fifo,
1245                         IRQF_TRIGGER_RISING | IRQF_SHARED, "inv_irq", st);
1246         if (ret)
1247                 goto error_iio_sw_rb_free;
1248
1249         indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
1250         return 0;
1251 error_iio_sw_rb_free:
1252         iio_kfifo_free(indio_dev->buffer);
1253         return ret;
1254 }
1255
1256 /**
1257  *  @}
1258  */
1259