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 static int inv_report_gyro_accl_compass(struct iio_dev *indio_dev,
 722                                         u8 *data, s64 t)
 723 {
 724         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
 725         short g[THREE_AXIS], a[THREE_AXIS], c[THREE_AXIS];
 726         int q[4];
 727         int result, ind;
 728         u32 word;
 729         u8 d[8], compass_divider;
 730         u8 buf[64];
 731         u64 *tmp;
 732         int source, i;
 733         struct inv_chip_config_s *conf;
 734
 735         conf = &st->chip_config;
 736         ind = 0;
 737
 738         if (conf->quaternion_on & conf->dmp_on) {
 739                 for (i = 0; i < ARRAY_SIZE(q); i++) {
 740                         q[i] = be32_to_cpup((__be32 *)(&data[ind + i * 4]));
 741                         st->raw_quaternion[i] = q[i];
 742                         memcpy(&buf[ind + i * sizeof(q[i])], &q[i],
 743                                                 sizeof(q[i]));
 744                 }
 745                 ind += QUATERNION_BYTES;
 746         }
 747
 748         if (conf->accl_fifo_enable) {
 749                 for (i = 0; i < ARRAY_SIZE(a); i++) {
 750                         a[i] = be16_to_cpup((__be16 *)(&data[ind + i * 2]));
 751                         memcpy(&buf[ind + i * sizeof(a[i])], &a[i],
 752                                                 sizeof(a[i]));
 753                 }
 754                 ind += BYTES_PER_SENSOR;
 755         }
 756
 757         if (conf->gyro_fifo_enable) {
 758                 for (i = 0; i < ARRAY_SIZE(g); i++) {
 759                         g[i] = be16_to_cpup((__be16 *)(&data[ind + i * 2]));
 760                         memcpy(&buf[ind + i * sizeof(g[i])], &g[i],
 761                                                 sizeof(g[i]));
 762                 }
 763                 ind += BYTES_PER_SENSOR;
 764         }
 765
 766         if (conf->dmp_on && (conf->tap_on || conf->display_orient_on)) {
 767                 word = (u32)(be32_to_cpup((u32 *)&data[ind]));
 768                 source = ((word >> 16) & 0xff);
 769                 if (source) {
 770                         st->tap_data = (DMP_MASK_TAP & (word & 0xff));
 771                         st->display_orient_data =
 772                         ((DMP_MASK_DIS_ORIEN & (word & 0xff)) >>
 773                           DMP_DIS_ORIEN_SHIFT);
 774                 }
 775
 776                 /* report tap information */
 777                 if (source & INT_SRC_TAP)
 778                         sysfs_notify(&indio_dev->dev.kobj, NULL, "event_tap");
 779                 /* report orientation information */
 780                 if (source & INT_SRC_DISPLAY_ORIENT)
 781                         sysfs_notify(&indio_dev->dev.kobj, NULL,
 782                                      "event_display_orientation");
 783         }
 784         /*divider and counter is used to decrease the speed of read in
 785                 high frequency sample rate*/
 786         if (conf->compass_fifo_enable) {
 787                 c[0] = 0;
 788                 c[1] = 0;
 789                 c[2] = 0;
 790                 if (conf->dmp_on)
 791                         compass_divider = st->compass_dmp_divider;
 792                 else
 793                         compass_divider = st->compass_divider;
 794                 if (compass_divider <= st->compass_counter) {
 795                         /*read from external sensor data register */
 796                         result = inv_plat_read(st, REG_EXT_SENS_DATA_00,
 797                                               NUM_BYTES_COMPASS_SLAVE, d);
 798                         /* d[7] is status 2 register */
 799                         /*for AKM8975, bit 2 and 3 should be all be zero*/
 800                         /* for AMK8963, bit 3 should be zero*/
 801                         if ((DATA_AKM_DRDY == d[0]) &&
 802                             (0 == (d[7] & DATA_AKM_STAT_MASK)) &&
 803                             (!result)) {
 804                                 u8 *sens;
 805                                 sens = st->chip_info.compass_sens;
 806                                 c[0] = (short)((d[2] << 8) | d[1]);
 807                                 c[1] = (short)((d[4] << 8) | d[3]);
 808                                 c[2] = (short)((d[6] << 8) | d[5]);
 809                                 c[0] = (short)(((int)c[0] *
 810                                                (sens[0] + 128)) >> 8);
 811                                 c[1] = (short)(((int)c[1] *
 812                                                (sens[1] + 128)) >> 8);
 813                                 c[2] = (short)(((int)c[2] *
 814                                                (sens[2] + 128)) >> 8);
 815                                 st->raw_compass[0] = c[0];
 816                                 st->raw_compass[1] = c[1];
 817                                 st->raw_compass[2] = c[2];
 818                         }
 819                         st->compass_counter = 0;
 820                 } else if (compass_divider != 0) {
 821                         st->compass_counter++;
 822                 }
 823                 if (!conf->normal_compass_measure) {
 824                         c[0] = 0;
 825                         c[1] = 0;
 826                         c[2] = 0;
 827                         conf->normal_compass_measure = 1;
 828                 }
 829                 for (i = 0; i < 3; i++)
 830                         memcpy(&buf[ind + i * sizeof(c[i])], &c[i],
 831                                                 sizeof(c[i]));
 832                 ind += BYTES_PER_SENSOR;
 833         }
 834         tmp = (u64 *)buf;
 835         tmp[DIV_ROUND_UP(ind, 8)] = t;
 836
 837         if (ind > 0) {
 838 #ifdef INV_KERNEL_3_10
 839                 iio_push_to_buffers(indio_dev, buf);
 840 #else
 841                 iio_push_to_buffer(indio_dev->buffer, buf, t);
 842 #endif
 843         }
 844
 845         return 0;
 846 }
 847
 848 static void inv_process_motion(struct inv_mpu_iio_s *st)
 849 {
 850         struct iio_dev *indio_dev = iio_priv_to_dev(st);
 851         s32 diff, true_motion;
 852         s64 timestamp;
 853         int result;
 854         u8 data[1];
 855
 856         /* motion interrupt */
 857         result = inv_plat_read(st, REG_INT_STATUS, 1, data);
 858         if (result)
 859                 return;
 860
 861         if (data[0] & BIT_MOT_INT) {
 862                 timestamp = get_time_ns();
 863                 diff = (int)(((timestamp - st->mpu6500_last_motion_time) >>
 864                         NS_PER_MS_SHIFT));
 865                 if (diff > st->mot_int.mot_dur) {
 866                         st->mpu6500_last_motion_time = timestamp;
 867                         true_motion = 1;
 868                 } else {
 869                         true_motion = 0;
 870                 }
 871                 if (true_motion)
 872                         sysfs_notify(&indio_dev->dev.kobj, NULL,
 873                                         "event_accel_motion");
 874         }
 875 }
 876
 877 static int get_bytes_per_datum(struct inv_mpu_iio_s *st)
 878 {
 879         int bytes_per_datum;
 880
 881         bytes_per_datum = 0;
 882         if (st->chip_config.dmp_on) {
 883                 if (st->chip_config.quaternion_on)
 884                         bytes_per_datum += QUATERNION_BYTES;
 885                 if (st->chip_config.tap_on ||
 886                         st->chip_config.display_orient_on)
 887                         bytes_per_datum += BYTES_FOR_EVENTS;
 888         }
 889         if (st->chip_config.accl_fifo_enable)
 890                 bytes_per_datum += BYTES_PER_SENSOR;
 891         if (st->chip_config.gyro_fifo_enable)
 892                 bytes_per_datum += BYTES_PER_SENSOR;
 893
 894         return bytes_per_datum;
 895 }
 896
 897 #ifdef _RATE_DEBUG
 898 static s64 tm_end_min = -1;
 899 static s64 tm_end_max = -1;
 900 static s64 tm_end_sum = -1;
 901 static s64 tm_end_last_print = -1;
 902 static s64 tm_end_count = -1;
 903 #endif
 904 /**
 905  *  inv_read_fifo() - Transfer data from FIFO to ring buffer.
 906  */
 907 irqreturn_t inv_read_fifo(int irq, void *dev_id)
 908 {
 909
 910         struct inv_mpu_iio_s *st = (struct inv_mpu_iio_s *)dev_id;
 911         struct iio_dev *indio_dev = iio_priv_to_dev(st);
 912         size_t bytes_per_datum;
 913         int result;
 914         u8 data[BYTES_FOR_DMP + QUATERNION_BYTES];
 915         u16 fifo_count;
 916         u32 copied;
 917         s64 timestamp;
 918         struct inv_reg_map_s *reg;
 919         s64 buf[8];
 920         s8 *tmp;
 921
 922         mutex_lock(&indio_dev->mlock);
 923         if (!(iio_buffer_enabled(indio_dev)))
 924                 goto end_session;
 925
 926         reg = &st->reg;
 927         if (!(st->chip_config.accl_fifo_enable |
 928                 st->chip_config.gyro_fifo_enable |
 929                 st->chip_config.dmp_on |
 930                 st->chip_config.compass_fifo_enable |
 931                 st->mot_int.mot_on))
 932                 goto end_session;
 933         if (st->mot_int.mot_on)
 934                 inv_process_motion(st);
 935         if (st->chip_config.dmp_on && st->chip_config.smd_enable) {
 936                 /* dmp interrupt status */
 937                 result = inv_plat_read(st, REG_DMP_INT_STATUS, 1, data);
 938                 if (!result)
 939                         if (data[0] & SMD_INT_ON) {
 940                                 sysfs_notify(&indio_dev->dev.kobj, NULL,
 941                                                 "event_smd");
 942                                 st->chip_config.smd_enable = 0;
 943                         }
 944         }
 945         if (st->chip_config.lpa_mode) {
 946                 result = inv_plat_read(st, reg->raw_accl,
 947                                       BYTES_PER_SENSOR, data);
 948                 if (result)
 949                         goto end_session;
 950                 inv_report_gyro_accl_compass(indio_dev, data,
 951                                              get_time_ns());
 952                 goto end_session;
 953         }
 954         bytes_per_datum = get_bytes_per_datum(st);
 955         fifo_count = 0;
 956         if (bytes_per_datum != 0) {
 957                 result = inv_plat_read(st, reg->fifo_count_h,
 958                                 FIFO_COUNT_BYTE, data);
 959                 if (result)
 960                         goto end_session;
 961                 fifo_count = be16_to_cpup((__be16 *)(&data[0]));
 962                 if ((fifo_count == 0) && (kfifo_len(&st->timestamps) > 0))
 963                         goto flush_fifo;
 964                 if (fifo_count < bytes_per_datum)
 965                         goto end_session;
 966                 /* fifo count can't be odd number */
 967                 if (fifo_count & 1)
 968                         goto flush_fifo;
 969                 if (fifo_count >  FIFO_THRESHOLD)
 970                         goto flush_fifo;
 971                 /* timestamp mismatch. */
 972                 if (kfifo_len(&st->timestamps) <
 973                         fifo_count / bytes_per_datum)
 974                         goto flush_fifo;
 975                 if (kfifo_len(&st->timestamps) >
 976                         fifo_count / bytes_per_datum + TIME_STAMP_TOR) {
 977                         if (st->chip_config.dmp_on) {
 978                                 result = kfifo_to_user(&st->timestamps,
 979                                 &timestamp, sizeof(timestamp), &copied);
 980                                 if (result)
 981                                         goto flush_fifo;
 982                         } else {
 983                                 goto flush_fifo;
 984                         }
 985                 }
 986         } else {
 987                 result = kfifo_to_user(&st->timestamps,
 988                         &timestamp, sizeof(timestamp), &copied);
 989                 if (result)
 990                         goto flush_fifo;
 991         }
 992         tmp = (s8 *)buf;
 993         while ((bytes_per_datum != 0) && (fifo_count >= bytes_per_datum)) {
 994                 result = inv_plat_read(st, reg->fifo_r_w, bytes_per_datum,
 995                         data);
 996                 if (result)
 997                         goto flush_fifo;
 998
 999                 result = kfifo_to_user(&st->timestamps,
1000                         &timestamp, sizeof(timestamp), &copied);
1001                 if (result)
1002                         goto flush_fifo;
1003                 inv_report_gyro_accl_compass(indio_dev, data, timestamp);
1004                 fifo_count -= bytes_per_datum;
1005         }
1006         if (bytes_per_datum == 0 && st->chip_config.compass_fifo_enable)
1007                 inv_report_gyro_accl_compass(indio_dev, data, timestamp);
1008
1009 #ifdef _RATE_DEBUG
1010 {
1011         s64 tm_end_cur = get_time_ns();
1012         s64 tm_end_delta = tm_end_cur - timestamp;
1013
1014         if (tm_end_min <= 0 && tm_end_max <= 0) {
1015                 tm_end_min = tm_end_delta;
1016                 tm_end_max = tm_end_delta;
1017                 tm_end_sum = 0;
1018                 tm_end_count = 0;
1019         } else if (tm_end_delta < tm_end_min) {
1020                 tm_end_min = tm_end_delta;
1021         } else if (tm_end_delta > tm_end_max) {
1022                 tm_end_max = tm_end_delta;
1023         }
1024         tm_end_sum += tm_end_delta;
1025         tm_end_count++;
1026
1027         if (unlikely((tm_end_cur - tm_end_last_print) >= 1000000000)) {
1028                 pr_info("MPU KRNL report rate[%4lld]: %8lld, %8d, %8lld\n",
1029                         tm_end_count, tm_end_min, (u32)tm_end_sum / (u32)tm_end_count, tm_end_max);
1030                 tm_end_last_print = tm_end_cur;
1031                 tm_end_min = 0;
1032                 tm_end_max = 0;
1033                 tm_end_count = 0;
1034                 tm_end_sum = 0;
1035         }
1036 }
1037 #endif
1038
1039 end_session:
1040         mutex_unlock(&indio_dev->mlock);
1041         return IRQ_HANDLED;
1042
1043 flush_fifo:
1044         /* Flush HW and SW FIFOs. */
1045         pr_info("fifo_count = %d, fifo_len = %d\n", fifo_count, kfifo_len(&st->timestamps));
1046         inv_reset_fifo(indio_dev);
1047         inv_clear_kfifo(st);
1048         mutex_unlock(&indio_dev->mlock);
1049
1050         return IRQ_HANDLED;
1051 }
1052
1053 void inv_mpu_unconfigure_ring(struct iio_dev *indio_dev)
1054 {
1055         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1056         free_irq(st->irq, st);
1057         iio_kfifo_free(indio_dev->buffer);
1058 };
1059
1060 static int inv_postenable(struct iio_dev *indio_dev)
1061 {
1062         return set_inv_enable(indio_dev, true);
1063 }
1064
1065 static int inv_predisable(struct iio_dev *indio_dev)
1066 {
1067         return set_inv_enable(indio_dev, false);
1068 }
1069
1070 static void inv_scan_query(struct iio_dev *indio_dev)
1071 {
1072         struct inv_mpu_iio_s  *st = iio_priv(indio_dev);
1073         struct iio_buffer *ring = indio_dev->buffer;
1074         int result;
1075
1076         if (iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_GYRO_X) ||
1077             iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_GYRO_Y) ||
1078             iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_GYRO_Z))
1079                 st->chip_config.gyro_fifo_enable = 1;
1080         else
1081                 st->chip_config.gyro_fifo_enable = 0;
1082
1083         if (iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_ACCL_X) ||
1084             iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_ACCL_Y) ||
1085             iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_ACCL_Z))
1086                 st->chip_config.accl_fifo_enable = 1;
1087         else
1088                 st->chip_config.accl_fifo_enable = 0;
1089
1090         if (iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_MAGN_X) ||
1091             iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_MAGN_Y) ||
1092             iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_MAGN_Z))
1093                 st->chip_config.compass_fifo_enable = 1;
1094         else
1095                 st->chip_config.compass_fifo_enable = 0;
1096
1097         /* check to make sure engine is turned on if fifo is turned on */
1098         if (st->chip_config.gyro_fifo_enable &&
1099                 (!st->chip_config.gyro_enable)) {
1100                 result = st->switch_gyro_engine(st, true);
1101                 if (result)
1102                         return;
1103                 st->chip_config.gyro_enable = true;
1104         }
1105         if (st->chip_config.accl_fifo_enable &&
1106                 (!st->chip_config.accl_enable)) {
1107                 result = st->switch_accl_engine(st, true);
1108                 if (result)
1109                         return;
1110                 st->chip_config.accl_enable = true;
1111         }
1112 }
1113
1114 static int inv_check_quaternion(struct iio_dev *indio_dev)
1115 {
1116         struct inv_mpu_iio_s  *st = iio_priv(indio_dev);
1117         struct iio_buffer *ring = indio_dev->buffer;
1118         int result;
1119
1120         if (st->chip_config.dmp_on) {
1121                 if (
1122                   iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_R) ||
1123                   iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_X) ||
1124                   iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_Y) ||
1125                   iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_Z))
1126                         st->chip_config.quaternion_on = 1;
1127                 else
1128                         st->chip_config.quaternion_on = 0;
1129
1130                 result = inv_send_quaternion(st,
1131                                 st->chip_config.quaternion_on);
1132                 if (result)
1133                         return result;
1134         } else {
1135                 st->chip_config.quaternion_on = 0;
1136                 clear_bit(INV_MPU_SCAN_QUAT_R, ring->scan_mask);
1137                 clear_bit(INV_MPU_SCAN_QUAT_X, ring->scan_mask);
1138                 clear_bit(INV_MPU_SCAN_QUAT_Y, ring->scan_mask);
1139                 clear_bit(INV_MPU_SCAN_QUAT_Z, ring->scan_mask);
1140         }
1141
1142         return 0;
1143 }
1144
1145 static int inv_check_conflict_sysfs(struct iio_dev *indio_dev)
1146 {
1147         struct inv_mpu_iio_s  *st = iio_priv(indio_dev);
1148         struct iio_buffer *ring = indio_dev->buffer;
1149         int result;
1150
1151         if (st->chip_config.lpa_mode) {
1152                 /* dmp cannot run with low power mode on */
1153                 st->chip_config.dmp_on = 0;
1154                 result = st->gyro_en(st, ring, false);
1155                 if (result)
1156                         return result;
1157                 result = st->compass_en(st, ring, false);
1158                 if (result)
1159                         return result;
1160                 result = st->quaternion_en(st, ring, false);
1161                 if (result)
1162                         return result;
1163
1164                 result = st->accl_en(st, ring, true);
1165                 if (result)
1166                         return result;
1167         }
1168         result = inv_check_quaternion(indio_dev);
1169         if (result)
1170                 return result;
1171
1172         return result;
1173 }
1174
1175 static int inv_preenable(struct iio_dev *indio_dev)
1176 {
1177         int result;
1178         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1179
1180         result = st->set_power_state(st, true);
1181         if (result)
1182                 return result;
1183
1184         result = inv_check_conflict_sysfs(indio_dev);
1185         if (result)
1186                 return result;
1187         inv_scan_query(indio_dev);
1188
1189         return result;
1190 }
1191
1192 static const struct iio_buffer_setup_ops inv_mpu_ring_setup_ops = {
1193         .preenable = &inv_preenable,
1194         .postenable = &inv_postenable,
1195         .predisable = &inv_predisable,
1196 };
1197
1198 int inv_mpu_configure_ring(struct iio_dev *indio_dev)
1199 {
1200         int ret;
1201         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1202         struct iio_buffer *ring;
1203
1204         ring = iio_kfifo_allocate();
1205         if (!ring)
1206                 return -ENOMEM;
1207         indio_dev->buffer = ring;
1208         /* setup ring buffer */
1209         ring->scan_timestamp = true;
1210         indio_dev->setup_ops = &inv_mpu_ring_setup_ops;
1211         /*scan count double count timestamp. should subtract 1. but
1212         number of channels still includes timestamp*/
1213         if (INV_MPU3050 == st->chip_type)
1214                 ret = request_threaded_irq(st->irq, inv_irq_handler,
1215                         inv_read_fifo_mpu3050,
1216                         IRQF_TRIGGER_RISING | IRQF_SHARED, "inv_irq", st);
1217         else
1218                 ret = request_threaded_irq(st->irq, inv_irq_handler,
1219                         inv_read_fifo,
1220                         IRQF_TRIGGER_RISING | IRQF_SHARED, "inv_irq", st);
1221         if (ret)
1222                 goto error_iio_sw_rb_free;
1223
1224         indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
1225         return 0;
1226 error_iio_sw_rb_free:
1227         iio_kfifo_free(indio_dev->buffer);
1228         return ret;
1229 }
1230
1231 /**
1232  *  @}
1233  */
1234