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 /**
 500  *  inv_irq_handler() - Cache a timestamp at each data ready interrupt.
 501  */
 502 static irqreturn_t inv_irq_handler(int irq, void *dev_id)
 503 {
 504         struct inv_mpu_iio_s *st;
 505         u64 timestamp;
 506         int catch_up;
 507         u64 time_since_last_irq;
 508
 509         st = (struct inv_mpu_iio_s *)dev_id;
 510         timestamp = get_time_ns();
 511         time_since_last_irq = timestamp - st->last_isr_time;
 512         spin_lock(&st->time_stamp_lock);
 513         catch_up = 0;
 514         while ((time_since_last_irq > st->irq_dur_ns * 2) &&
 515                (catch_up < MAX_CATCH_UP) &&
 516                (!st->chip_config.lpa_mode) &&
 517                (!st->chip_config.dmp_on)) {
 518                 st->last_isr_time += st->irq_dur_ns;
 519                 kfifo_in(&st->timestamps,
 520                          &st->last_isr_time, 1);
 521                 time_since_last_irq = timestamp - st->last_isr_time;
 522                 catch_up++;
 523         }
 524         kfifo_in(&st->timestamps, &timestamp, 1);
 525         st->last_isr_time = timestamp;
 526         spin_unlock(&st->time_stamp_lock);
 527
 528         return IRQ_WAKE_THREAD;
 529 }
 530
 531 static int put_scan_to_buf(struct iio_dev *indio_dev, u8 *d,
 532                                 short *s, int scan_index, int d_ind) {
 533         struct iio_buffer *ring = indio_dev->buffer;
 534         int st;
 535         int i;
 536         for (i = 0; i < 3; i++) {
 537                 st = iio_scan_mask_query(indio_dev, ring, scan_index + i);
 538                 if (st) {
 539                         memcpy(&d[d_ind], &s[i], sizeof(s[i]));
 540                         d_ind += sizeof(s[i]);
 541                 }
 542         }
 543         return d_ind;
 544 }
 545
 546 static void inv_report_data_3050(struct iio_dev *indio_dev, s64 t,
 547                         int has_footer, u8 *data)
 548 {
 549         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
 550         struct iio_buffer *ring = indio_dev->buffer;
 551         int ind, i, d_ind;
 552         struct inv_chip_config_s *conf;
 553         short g[THREE_AXIS], a[THREE_AXIS];
 554         s64 buf[8];
 555         u8 *tmp;
 556         int bytes_per_datum, scan_count;
 557         conf = &st->chip_config;
 558
 559         scan_count = bitmap_weight(indio_dev->active_scan_mask,
 560                                        indio_dev->masklength);
 561         bytes_per_datum = scan_count * 2;
 562
 563         ind = 0;
 564         if (has_footer)
 565                 ind += 2;
 566         tmp = (u8 *)buf;
 567         d_ind = 0;
 568
 569         if (conf->gyro_fifo_enable) {
 570                 for (i = 0; i < ARRAY_SIZE(g); i++) {
 571                         g[i] = be16_to_cpup((__be16 *)(&data[ind + i * 2]));
 572                         st->raw_gyro[i] = g[i];
 573                 }
 574                 ind += BYTES_PER_SENSOR;
 575         }
 576         if (conf->accl_fifo_enable) {
 577                 st->mpu_slave->combine_data(&data[ind], a);
 578                 for (i = 0; i < ARRAY_SIZE(a); i++)
 579                         st->raw_accel[i] = a[i];
 580
 581                 ind += BYTES_PER_SENSOR;
 582                 d_ind = put_scan_to_buf(indio_dev, tmp, a,
 583                         INV_MPU_SCAN_ACCL_X, d_ind);
 584         }
 585         if (conf->gyro_fifo_enable)
 586                 d_ind = put_scan_to_buf(indio_dev, tmp, g,
 587                         INV_MPU_SCAN_GYRO_X, d_ind);
 588
 589         i = (bytes_per_datum + 7) / 8;
 590         if (ring->scan_timestamp)
 591                 buf[i] = t;
 592 #ifdef INV_KERNEL_3_10
 593         ring->access->store_to(indio_dev->buffer, (u8 *)buf);
 594 #else
 595         ring->access->store_to(indio_dev->buffer, (u8 *)buf, t);
 596 #endif
 597 }
 598
 599 /**
 600  *  inv_read_fifo_mpu3050() - Transfer data from FIFO to ring buffer for
 601  *                            mpu3050.
 602  */
 603 irqreturn_t inv_read_fifo_mpu3050(int irq, void *dev_id)
 604 {
 605
 606         struct inv_mpu_iio_s *st = (struct inv_mpu_iio_s *)dev_id;
 607         struct iio_dev *indio_dev = iio_priv_to_dev(st);
 608         int bytes_per_datum;
 609         u8 data[64];
 610         int result;
 611         short fifo_count, byte_read;
 612         u32 copied;
 613         s64 timestamp;
 614         struct inv_reg_map_s *reg;
 615         reg = &st->reg;
 616         /* It is impossible that chip is asleep or enable is
 617         zero when interrupt is on
 618         *  because interrupt is now connected with enable */
 619         if (st->chip_config.dmp_on)
 620                 bytes_per_datum = BYTES_FOR_DMP;
 621         else
 622                 bytes_per_datum = (st->chip_config.accl_fifo_enable +
 623                         st->chip_config.gyro_fifo_enable)*BYTES_PER_SENSOR;
 624         if (st->chip_config.has_footer)
 625                 byte_read = bytes_per_datum + MPU3050_FOOTER_SIZE;
 626         else
 627                 byte_read = bytes_per_datum;
 628
 629         fifo_count = 0;
 630         if (byte_read != 0) {
 631                 result = inv_plat_read(st, reg->fifo_count_h,
 632                                 FIFO_COUNT_BYTE, data);
 633                 if (result)
 634                         goto end_session;
 635                 fifo_count = be16_to_cpup((__be16 *)(&data[0]));
 636                 if (fifo_count < byte_read)
 637                         goto end_session;
 638                 if (fifo_count & 1)
 639                         goto flush_fifo;
 640                 if (fifo_count > FIFO_THRESHOLD)
 641                         goto flush_fifo;
 642                 /* Timestamp mismatch. */
 643                 if (kfifo_len(&st->timestamps) <
 644                         fifo_count / byte_read)
 645                         goto flush_fifo;
 646                 if (kfifo_len(&st->timestamps) >
 647                         fifo_count / byte_read + TIME_STAMP_TOR) {
 648                         if (st->chip_config.dmp_on) {
 649                                 result = kfifo_to_user(&st->timestamps,
 650                                 &timestamp, sizeof(timestamp), &copied);
 651                                 if (result)
 652                                         goto flush_fifo;
 653                         } else {
 654                                 goto flush_fifo;
 655                         }
 656                 }
 657         }
 658         while ((bytes_per_datum != 0) && (fifo_count >= byte_read)) {
 659                 result = inv_plat_read(st, reg->fifo_r_w, byte_read, data);
 660                 if (result)
 661                         goto flush_fifo;
 662
 663                 result = kfifo_to_user(&st->timestamps,
 664                         &timestamp, sizeof(timestamp), &copied);
 665                 if (result)
 666                         goto flush_fifo;
 667                 inv_report_data_3050(indio_dev, timestamp,
 668                                      st->chip_config.has_footer, data);
 669                 fifo_count -= byte_read;
 670                 if (st->chip_config.has_footer == 0) {
 671                         st->chip_config.has_footer = 1;
 672                         byte_read = bytes_per_datum + MPU3050_FOOTER_SIZE;
 673                 }
 674         }
 675
 676 end_session:
 677         return IRQ_HANDLED;
 678
 679 flush_fifo:
 680         /* Flush HW and SW FIFOs. */
 681         inv_reset_fifo(indio_dev);
 682         inv_clear_kfifo(st);
 683         return IRQ_HANDLED;
 684 }
 685
 686 static int inv_report_gyro_accl_compass(struct iio_dev *indio_dev,
 687                                         u8 *data, s64 t)
 688 {
 689         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
 690         short g[THREE_AXIS], a[THREE_AXIS], c[THREE_AXIS];
 691         int q[4];
 692         int result, ind;
 693         u32 word;
 694         u8 d[8], compass_divider;
 695         u8 buf[64];
 696         u64 *tmp;
 697         int source, i;
 698         struct inv_chip_config_s *conf;
 699
 700         conf = &st->chip_config;
 701         ind = 0;
 702
 703         if (conf->quaternion_on & conf->dmp_on) {
 704                 for (i = 0; i < ARRAY_SIZE(q); i++) {
 705                         q[i] = be32_to_cpup((__be32 *)(&data[ind + i * 4]));
 706                         st->raw_quaternion[i] = q[i];
 707                         memcpy(&buf[ind + i * sizeof(q[i])], &q[i],
 708                                                 sizeof(q[i]));
 709                 }
 710                 ind += QUATERNION_BYTES;
 711         }
 712
 713         if (conf->accl_fifo_enable) {
 714                 for (i = 0; i < ARRAY_SIZE(a); i++) {
 715                         a[i] = be16_to_cpup((__be16 *)(&data[ind + i * 2]));
 716                         memcpy(&buf[ind + i * sizeof(a[i])], &a[i],
 717                                                 sizeof(a[i]));
 718                 }
 719                 ind += BYTES_PER_SENSOR;
 720         }
 721
 722         if (conf->gyro_fifo_enable) {
 723                 for (i = 0; i < ARRAY_SIZE(g); i++) {
 724                         g[i] = be16_to_cpup((__be16 *)(&data[ind + i * 2]));
 725                         memcpy(&buf[ind + i * sizeof(g[i])], &g[i],
 726                                                 sizeof(g[i]));
 727                 }
 728                 ind += BYTES_PER_SENSOR;
 729         }
 730
 731         if (conf->dmp_on && (conf->tap_on || conf->display_orient_on)) {
 732                 word = (u32)(be32_to_cpup((u32 *)&data[ind]));
 733                 source = ((word >> 16) & 0xff);
 734                 if (source) {
 735                         st->tap_data = (DMP_MASK_TAP & (word & 0xff));
 736                         st->display_orient_data =
 737                         ((DMP_MASK_DIS_ORIEN & (word & 0xff)) >>
 738                           DMP_DIS_ORIEN_SHIFT);
 739                 }
 740
 741                 /* report tap information */
 742                 if (source & INT_SRC_TAP)
 743                         sysfs_notify(&indio_dev->dev.kobj, NULL, "event_tap");
 744                 /* report orientation information */
 745                 if (source & INT_SRC_DISPLAY_ORIENT)
 746                         sysfs_notify(&indio_dev->dev.kobj, NULL,
 747                                      "event_display_orientation");
 748         }
 749         /*divider and counter is used to decrease the speed of read in
 750                 high frequency sample rate*/
 751         if (conf->compass_fifo_enable) {
 752                 c[0] = 0;
 753                 c[1] = 0;
 754                 c[2] = 0;
 755                 if (conf->dmp_on)
 756                         compass_divider = st->compass_dmp_divider;
 757                 else
 758                         compass_divider = st->compass_divider;
 759                 if (compass_divider <= st->compass_counter) {
 760                         /*read from external sensor data register */
 761                         result = inv_plat_read(st, REG_EXT_SENS_DATA_00,
 762                                               NUM_BYTES_COMPASS_SLAVE, d);
 763                         /* d[7] is status 2 register */
 764                         /*for AKM8975, bit 2 and 3 should be all be zero*/
 765                         /* for AMK8963, bit 3 should be zero*/
 766                         if ((DATA_AKM_DRDY == d[0]) &&
 767                             (0 == (d[7] & DATA_AKM_STAT_MASK)) &&
 768                             (!result)) {
 769                                 u8 *sens;
 770                                 sens = st->chip_info.compass_sens;
 771                                 c[0] = (short)((d[2] << 8) | d[1]);
 772                                 c[1] = (short)((d[4] << 8) | d[3]);
 773                                 c[2] = (short)((d[6] << 8) | d[5]);
 774                                 c[0] = (short)(((int)c[0] *
 775                                                (sens[0] + 128)) >> 8);
 776                                 c[1] = (short)(((int)c[1] *
 777                                                (sens[1] + 128)) >> 8);
 778                                 c[2] = (short)(((int)c[2] *
 779                                                (sens[2] + 128)) >> 8);
 780                                 st->raw_compass[0] = c[0];
 781                                 st->raw_compass[1] = c[1];
 782                                 st->raw_compass[2] = c[2];
 783                         }
 784                         st->compass_counter = 0;
 785                 } else if (compass_divider != 0) {
 786                         st->compass_counter++;
 787                 }
 788                 if (!conf->normal_compass_measure) {
 789                         c[0] = 0;
 790                         c[1] = 0;
 791                         c[2] = 0;
 792                         conf->normal_compass_measure = 1;
 793                 }
 794                 for (i = 0; i < 3; i++)
 795                         memcpy(&buf[ind + i * sizeof(c[i])], &c[i],
 796                                                 sizeof(c[i]));
 797                 ind += BYTES_PER_SENSOR;
 798         }
 799         tmp = (u64 *)buf;
 800         tmp[DIV_ROUND_UP(ind, 8)] = t;
 801
 802         if (ind > 0)
 803 #ifdef INV_KERNEL_3_10
 804                 iio_push_to_buffers(indio_dev, buf);
 805 #else
 806                 iio_push_to_buffer(indio_dev->buffer, buf, t);
 807 #endif
 808
 809         return 0;
 810 }
 811
 812 static void inv_process_motion(struct inv_mpu_iio_s *st)
 813 {
 814         struct iio_dev *indio_dev = iio_priv_to_dev(st);
 815         s32 diff, true_motion;
 816         s64 timestamp;
 817         int result;
 818         u8 data[1];
 819
 820         /* motion interrupt */
 821         result = inv_plat_read(st, REG_INT_STATUS, 1, data);
 822         if (result)
 823                 return;
 824
 825         if (data[0] & BIT_MOT_INT) {
 826                 timestamp = get_time_ns();
 827                 diff = (int)(((timestamp - st->mpu6500_last_motion_time) >>
 828                         NS_PER_MS_SHIFT));
 829                 if (diff > st->mot_int.mot_dur) {
 830                         st->mpu6500_last_motion_time = timestamp;
 831                         true_motion = 1;
 832                 } else {
 833                         true_motion = 0;
 834                 }
 835                 if (true_motion)
 836                         sysfs_notify(&indio_dev->dev.kobj, NULL,
 837                                         "event_accel_motion");
 838         }
 839 }
 840
 841 static int get_bytes_per_datum(struct inv_mpu_iio_s *st)
 842 {
 843         int bytes_per_datum;
 844
 845         bytes_per_datum = 0;
 846         if (st->chip_config.dmp_on) {
 847                 if (st->chip_config.quaternion_on)
 848                         bytes_per_datum += QUATERNION_BYTES;
 849                 if (st->chip_config.tap_on ||
 850                         st->chip_config.display_orient_on)
 851                         bytes_per_datum += BYTES_FOR_EVENTS;
 852         }
 853         if (st->chip_config.accl_fifo_enable)
 854                 bytes_per_datum += BYTES_PER_SENSOR;
 855         if (st->chip_config.gyro_fifo_enable)
 856                 bytes_per_datum += BYTES_PER_SENSOR;
 857
 858         return bytes_per_datum;
 859 }
 860
 861 /**
 862  *  inv_read_fifo() - Transfer data from FIFO to ring buffer.
 863  */
 864 irqreturn_t inv_read_fifo(int irq, void *dev_id)
 865 {
 866
 867         struct inv_mpu_iio_s *st = (struct inv_mpu_iio_s *)dev_id;
 868         struct iio_dev *indio_dev = iio_priv_to_dev(st);
 869         size_t bytes_per_datum;
 870         int result;
 871         u8 data[BYTES_FOR_DMP + QUATERNION_BYTES];
 872         u16 fifo_count;
 873         u32 copied;
 874         s64 timestamp;
 875         struct inv_reg_map_s *reg;
 876         s64 buf[8];
 877         s8 *tmp;
 878
 879         mutex_lock(&indio_dev->mlock);
 880         if (!(iio_buffer_enabled(indio_dev)))
 881                 goto end_session;
 882
 883         reg = &st->reg;
 884         if (!(st->chip_config.accl_fifo_enable |
 885                 st->chip_config.gyro_fifo_enable |
 886                 st->chip_config.dmp_on |
 887                 st->chip_config.compass_fifo_enable |
 888                 st->mot_int.mot_on))
 889                 goto end_session;
 890         if (st->mot_int.mot_on)
 891                 inv_process_motion(st);
 892         if (st->chip_config.dmp_on && st->chip_config.smd_enable) {
 893                 /* dmp interrupt status */
 894                 result = inv_plat_read(st, REG_DMP_INT_STATUS, 1, data);
 895                 if (!result)
 896                         if (data[0] & SMD_INT_ON) {
 897                                 sysfs_notify(&indio_dev->dev.kobj, NULL,
 898                                                 "event_smd");
 899                                 st->chip_config.smd_enable = 0;
 900                         }
 901         }
 902         if (st->chip_config.lpa_mode) {
 903                 result = inv_plat_read(st, reg->raw_accl,
 904                                       BYTES_PER_SENSOR, data);
 905                 if (result)
 906                         goto end_session;
 907                 inv_report_gyro_accl_compass(indio_dev, data,
 908                                              get_time_ns());
 909                 goto end_session;
 910         }
 911         bytes_per_datum = get_bytes_per_datum(st);
 912         fifo_count = 0;
 913         if (bytes_per_datum != 0) {
 914                 result = inv_plat_read(st, reg->fifo_count_h,
 915                                 FIFO_COUNT_BYTE, data);
 916                 if (result)
 917                         goto end_session;
 918                 fifo_count = be16_to_cpup((__be16 *)(&data[0]));
 919                 if (fifo_count == 0)
 920                         goto flush_fifo;
 921                 if (fifo_count < bytes_per_datum)
 922                         goto end_session;
 923                 /* fifo count can't be odd number */
 924                 if (fifo_count & 1)
 925                         goto flush_fifo;
 926                 if (fifo_count >  FIFO_THRESHOLD)
 927                         goto flush_fifo;
 928                 /* timestamp mismatch. */
 929                 if (kfifo_len(&st->timestamps) <
 930                         fifo_count / bytes_per_datum)
 931                         goto flush_fifo;
 932                 if (kfifo_len(&st->timestamps) >
 933                         fifo_count / bytes_per_datum + TIME_STAMP_TOR) {
 934                         if (st->chip_config.dmp_on) {
 935                                 result = kfifo_to_user(&st->timestamps,
 936                                 &timestamp, sizeof(timestamp), &copied);
 937                                 if (result)
 938                                         goto flush_fifo;
 939                         } else {
 940                                 goto flush_fifo;
 941                         }
 942                 }
 943         } else {
 944                 result = kfifo_to_user(&st->timestamps,
 945                         &timestamp, sizeof(timestamp), &copied);
 946                 if (result)
 947                         goto flush_fifo;
 948         }
 949         tmp = (s8 *)buf;
 950         while ((bytes_per_datum != 0) && (fifo_count >= bytes_per_datum)) {
 951                 result = inv_plat_read(st, reg->fifo_r_w, bytes_per_datum,
 952                         data);
 953                 if (result)
 954                         goto flush_fifo;
 955
 956                 result = kfifo_to_user(&st->timestamps,
 957                         &timestamp, sizeof(timestamp), &copied);
 958                 if (result)
 959                         goto flush_fifo;
 960                 inv_report_gyro_accl_compass(indio_dev, data, timestamp);
 961                 fifo_count -= bytes_per_datum;
 962         }
 963         if (bytes_per_datum == 0 && st->chip_config.compass_fifo_enable)
 964                 inv_report_gyro_accl_compass(indio_dev, data, timestamp);
 965
 966 end_session:
 967         mutex_unlock(&indio_dev->mlock);
 968
 969         return IRQ_HANDLED;
 970
 971 flush_fifo:
 972         /* Flush HW and SW FIFOs. */
 973         inv_reset_fifo(indio_dev);
 974         inv_clear_kfifo(st);
 975         mutex_unlock(&indio_dev->mlock);
 976
 977         return IRQ_HANDLED;
 978 }
 979
 980 void inv_mpu_unconfigure_ring(struct iio_dev *indio_dev)
 981 {
 982         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
 983         free_irq(st->irq, st);
 984         iio_kfifo_free(indio_dev->buffer);
 985 };
 986
 987 static int inv_postenable(struct iio_dev *indio_dev)
 988 {
 989         return set_inv_enable(indio_dev, true);
 990 }
 991
 992 static int inv_predisable(struct iio_dev *indio_dev)
 993 {
 994         return set_inv_enable(indio_dev, false);
 995 }
 996
 997 static void inv_scan_query(struct iio_dev *indio_dev)
 998 {
 999         struct inv_mpu_iio_s  *st = iio_priv(indio_dev);
1000         struct iio_buffer *ring = indio_dev->buffer;
1001         int result;
1002
1003         if (iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_GYRO_X) ||
1004             iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_GYRO_Y) ||
1005             iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_GYRO_Z))
1006                 st->chip_config.gyro_fifo_enable = 1;
1007         else
1008                 st->chip_config.gyro_fifo_enable = 0;
1009
1010         if (iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_ACCL_X) ||
1011             iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_ACCL_Y) ||
1012             iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_ACCL_Z))
1013                 st->chip_config.accl_fifo_enable = 1;
1014         else
1015                 st->chip_config.accl_fifo_enable = 0;
1016
1017         if (iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_MAGN_X) ||
1018             iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_MAGN_Y) ||
1019             iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_MAGN_Z))
1020                 st->chip_config.compass_fifo_enable = 1;
1021         else
1022                 st->chip_config.compass_fifo_enable = 0;
1023
1024         /* check to make sure engine is turned on if fifo is turned on */
1025         if (st->chip_config.gyro_fifo_enable &&
1026                 (!st->chip_config.gyro_enable)) {
1027                 result = st->switch_gyro_engine(st, true);
1028                 if (result)
1029                         return;
1030                 st->chip_config.gyro_enable = true;
1031         }
1032         if (st->chip_config.accl_fifo_enable &&
1033                 (!st->chip_config.accl_enable)) {
1034                 result = st->switch_accl_engine(st, true);
1035                 if (result)
1036                         return;
1037                 st->chip_config.accl_enable = true;
1038         }
1039 }
1040
1041 static int inv_check_quaternion(struct iio_dev *indio_dev)
1042 {
1043         struct inv_mpu_iio_s  *st = iio_priv(indio_dev);
1044         struct iio_buffer *ring = indio_dev->buffer;
1045         int result;
1046
1047         if (st->chip_config.dmp_on) {
1048                 if (
1049                   iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_R) ||
1050                   iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_X) ||
1051                   iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_Y) ||
1052                   iio_scan_mask_query(indio_dev, ring, INV_MPU_SCAN_QUAT_Z))
1053                         st->chip_config.quaternion_on = 1;
1054                 else
1055                         st->chip_config.quaternion_on = 0;
1056
1057                 result = inv_send_quaternion(st,
1058                                 st->chip_config.quaternion_on);
1059                 if (result)
1060                         return result;
1061         } else {
1062                 st->chip_config.quaternion_on = 0;
1063                 clear_bit(INV_MPU_SCAN_QUAT_R, ring->scan_mask);
1064                 clear_bit(INV_MPU_SCAN_QUAT_X, ring->scan_mask);
1065                 clear_bit(INV_MPU_SCAN_QUAT_Y, ring->scan_mask);
1066                 clear_bit(INV_MPU_SCAN_QUAT_Z, ring->scan_mask);
1067         }
1068
1069         return 0;
1070 }
1071
1072 static int inv_check_conflict_sysfs(struct iio_dev *indio_dev)
1073 {
1074         struct inv_mpu_iio_s  *st = iio_priv(indio_dev);
1075         struct iio_buffer *ring = indio_dev->buffer;
1076         int result;
1077
1078         if (st->chip_config.lpa_mode) {
1079                 /* dmp cannot run with low power mode on */
1080                 st->chip_config.dmp_on = 0;
1081                 result = st->gyro_en(st, ring, false);
1082                 if (result)
1083                         return result;
1084                 result = st->compass_en(st, ring, false);
1085                 if (result)
1086                         return result;
1087                 result = st->quaternion_en(st, ring, false);
1088                 if (result)
1089                         return result;
1090
1091                 result = st->accl_en(st, ring, true);
1092                 if (result)
1093                         return result;
1094         }
1095         result = inv_check_quaternion(indio_dev);
1096         if (result)
1097                 return result;
1098
1099         return result;
1100 }
1101
1102 static int inv_preenable(struct iio_dev *indio_dev)
1103 {
1104         int result;
1105         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1106
1107         result = st->set_power_state(st, true);
1108         if (result)
1109                 return result;
1110
1111         result = inv_check_conflict_sysfs(indio_dev);
1112         if (result)
1113                 return result;
1114         inv_scan_query(indio_dev);
1115
1116         return result;
1117 }
1118
1119 static const struct iio_buffer_setup_ops inv_mpu_ring_setup_ops = {
1120         .preenable = &inv_preenable,
1121         .postenable = &inv_postenable,
1122         .predisable = &inv_predisable,
1123 };
1124
1125 int inv_mpu_configure_ring(struct iio_dev *indio_dev)
1126 {
1127         int ret;
1128         struct inv_mpu_iio_s *st = iio_priv(indio_dev);
1129         struct iio_buffer *ring;
1130
1131         ring = iio_kfifo_allocate();
1132         if (!ring)
1133                 return -ENOMEM;
1134         indio_dev->buffer = ring;
1135         /* setup ring buffer */
1136         ring->scan_timestamp = true;
1137         indio_dev->setup_ops = &inv_mpu_ring_setup_ops;
1138         /*scan count double count timestamp. should subtract 1. but
1139         number of channels still includes timestamp*/
1140         if (INV_MPU3050 == st->chip_type)
1141                 ret = request_threaded_irq(st->irq, inv_irq_handler,
1142                         inv_read_fifo_mpu3050,
1143                         IRQF_TRIGGER_RISING | IRQF_SHARED, "inv_irq", st);
1144         else
1145                 ret = request_threaded_irq(st->irq, inv_irq_handler,
1146                         inv_read_fifo,
1147                         IRQF_TRIGGER_RISING | IRQF_SHARED, "inv_irq", st);
1148         if (ret)
1149                 goto error_iio_sw_rb_free;
1150
1151         indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
1152         return 0;
1153 error_iio_sw_rb_free:
1154         iio_kfifo_free(indio_dev->buffer);
1155         return ret;
1156 }
1157
1158 /**
1159  *  @}
1160  */
1161