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#include <linux/module.h> 15#include <linux/slab.h> 16#include <linux/i2c.h> 17#include <linux/err.h> 18#include <linux/delay.h> 19#include <linux/sysfs.h> 20#include <linux/jiffies.h> 21#include <linux/irq.h> 22#include <linux/interrupt.h> 23#include <linux/kfifo.h> 24#include <linux/spinlock.h> 25#include <linux/iio/iio.h> 26#include "inv_mpu_iio.h" 27 28/* 29 * this is the gyro scale translated from dynamic range plus/minus 30 * {250, 500, 1000, 2000} to rad/s 31 */ 32static const int gyro_scale_6050[] = {133090, 266181, 532362, 1064724}; 33 34/* 35 * this is the accel scale translated from dynamic range plus/minus 36 * {2, 4, 8, 16} to m/s^2 37 */ 38static const int accel_scale[] = {598, 1196, 2392, 4785}; 39 40static const struct inv_mpu6050_reg_map reg_set_6050 = { 41 .sample_rate_div = INV_MPU6050_REG_SAMPLE_RATE_DIV, 42 .lpf = INV_MPU6050_REG_CONFIG, 43 .user_ctrl = INV_MPU6050_REG_USER_CTRL, 44 .fifo_en = INV_MPU6050_REG_FIFO_EN, 45 .gyro_config = INV_MPU6050_REG_GYRO_CONFIG, 46 .accl_config = INV_MPU6050_REG_ACCEL_CONFIG, 47 .fifo_count_h = INV_MPU6050_REG_FIFO_COUNT_H, 48 .fifo_r_w = INV_MPU6050_REG_FIFO_R_W, 49 .raw_gyro = INV_MPU6050_REG_RAW_GYRO, 50 .raw_accl = INV_MPU6050_REG_RAW_ACCEL, 51 .temperature = INV_MPU6050_REG_TEMPERATURE, 52 .int_enable = INV_MPU6050_REG_INT_ENABLE, 53 .pwr_mgmt_1 = INV_MPU6050_REG_PWR_MGMT_1, 54 .pwr_mgmt_2 = INV_MPU6050_REG_PWR_MGMT_2, 55}; 56 57static const struct inv_mpu6050_chip_config chip_config_6050 = { 58 .fsr = INV_MPU6050_FSR_2000DPS, 59 .lpf = INV_MPU6050_FILTER_20HZ, 60 .fifo_rate = INV_MPU6050_INIT_FIFO_RATE, 61 .gyro_fifo_enable = false, 62 .accl_fifo_enable = false, 63 .accl_fs = INV_MPU6050_FS_02G, 64}; 65 66static const struct inv_mpu6050_hw hw_info[INV_NUM_PARTS] = { 67 { 68 .num_reg = 117, 69 .name = "MPU6050", 70 .reg = ®_set_6050, 71 .config = &chip_config_6050, 72 }, 73}; 74 75int inv_mpu6050_write_reg(struct inv_mpu6050_state *st, int reg, u8 d) 76{ 77 return i2c_smbus_write_i2c_block_data(st->client, reg, 1, &d); 78} 79 80int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask) 81{ 82 u8 d, mgmt_1; 83 int result; 84 85 /* switch clock needs to be careful. Only when gyro is on, can 86 clock source be switched to gyro. Otherwise, it must be set to 87 internal clock */ 88 if (INV_MPU6050_BIT_PWR_GYRO_STBY == mask) { 89 result = i2c_smbus_read_i2c_block_data(st->client, 90 st->reg->pwr_mgmt_1, 1, &mgmt_1); 91 if (result != 1) 92 return result; 93 94 mgmt_1 &= ~INV_MPU6050_BIT_CLK_MASK; 95 } 96 97 if ((INV_MPU6050_BIT_PWR_GYRO_STBY == mask) && (!en)) { 98 /* turning off gyro requires switch to internal clock first. 99 Then turn off gyro engine */ 100 mgmt_1 |= INV_CLK_INTERNAL; 101 result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_1, mgmt_1); 102 if (result) 103 return result; 104 } 105 106 result = i2c_smbus_read_i2c_block_data(st->client, 107 st->reg->pwr_mgmt_2, 1, &d); 108 if (result != 1) 109 return result; 110 if (en) 111 d &= ~mask; 112 else 113 d |= mask; 114 result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_2, d); 115 if (result) 116 return result; 117 118 if (en) { 119 /* Wait for output stabilize */ 120 msleep(INV_MPU6050_TEMP_UP_TIME); 121 if (INV_MPU6050_BIT_PWR_GYRO_STBY == mask) { 122 /* switch internal clock to PLL */ 123 mgmt_1 |= INV_CLK_PLL; 124 result = inv_mpu6050_write_reg(st, 125 st->reg->pwr_mgmt_1, mgmt_1); 126 if (result) 127 return result; 128 } 129 } 130 131 return 0; 132} 133 134int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on) 135{ 136 int result; 137 138 if (power_on) 139 result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_1, 0); 140 else 141 result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_1, 142 INV_MPU6050_BIT_SLEEP); 143 if (result) 144 return result; 145 146 if (power_on) 147 msleep(INV_MPU6050_REG_UP_TIME); 148 149 return 0; 150} 151 152/** 153 * inv_mpu6050_init_config() - Initialize hardware, disable FIFO. 154 * 155 * Initial configuration: 156 * FSR: ± 2000DPS 157 * DLPF: 20Hz 158 * FIFO rate: 50Hz 159 * Clock source: Gyro PLL 160 */ 161static int inv_mpu6050_init_config(struct iio_dev *indio_dev) 162{ 163 int result; 164 u8 d; 165 struct inv_mpu6050_state *st = iio_priv(indio_dev); 166 167 result = inv_mpu6050_set_power_itg(st, true); 168 if (result) 169 return result; 170 d = (INV_MPU6050_FSR_2000DPS << INV_MPU6050_GYRO_CONFIG_FSR_SHIFT); 171 result = inv_mpu6050_write_reg(st, st->reg->gyro_config, d); 172 if (result) 173 return result; 174 175 d = INV_MPU6050_FILTER_20HZ; 176 result = inv_mpu6050_write_reg(st, st->reg->lpf, d); 177 if (result) 178 return result; 179 180 d = INV_MPU6050_ONE_K_HZ / INV_MPU6050_INIT_FIFO_RATE - 1; 181 result = inv_mpu6050_write_reg(st, st->reg->sample_rate_div, d); 182 if (result) 183 return result; 184 185 d = (INV_MPU6050_FS_02G << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT); 186 result = inv_mpu6050_write_reg(st, st->reg->accl_config, d); 187 if (result) 188 return result; 189 190 memcpy(&st->chip_config, hw_info[st->chip_type].config, 191 sizeof(struct inv_mpu6050_chip_config)); 192 result = inv_mpu6050_set_power_itg(st, false); 193 194 return result; 195} 196 197static int inv_mpu6050_sensor_show(struct inv_mpu6050_state *st, int reg, 198 int axis, int *val) 199{ 200 int ind, result; 201 __be16 d; 202 203 ind = (axis - IIO_MOD_X) * 2; 204 result = i2c_smbus_read_i2c_block_data(st->client, reg + ind, 2, 205 (u8 *)&d); 206 if (result != 2) 207 return -EINVAL; 208 *val = (short)be16_to_cpup(&d); 209 210 return IIO_VAL_INT; 211} 212 213static int inv_mpu6050_read_raw(struct iio_dev *indio_dev, 214 struct iio_chan_spec const *chan, 215 int *val, 216 int *val2, 217 long mask) { 218 struct inv_mpu6050_state *st = iio_priv(indio_dev); 219 220 switch (mask) { 221 case IIO_CHAN_INFO_RAW: 222 { 223 int ret, result; 224 225 ret = IIO_VAL_INT; 226 result = 0; 227 mutex_lock(&indio_dev->mlock); 228 if (!st->chip_config.enable) { 229 result = inv_mpu6050_set_power_itg(st, true); 230 if (result) 231 goto error_read_raw; 232 } 233 /* when enable is on, power is already on */ 234 switch (chan->type) { 235 case IIO_ANGL_VEL: 236 if (!st->chip_config.gyro_fifo_enable || 237 !st->chip_config.enable) { 238 result = inv_mpu6050_switch_engine(st, true, 239 INV_MPU6050_BIT_PWR_GYRO_STBY); 240 if (result) 241 goto error_read_raw; 242 } 243 ret = inv_mpu6050_sensor_show(st, st->reg->raw_gyro, 244 chan->channel2, val); 245 if (!st->chip_config.gyro_fifo_enable || 246 !st->chip_config.enable) { 247 result = inv_mpu6050_switch_engine(st, false, 248 INV_MPU6050_BIT_PWR_GYRO_STBY); 249 if (result) 250 goto error_read_raw; 251 } 252 break; 253 case IIO_ACCEL: 254 if (!st->chip_config.accl_fifo_enable || 255 !st->chip_config.enable) { 256 result = inv_mpu6050_switch_engine(st, true, 257 INV_MPU6050_BIT_PWR_ACCL_STBY); 258 if (result) 259 goto error_read_raw; 260 } 261 ret = inv_mpu6050_sensor_show(st, st->reg->raw_accl, 262 chan->channel2, val); 263 if (!st->chip_config.accl_fifo_enable || 264 !st->chip_config.enable) { 265 result = inv_mpu6050_switch_engine(st, false, 266 INV_MPU6050_BIT_PWR_ACCL_STBY); 267 if (result) 268 goto error_read_raw; 269 } 270 break; 271 case IIO_TEMP: 272 /* wait for stablization */ 273 msleep(INV_MPU6050_SENSOR_UP_TIME); 274 inv_mpu6050_sensor_show(st, st->reg->temperature, 275 IIO_MOD_X, val); 276 break; 277 default: 278 ret = -EINVAL; 279 break; 280 } 281error_read_raw: 282 if (!st->chip_config.enable) 283 result |= inv_mpu6050_set_power_itg(st, false); 284 mutex_unlock(&indio_dev->mlock); 285 if (result) 286 return result; 287 288 return ret; 289 } 290 case IIO_CHAN_INFO_SCALE: 291 switch (chan->type) { 292 case IIO_ANGL_VEL: 293 *val = 0; 294 *val2 = gyro_scale_6050[st->chip_config.fsr]; 295 296 return IIO_VAL_INT_PLUS_NANO; 297 case IIO_ACCEL: 298 *val = 0; 299 *val2 = accel_scale[st->chip_config.accl_fs]; 300 301 return IIO_VAL_INT_PLUS_MICRO; 302 case IIO_TEMP: 303 *val = 0; 304 *val2 = INV_MPU6050_TEMP_SCALE; 305 306 return IIO_VAL_INT_PLUS_MICRO; 307 default: 308 return -EINVAL; 309 } 310 case IIO_CHAN_INFO_OFFSET: 311 switch (chan->type) { 312 case IIO_TEMP: 313 *val = INV_MPU6050_TEMP_OFFSET; 314 315 return IIO_VAL_INT; 316 default: 317 return -EINVAL; 318 } 319 default: 320 return -EINVAL; 321 } 322} 323 324static int inv_mpu6050_write_fsr(struct inv_mpu6050_state *st, int fsr) 325{ 326 int result; 327 u8 d; 328 329 if (fsr < 0 || fsr > INV_MPU6050_MAX_GYRO_FS_PARAM) 330 return -EINVAL; 331 if (fsr == st->chip_config.fsr) 332 return 0; 333 334 d = (fsr << INV_MPU6050_GYRO_CONFIG_FSR_SHIFT); 335 result = inv_mpu6050_write_reg(st, st->reg->gyro_config, d); 336 if (result) 337 return result; 338 st->chip_config.fsr = fsr; 339 340 return 0; 341} 342 343static int inv_mpu6050_write_accel_fs(struct inv_mpu6050_state *st, int fs) 344{ 345 int result; 346 u8 d; 347 348 if (fs < 0 || fs > INV_MPU6050_MAX_ACCL_FS_PARAM) 349 return -EINVAL; 350 if (fs == st->chip_config.accl_fs) 351 return 0; 352 353 d = (fs << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT); 354 result = inv_mpu6050_write_reg(st, st->reg->accl_config, d); 355 if (result) 356 return result; 357 st->chip_config.accl_fs = fs; 358 359 return 0; 360} 361 362static int inv_mpu6050_write_raw(struct iio_dev *indio_dev, 363 struct iio_chan_spec const *chan, 364 int val, 365 int val2, 366 long mask) { 367 struct inv_mpu6050_state *st = iio_priv(indio_dev); 368 int result; 369 370 mutex_lock(&indio_dev->mlock); 371 /* we should only update scale when the chip is disabled, i.e., 372 not running */ 373 if (st->chip_config.enable) { 374 result = -EBUSY; 375 goto error_write_raw; 376 } 377 result = inv_mpu6050_set_power_itg(st, true); 378 if (result) 379 goto error_write_raw; 380 381 switch (mask) { 382 case IIO_CHAN_INFO_SCALE: 383 switch (chan->type) { 384 case IIO_ANGL_VEL: 385 result = inv_mpu6050_write_fsr(st, val); 386 break; 387 case IIO_ACCEL: 388 result = inv_mpu6050_write_accel_fs(st, val); 389 break; 390 default: 391 result = -EINVAL; 392 break; 393 } 394 break; 395 default: 396 result = -EINVAL; 397 break; 398 } 399 400error_write_raw: 401 result |= inv_mpu6050_set_power_itg(st, false); 402 mutex_unlock(&indio_dev->mlock); 403 404 return result; 405} 406 407/** 408 * inv_mpu6050_set_lpf() - set low pass filer based on fifo rate. 409 * 410 * Based on the Nyquist principle, the sampling rate must 411 * exceed twice of the bandwidth of the signal, or there 412 * would be alising. This function basically search for the 413 * correct low pass parameters based on the fifo rate, e.g, 414 * sampling frequency. 415 */ 416static int inv_mpu6050_set_lpf(struct inv_mpu6050_state *st, int rate) 417{ 418 const int hz[] = {188, 98, 42, 20, 10, 5}; 419 const int d[] = {INV_MPU6050_FILTER_188HZ, INV_MPU6050_FILTER_98HZ, 420 INV_MPU6050_FILTER_42HZ, INV_MPU6050_FILTER_20HZ, 421 INV_MPU6050_FILTER_10HZ, INV_MPU6050_FILTER_5HZ}; 422 int i, h, result; 423 u8 data; 424 425 h = (rate >> 1); 426 i = 0; 427 while ((h < hz[i]) && (i < ARRAY_SIZE(d) - 1)) 428 i++; 429 data = d[i]; 430 result = inv_mpu6050_write_reg(st, st->reg->lpf, data); 431 if (result) 432 return result; 433 st->chip_config.lpf = data; 434 435 return 0; 436} 437 438/** 439 * inv_mpu6050_fifo_rate_store() - Set fifo rate. 440 */ 441static ssize_t inv_mpu6050_fifo_rate_store(struct device *dev, 442 struct device_attribute *attr, const char *buf, size_t count) 443{ 444 s32 fifo_rate; 445 u8 d; 446 int result; 447 struct iio_dev *indio_dev = dev_to_iio_dev(dev); 448 struct inv_mpu6050_state *st = iio_priv(indio_dev); 449 450 if (kstrtoint(buf, 10, &fifo_rate)) 451 return -EINVAL; 452 if (fifo_rate < INV_MPU6050_MIN_FIFO_RATE || 453 fifo_rate > INV_MPU6050_MAX_FIFO_RATE) 454 return -EINVAL; 455 if (fifo_rate == st->chip_config.fifo_rate) 456 return count; 457 458 mutex_lock(&indio_dev->mlock); 459 if (st->chip_config.enable) { 460 result = -EBUSY; 461 goto fifo_rate_fail; 462 } 463 result = inv_mpu6050_set_power_itg(st, true); 464 if (result) 465 goto fifo_rate_fail; 466 467 d = INV_MPU6050_ONE_K_HZ / fifo_rate - 1; 468 result = inv_mpu6050_write_reg(st, st->reg->sample_rate_div, d); 469 if (result) 470 goto fifo_rate_fail; 471 st->chip_config.fifo_rate = fifo_rate; 472 473 result = inv_mpu6050_set_lpf(st, fifo_rate); 474 if (result) 475 goto fifo_rate_fail; 476 477fifo_rate_fail: 478 result |= inv_mpu6050_set_power_itg(st, false); 479 mutex_unlock(&indio_dev->mlock); 480 if (result) 481 return result; 482 483 return count; 484} 485 486/** 487 * inv_fifo_rate_show() - Get the current sampling rate. 488 */ 489static ssize_t inv_fifo_rate_show(struct device *dev, 490 struct device_attribute *attr, char *buf) 491{ 492 struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev)); 493 494 return sprintf(buf, "%d\n", st->chip_config.fifo_rate); 495} 496 497/** 498 * inv_attr_show() - calling this function will show current 499 * parameters. 500 */ 501static ssize_t inv_attr_show(struct device *dev, 502 struct device_attribute *attr, char *buf) 503{ 504 struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev)); 505 struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); 506 s8 *m; 507 508 switch (this_attr->address) { 509 /* In MPU6050, the two matrix are the same because gyro and accel 510 are integrated in one chip */ 511 case ATTR_GYRO_MATRIX: 512 case ATTR_ACCL_MATRIX: 513 m = st->plat_data.orientation; 514 515 return sprintf(buf, "%d, %d, %d; %d, %d, %d; %d, %d, %d\n", 516 m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); 517 default: 518 return -EINVAL; 519 } 520} 521 522/** 523 * inv_mpu6050_validate_trigger() - validate_trigger callback for invensense 524 * MPU6050 device. 525 * @indio_dev: The IIO device 526 * @trig: The new trigger 527 * 528 * Returns: 0 if the 'trig' matches the trigger registered by the MPU6050 529 * device, -EINVAL otherwise. 530 */ 531static int inv_mpu6050_validate_trigger(struct iio_dev *indio_dev, 532 struct iio_trigger *trig) 533{ 534 struct inv_mpu6050_state *st = iio_priv(indio_dev); 535 536 if (st->trig != trig) 537 return -EINVAL; 538 539 return 0; 540} 541 542#define INV_MPU6050_CHAN(_type, _channel2, _index) \ 543 { \ 544 .type = _type, \ 545 .modified = 1, \ 546 .channel2 = _channel2, \ 547 .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \ 548 .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \ 549 .scan_index = _index, \ 550 .scan_type = { \ 551 .sign = 's', \ 552 .realbits = 16, \ 553 .storagebits = 16, \ 554 .shift = 0 , \ 555 .endianness = IIO_BE, \ 556 }, \ 557 } 558 559static const struct iio_chan_spec inv_mpu_channels[] = { 560 IIO_CHAN_SOFT_TIMESTAMP(INV_MPU6050_SCAN_TIMESTAMP), 561 /* 562 * Note that temperature should only be via polled reading only, 563 * not the final scan elements output. 564 */ 565 { 566 .type = IIO_TEMP, 567 .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) 568 | BIT(IIO_CHAN_INFO_OFFSET) 569 | BIT(IIO_CHAN_INFO_SCALE), 570 .scan_index = -1, 571 }, 572 INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_X, INV_MPU6050_SCAN_GYRO_X), 573 INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Y, INV_MPU6050_SCAN_GYRO_Y), 574 INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Z, INV_MPU6050_SCAN_GYRO_Z), 575 576 INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_X, INV_MPU6050_SCAN_ACCL_X), 577 INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Y, INV_MPU6050_SCAN_ACCL_Y), 578 INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Z, INV_MPU6050_SCAN_ACCL_Z), 579}; 580 581/* constant IIO attribute */ 582static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("10 20 50 100 200 500"); 583static IIO_DEV_ATTR_SAMP_FREQ(S_IRUGO | S_IWUSR, inv_fifo_rate_show, 584 inv_mpu6050_fifo_rate_store); 585static IIO_DEVICE_ATTR(in_gyro_matrix, S_IRUGO, inv_attr_show, NULL, 586 ATTR_GYRO_MATRIX); 587static IIO_DEVICE_ATTR(in_accel_matrix, S_IRUGO, inv_attr_show, NULL, 588 ATTR_ACCL_MATRIX); 589 590static struct attribute *inv_attributes[] = { 591 &iio_dev_attr_in_gyro_matrix.dev_attr.attr, 592 &iio_dev_attr_in_accel_matrix.dev_attr.attr, 593 &iio_dev_attr_sampling_frequency.dev_attr.attr, 594 &iio_const_attr_sampling_frequency_available.dev_attr.attr, 595 NULL, 596}; 597 598static const struct attribute_group inv_attribute_group = { 599 .attrs = inv_attributes 600}; 601 602static const struct iio_info mpu_info = { 603 .driver_module = THIS_MODULE, 604 .read_raw = &inv_mpu6050_read_raw, 605 .write_raw = &inv_mpu6050_write_raw, 606 .attrs = &inv_attribute_group, 607 .validate_trigger = inv_mpu6050_validate_trigger, 608}; 609 610/** 611 * inv_check_and_setup_chip() - check and setup chip. 612 */ 613static int inv_check_and_setup_chip(struct inv_mpu6050_state *st, 614 const struct i2c_device_id *id) 615{ 616 int result; 617 618 st->chip_type = INV_MPU6050; 619 st->hw = &hw_info[st->chip_type]; 620 st->reg = hw_info[st->chip_type].reg; 621 622 /* reset to make sure previous state are not there */ 623 result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_1, 624 INV_MPU6050_BIT_H_RESET); 625 if (result) 626 return result; 627 msleep(INV_MPU6050_POWER_UP_TIME); 628 /* toggle power state. After reset, the sleep bit could be on 629 or off depending on the OTP settings. Toggling power would 630 make it in a definite state as well as making the hardware 631 state align with the software state */ 632 result = inv_mpu6050_set_power_itg(st, false); 633 if (result) 634 return result; 635 result = inv_mpu6050_set_power_itg(st, true); 636 if (result) 637 return result; 638 639 result = inv_mpu6050_switch_engine(st, false, 640 INV_MPU6050_BIT_PWR_ACCL_STBY); 641 if (result) 642 return result; 643 result = inv_mpu6050_switch_engine(st, false, 644 INV_MPU6050_BIT_PWR_GYRO_STBY); 645 if (result) 646 return result; 647 648 return 0; 649} 650 651/** 652 * inv_mpu_probe() - probe function. 653 * @client: i2c client. 654 * @id: i2c device id. 655 * 656 * Returns 0 on success, a negative error code otherwise. 657 */ 658static int inv_mpu_probe(struct i2c_client *client, 659 const struct i2c_device_id *id) 660{ 661 struct inv_mpu6050_state *st; 662 struct iio_dev *indio_dev; 663 struct inv_mpu6050_platform_data *pdata; 664 int result; 665 666 if (!i2c_check_functionality(client->adapter, 667 I2C_FUNC_SMBUS_I2C_BLOCK)) 668 return -ENOSYS; 669 670 indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*st)); 671 if (!indio_dev) 672 return -ENOMEM; 673 674 st = iio_priv(indio_dev); 675 st->client = client; 676 pdata = dev_get_platdata(&client->dev); 677 if (pdata) 678 st->plat_data = *pdata; 679 /* power is turned on inside check chip type*/ 680 result = inv_check_and_setup_chip(st, id); 681 if (result) 682 return result; 683 684 result = inv_mpu6050_init_config(indio_dev); 685 if (result) { 686 dev_err(&client->dev, 687 "Could not initialize device.\n"); 688 return result; 689 } 690 691 i2c_set_clientdata(client, indio_dev); 692 indio_dev->dev.parent = &client->dev; 693 indio_dev->name = id->name; 694 indio_dev->channels = inv_mpu_channels; 695 indio_dev->num_channels = ARRAY_SIZE(inv_mpu_channels); 696 697 indio_dev->info = &mpu_info; 698 indio_dev->modes = INDIO_BUFFER_TRIGGERED; 699 700 result = iio_triggered_buffer_setup(indio_dev, 701 inv_mpu6050_irq_handler, 702 inv_mpu6050_read_fifo, 703 NULL); 704 if (result) { 705 dev_err(&st->client->dev, "configure buffer fail %d\n", 706 result); 707 return result; 708 } 709 result = inv_mpu6050_probe_trigger(indio_dev); 710 if (result) { 711 dev_err(&st->client->dev, "trigger probe fail %d\n", result); 712 goto out_unreg_ring; 713 } 714 715 INIT_KFIFO(st->timestamps); 716 spin_lock_init(&st->time_stamp_lock); 717 result = iio_device_register(indio_dev); 718 if (result) { 719 dev_err(&st->client->dev, "IIO register fail %d\n", result); 720 goto out_remove_trigger; 721 } 722 723 return 0; 724 725out_remove_trigger: 726 inv_mpu6050_remove_trigger(st); 727out_unreg_ring: 728 iio_triggered_buffer_cleanup(indio_dev); 729 return result; 730} 731 732static int inv_mpu_remove(struct i2c_client *client) 733{ 734 struct iio_dev *indio_dev = i2c_get_clientdata(client); 735 struct inv_mpu6050_state *st = iio_priv(indio_dev); 736 737 iio_device_unregister(indio_dev); 738 inv_mpu6050_remove_trigger(st); 739 iio_triggered_buffer_cleanup(indio_dev); 740 741 return 0; 742} 743#ifdef CONFIG_PM_SLEEP 744 745static int inv_mpu_resume(struct device *dev) 746{ 747 return inv_mpu6050_set_power_itg( 748 iio_priv(i2c_get_clientdata(to_i2c_client(dev))), true); 749} 750 751static int inv_mpu_suspend(struct device *dev) 752{ 753 return inv_mpu6050_set_power_itg( 754 iio_priv(i2c_get_clientdata(to_i2c_client(dev))), false); 755} 756static SIMPLE_DEV_PM_OPS(inv_mpu_pmops, inv_mpu_suspend, inv_mpu_resume); 757 758#define INV_MPU6050_PMOPS (&inv_mpu_pmops) 759#else 760#define INV_MPU6050_PMOPS NULL 761#endif /* CONFIG_PM_SLEEP */ 762 763/* 764 * device id table is used to identify what device can be 765 * supported by this driver 766 */ 767static const struct i2c_device_id inv_mpu_id[] = { 768 {"mpu6050", INV_MPU6050}, 769 {"mpu6500", INV_MPU6500}, 770 {} 771}; 772 773MODULE_DEVICE_TABLE(i2c, inv_mpu_id); 774 775static struct i2c_driver inv_mpu_driver = { 776 .probe = inv_mpu_probe, 777 .remove = inv_mpu_remove, 778 .id_table = inv_mpu_id, 779 .driver = { 780 .owner = THIS_MODULE, 781 .name = "inv-mpu6050", 782 .pm = INV_MPU6050_PMOPS, 783 }, 784}; 785 786module_i2c_driver(inv_mpu_driver); 787 788MODULE_AUTHOR("Invensense Corporation"); 789MODULE_DESCRIPTION("Invensense device MPU6050 driver"); 790MODULE_LICENSE("GPL"); 791