FILE
kmx61.c
File preview with syntax highlighting.
linux-6.19.14/drivers/iio/imu/kmx61.c
1// SPDX-License-Identifier: GPL-2.0-only 2/* 3 * KMX61 - Kionix 6-axis Accelerometer/Magnetometer 4 * 5 * Copyright (c) 2014, Intel Corporation. 6 * 7 * IIO driver for KMX61 (7-bit I2C slave address 0x0E or 0x0F). 8 */ 9 10#include <linux/i2c.h> 11#include <linux/interrupt.h> 12#include <linux/mod_devicetable.h> 13#include <linux/module.h> 14#include <linux/pm.h> 15#include <linux/pm_runtime.h> 16 17#include <linux/iio/iio.h> 18#include <linux/iio/sysfs.h> 19#include <linux/iio/events.h> 20#include <linux/iio/trigger.h> 21#include <linux/iio/buffer.h> 22#include <linux/iio/triggered_buffer.h> 23#include <linux/iio/trigger_consumer.h> 24 25#define KMX61_REG_WHO_AM_I 0x00 26#define KMX61_REG_INS1 0x01 27#define KMX61_REG_INS2 0x02 28 29/* 30 * three 16-bit accelerometer output registers for X/Y/Z axis 31 * we use only XOUT_L as a base register, all other addresses 32 * can be obtained by applying an offset and are provided here 33 * only for clarity. 34 */ 35#define KMX61_ACC_XOUT_L 0x0A 36#define KMX61_ACC_XOUT_H 0x0B 37#define KMX61_ACC_YOUT_L 0x0C 38#define KMX61_ACC_YOUT_H 0x0D 39#define KMX61_ACC_ZOUT_L 0x0E 40#define KMX61_ACC_ZOUT_H 0x0F 41 42/* 43 * one 16-bit temperature output register 44 */ 45#define KMX61_TEMP_L 0x10 46#define KMX61_TEMP_H 0x11 47 48/* 49 * three 16-bit magnetometer output registers for X/Y/Z axis 50 */ 51#define KMX61_MAG_XOUT_L 0x12 52#define KMX61_MAG_XOUT_H 0x13 53#define KMX61_MAG_YOUT_L 0x14 54#define KMX61_MAG_YOUT_H 0x15 55#define KMX61_MAG_ZOUT_L 0x16 56#define KMX61_MAG_ZOUT_H 0x17 57 58#define KMX61_REG_INL 0x28 59#define KMX61_REG_STBY 0x29 60#define KMX61_REG_CTRL1 0x2A 61#define KMX61_REG_CTRL2 0x2B 62#define KMX61_REG_ODCNTL 0x2C 63#define KMX61_REG_INC1 0x2D 64 65#define KMX61_REG_WUF_THRESH 0x3D 66#define KMX61_REG_WUF_TIMER 0x3E 67 68#define KMX61_ACC_STBY_BIT BIT(0) 69#define KMX61_MAG_STBY_BIT BIT(1) 70#define KMX61_ACT_STBY_BIT BIT(7) 71 72#define KMX61_ALL_STBY (KMX61_ACC_STBY_BIT | KMX61_MAG_STBY_BIT) 73 74#define KMX61_REG_INS1_BIT_WUFS BIT(1) 75 76#define KMX61_REG_INS2_BIT_ZP BIT(0) 77#define KMX61_REG_INS2_BIT_ZN BIT(1) 78#define KMX61_REG_INS2_BIT_YP BIT(2) 79#define KMX61_REG_INS2_BIT_YN BIT(3) 80#define KMX61_REG_INS2_BIT_XP BIT(4) 81#define KMX61_REG_INS2_BIT_XN BIT(5) 82 83#define KMX61_REG_CTRL1_GSEL_MASK 0x03 84 85#define KMX61_REG_CTRL1_BIT_RES BIT(4) 86#define KMX61_REG_CTRL1_BIT_DRDYE BIT(5) 87#define KMX61_REG_CTRL1_BIT_WUFE BIT(6) 88#define KMX61_REG_CTRL1_BIT_BTSE BIT(7) 89 90#define KMX61_REG_INC1_BIT_WUFS BIT(0) 91#define KMX61_REG_INC1_BIT_DRDYM BIT(1) 92#define KMX61_REG_INC1_BIT_DRDYA BIT(2) 93#define KMX61_REG_INC1_BIT_IEN BIT(5) 94 95#define KMX61_ACC_ODR_SHIFT 0 96#define KMX61_MAG_ODR_SHIFT 4 97#define KMX61_ACC_ODR_MASK 0x0F 98#define KMX61_MAG_ODR_MASK 0xF0 99 100#define KMX61_OWUF_MASK 0x7 101 102#define KMX61_DEFAULT_WAKE_THRESH 1 103#define KMX61_DEFAULT_WAKE_DURATION 1 104 105#define KMX61_SLEEP_DELAY_MS 2000 106 107#define KMX61_CHIP_ID 0x12 108 109/* KMX61 devices */ 110#define KMX61_ACC 0x01 111#define KMX61_MAG 0x02 112 113struct kmx61_data { 114 struct i2c_client *client; 115 116 /* serialize access to non-atomic ops, e.g set_mode */ 117 struct mutex lock; 118 119 /* standby state */ 120 bool acc_stby; 121 bool mag_stby; 122 123 /* power state */ 124 bool acc_ps; 125 bool mag_ps; 126 127 /* config bits */ 128 u8 range; 129 u8 odr_bits; 130 u8 wake_thresh; 131 u8 wake_duration; 132 133 /* accelerometer specific data */ 134 struct iio_dev *acc_indio_dev; 135 struct iio_trigger *acc_dready_trig; 136 struct iio_trigger *motion_trig; 137 bool acc_dready_trig_on; 138 bool motion_trig_on; 139 bool ev_enable_state; 140 141 /* magnetometer specific data */ 142 struct iio_dev *mag_indio_dev; 143 struct iio_trigger *mag_dready_trig; 144 bool mag_dready_trig_on; 145}; 146 147enum kmx61_range { 148 KMX61_RANGE_2G, 149 KMX61_RANGE_4G, 150 KMX61_RANGE_8G, 151}; 152 153enum kmx61_axis { 154 KMX61_AXIS_X, 155 KMX61_AXIS_Y, 156 KMX61_AXIS_Z, 157}; 158 159static const u16 kmx61_uscale_table[] = {9582, 19163, 38326}; 160 161static const struct { 162 int val; 163 int val2; 164} kmx61_samp_freq_table[] = { {12, 500000}, 165 {25, 0}, 166 {50, 0}, 167 {100, 0}, 168 {200, 0}, 169 {400, 0}, 170 {800, 0}, 171 {1600, 0}, 172 {0, 781000}, 173 {1, 563000}, 174 {3, 125000}, 175 {6, 250000} }; 176 177static const struct { 178 int val; 179 int val2; 180 int odr_bits; 181} kmx61_wake_up_odr_table[] = { {0, 781000, 0x00}, 182 {1, 563000, 0x01}, 183 {3, 125000, 0x02}, 184 {6, 250000, 0x03}, 185 {12, 500000, 0x04}, 186 {25, 0, 0x05}, 187 {50, 0, 0x06}, 188 {100, 0, 0x06}, 189 {200, 0, 0x06}, 190 {400, 0, 0x06}, 191 {800, 0, 0x06}, 192 {1600, 0, 0x06} }; 193 194static IIO_CONST_ATTR(accel_scale_available, "0.009582 0.019163 0.038326"); 195static IIO_CONST_ATTR(magn_scale_available, "0.001465"); 196static IIO_CONST_ATTR_SAMP_FREQ_AVAIL( 197 "0.781000 1.563000 3.125000 6.250000 12.500000 25 50 100 200 400 800"); 198 199static struct attribute *kmx61_acc_attributes[] = { 200 &iio_const_attr_accel_scale_available.dev_attr.attr, 201 &iio_const_attr_sampling_frequency_available.dev_attr.attr, 202 NULL, 203}; 204 205static struct attribute *kmx61_mag_attributes[] = { 206 &iio_const_attr_magn_scale_available.dev_attr.attr, 207 &iio_const_attr_sampling_frequency_available.dev_attr.attr, 208 NULL, 209}; 210 211static const struct attribute_group kmx61_acc_attribute_group = { 212 .attrs = kmx61_acc_attributes, 213}; 214 215static const struct attribute_group kmx61_mag_attribute_group = { 216 .attrs = kmx61_mag_attributes, 217}; 218 219static const struct iio_event_spec kmx61_event = { 220 .type = IIO_EV_TYPE_THRESH, 221 .dir = IIO_EV_DIR_EITHER, 222 .mask_separate = BIT(IIO_EV_INFO_VALUE) | 223 BIT(IIO_EV_INFO_ENABLE) | 224 BIT(IIO_EV_INFO_PERIOD), 225}; 226 227#define KMX61_ACC_CHAN(_axis) { \ 228 .type = IIO_ACCEL, \ 229 .modified = 1, \ 230 .channel2 = IIO_MOD_ ## _axis, \ 231 .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \ 232 .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) | \ 233 BIT(IIO_CHAN_INFO_SAMP_FREQ), \ 234 .address = KMX61_ACC, \ 235 .scan_index = KMX61_AXIS_ ## _axis, \ 236 .scan_type = { \ 237 .sign = 's', \ 238 .realbits = 12, \ 239 .storagebits = 16, \ 240 .shift = 4, \ 241 .endianness = IIO_LE, \ 242 }, \ 243 .event_spec = &kmx61_event, \ 244 .num_event_specs = 1 \ 245} 246 247#define KMX61_MAG_CHAN(_axis) { \ 248 .type = IIO_MAGN, \ 249 .modified = 1, \ 250 .channel2 = IIO_MOD_ ## _axis, \ 251 .address = KMX61_MAG, \ 252 .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \ 253 .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) | \ 254 BIT(IIO_CHAN_INFO_SAMP_FREQ), \ 255 .scan_index = KMX61_AXIS_ ## _axis, \ 256 .scan_type = { \ 257 .sign = 's', \ 258 .realbits = 14, \ 259 .storagebits = 16, \ 260 .shift = 2, \ 261 .endianness = IIO_LE, \ 262 }, \ 263} 264 265static const struct iio_chan_spec kmx61_acc_channels[] = { 266 KMX61_ACC_CHAN(X), 267 KMX61_ACC_CHAN(Y), 268 KMX61_ACC_CHAN(Z), 269}; 270 271static const struct iio_chan_spec kmx61_mag_channels[] = { 272 KMX61_MAG_CHAN(X), 273 KMX61_MAG_CHAN(Y), 274 KMX61_MAG_CHAN(Z), 275}; 276 277static void kmx61_set_data(struct iio_dev *indio_dev, struct kmx61_data *data) 278{ 279 struct kmx61_data **priv = iio_priv(indio_dev); 280 281 *priv = data; 282} 283 284static struct kmx61_data *kmx61_get_data(struct iio_dev *indio_dev) 285{ 286 return *(struct kmx61_data **)iio_priv(indio_dev); 287} 288 289static int kmx61_convert_freq_to_bit(int val, int val2) 290{ 291 int i; 292 293 for (i = 0; i < ARRAY_SIZE(kmx61_samp_freq_table); i++) 294 if (val == kmx61_samp_freq_table[i].val && 295 val2 == kmx61_samp_freq_table[i].val2) 296 return i; 297 return -EINVAL; 298} 299 300static int kmx61_convert_wake_up_odr_to_bit(int val, int val2) 301{ 302 int i; 303 304 for (i = 0; i < ARRAY_SIZE(kmx61_wake_up_odr_table); ++i) 305 if (kmx61_wake_up_odr_table[i].val == val && 306 kmx61_wake_up_odr_table[i].val2 == val2) 307 return kmx61_wake_up_odr_table[i].odr_bits; 308 return -EINVAL; 309} 310 311/** 312 * kmx61_set_mode() - set KMX61 device operating mode 313 * @data: kmx61 device private data pointer 314 * @mode: bitmask, indicating operating mode for @device 315 * @device: bitmask, indicating device for which @mode needs to be set 316 * @update: update stby bits stored in device's private @data 317 * 318 * For each sensor (accelerometer/magnetometer) there are two operating modes 319 * STANDBY and OPERATION. Neither accel nor magn can be disabled independently 320 * if they are both enabled. Internal sensors state is saved in acc_stby and 321 * mag_stby members of driver's private @data. 322 */ 323static int kmx61_set_mode(struct kmx61_data *data, u8 mode, u8 device, 324 bool update) 325{ 326 int ret; 327 int acc_stby = -1, mag_stby = -1; 328 329 ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_STBY); 330 if (ret < 0) { 331 dev_err(&data->client->dev, "Error reading reg_stby\n"); 332 return ret; 333 } 334 if (device & KMX61_ACC) { 335 if (mode & KMX61_ACC_STBY_BIT) { 336 ret |= KMX61_ACC_STBY_BIT; 337 acc_stby = 1; 338 } else { 339 ret &= ~KMX61_ACC_STBY_BIT; 340 acc_stby = 0; 341 } 342 } 343 344 if (device & KMX61_MAG) { 345 if (mode & KMX61_MAG_STBY_BIT) { 346 ret |= KMX61_MAG_STBY_BIT; 347 mag_stby = 1; 348 } else { 349 ret &= ~KMX61_MAG_STBY_BIT; 350 mag_stby = 0; 351 } 352 } 353 354 if (mode & KMX61_ACT_STBY_BIT) 355 ret |= KMX61_ACT_STBY_BIT; 356 357 ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_STBY, ret); 358 if (ret < 0) { 359 dev_err(&data->client->dev, "Error writing reg_stby\n"); 360 return ret; 361 } 362 363 if (acc_stby != -1 && update) 364 data->acc_stby = acc_stby; 365 if (mag_stby != -1 && update) 366 data->mag_stby = mag_stby; 367 368 return 0; 369} 370 371static int kmx61_get_mode(struct kmx61_data *data, u8 *mode, u8 device) 372{ 373 int ret; 374 375 ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_STBY); 376 if (ret < 0) { 377 dev_err(&data->client->dev, "Error reading reg_stby\n"); 378 return ret; 379 } 380 *mode = 0; 381 382 if (device & KMX61_ACC) { 383 if (ret & KMX61_ACC_STBY_BIT) 384 *mode |= KMX61_ACC_STBY_BIT; 385 else 386 *mode &= ~KMX61_ACC_STBY_BIT; 387 } 388 389 if (device & KMX61_MAG) { 390 if (ret & KMX61_MAG_STBY_BIT) 391 *mode |= KMX61_MAG_STBY_BIT; 392 else 393 *mode &= ~KMX61_MAG_STBY_BIT; 394 } 395 396 return 0; 397} 398 399static int kmx61_set_wake_up_odr(struct kmx61_data *data, int val, int val2) 400{ 401 int ret, odr_bits; 402 403 odr_bits = kmx61_convert_wake_up_odr_to_bit(val, val2); 404 if (odr_bits < 0) 405 return odr_bits; 406 407 ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_CTRL2, 408 odr_bits); 409 if (ret < 0) 410 dev_err(&data->client->dev, "Error writing reg_ctrl2\n"); 411 return ret; 412} 413 414static int kmx61_set_odr(struct kmx61_data *data, int val, int val2, u8 device) 415{ 416 int ret; 417 u8 mode; 418 int lodr_bits, odr_bits; 419 420 ret = kmx61_get_mode(data, &mode, KMX61_ACC | KMX61_MAG); 421 if (ret < 0) 422 return ret; 423 424 lodr_bits = kmx61_convert_freq_to_bit(val, val2); 425 if (lodr_bits < 0) 426 return lodr_bits; 427 428 /* To change ODR, accel and magn must be in STDBY */ 429 ret = kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, 430 true); 431 if (ret < 0) 432 return ret; 433 434 odr_bits = 0; 435 if (device & KMX61_ACC) 436 odr_bits |= lodr_bits << KMX61_ACC_ODR_SHIFT; 437 if (device & KMX61_MAG) 438 odr_bits |= lodr_bits << KMX61_MAG_ODR_SHIFT; 439 440 ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_ODCNTL, 441 odr_bits); 442 if (ret < 0) 443 return ret; 444 445 data->odr_bits = odr_bits; 446 447 if (device & KMX61_ACC) { 448 ret = kmx61_set_wake_up_odr(data, val, val2); 449 if (ret) 450 return ret; 451 } 452 453 return kmx61_set_mode(data, mode, KMX61_ACC | KMX61_MAG, true); 454} 455 456static int kmx61_get_odr(struct kmx61_data *data, int *val, int *val2, 457 u8 device) 458{ 459 u8 lodr_bits; 460 461 if (device & KMX61_ACC) 462 lodr_bits = (data->odr_bits >> KMX61_ACC_ODR_SHIFT) & 463 KMX61_ACC_ODR_MASK; 464 else if (device & KMX61_MAG) 465 lodr_bits = (data->odr_bits >> KMX61_MAG_ODR_SHIFT) & 466 KMX61_MAG_ODR_MASK; 467 else 468 return -EINVAL; 469 470 if (lodr_bits >= ARRAY_SIZE(kmx61_samp_freq_table)) 471 return -EINVAL; 472 473 *val = kmx61_samp_freq_table[lodr_bits].val; 474 *val2 = kmx61_samp_freq_table[lodr_bits].val2; 475 476 return 0; 477} 478 479static int kmx61_set_range(struct kmx61_data *data, u8 range) 480{ 481 int ret; 482 483 ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_CTRL1); 484 if (ret < 0) { 485 dev_err(&data->client->dev, "Error reading reg_ctrl1\n"); 486 return ret; 487 } 488 489 ret &= ~KMX61_REG_CTRL1_GSEL_MASK; 490 ret |= range & KMX61_REG_CTRL1_GSEL_MASK; 491 492 ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_CTRL1, ret); 493 if (ret < 0) { 494 dev_err(&data->client->dev, "Error writing reg_ctrl1\n"); 495 return ret; 496 } 497 498 data->range = range; 499 500 return 0; 501} 502 503static int kmx61_set_scale(struct kmx61_data *data, u16 uscale) 504{ 505 int ret, i; 506 u8 mode; 507 508 for (i = 0; i < ARRAY_SIZE(kmx61_uscale_table); i++) { 509 if (kmx61_uscale_table[i] == uscale) { 510 ret = kmx61_get_mode(data, &mode, 511 KMX61_ACC | KMX61_MAG); 512 if (ret < 0) 513 return ret; 514 515 ret = kmx61_set_mode(data, KMX61_ALL_STBY, 516 KMX61_ACC | KMX61_MAG, true); 517 if (ret < 0) 518 return ret; 519 520 ret = kmx61_set_range(data, i); 521 if (ret < 0) 522 return ret; 523 524 return kmx61_set_mode(data, mode, 525 KMX61_ACC | KMX61_MAG, true); 526 } 527 } 528 return -EINVAL; 529} 530 531static int kmx61_chip_init(struct kmx61_data *data) 532{ 533 int ret, val, val2; 534 535 ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_WHO_AM_I); 536 if (ret < 0) { 537 dev_err(&data->client->dev, "Error reading who_am_i\n"); 538 return ret; 539 } 540 541 if (ret != KMX61_CHIP_ID) { 542 dev_err(&data->client->dev, 543 "Wrong chip id, got %x expected %x\n", 544 ret, KMX61_CHIP_ID); 545 return -EINVAL; 546 } 547 548 /* set accel 12bit, 4g range */ 549 ret = kmx61_set_range(data, KMX61_RANGE_4G); 550 if (ret < 0) 551 return ret; 552 553 ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_ODCNTL); 554 if (ret < 0) { 555 dev_err(&data->client->dev, "Error reading reg_odcntl\n"); 556 return ret; 557 } 558 data->odr_bits = ret; 559 560 /* 561 * set output data rate for wake up (motion detection) function 562 * to match data rate for accelerometer sampling 563 */ 564 ret = kmx61_get_odr(data, &val, &val2, KMX61_ACC); 565 if (ret < 0) 566 return ret; 567 568 ret = kmx61_set_wake_up_odr(data, val, val2); 569 if (ret < 0) 570 return ret; 571 572 /* set acc/magn to OPERATION mode */ 573 ret = kmx61_set_mode(data, 0, KMX61_ACC | KMX61_MAG, true); 574 if (ret < 0) 575 return ret; 576 577 data->wake_thresh = KMX61_DEFAULT_WAKE_THRESH; 578 data->wake_duration = KMX61_DEFAULT_WAKE_DURATION; 579 580 return 0; 581} 582 583static int kmx61_setup_new_data_interrupt(struct kmx61_data *data, 584 bool status, u8 device) 585{ 586 u8 mode; 587 int ret; 588 589 ret = kmx61_get_mode(data, &mode, KMX61_ACC | KMX61_MAG); 590 if (ret < 0) 591 return ret; 592 593 ret = kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, true); 594 if (ret < 0) 595 return ret; 596 597 ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_INC1); 598 if (ret < 0) { 599 dev_err(&data->client->dev, "Error reading reg_ctrl1\n"); 600 return ret; 601 } 602 603 if (status) { 604 ret |= KMX61_REG_INC1_BIT_IEN; 605 if (device & KMX61_ACC) 606 ret |= KMX61_REG_INC1_BIT_DRDYA; 607 if (device & KMX61_MAG) 608 ret |= KMX61_REG_INC1_BIT_DRDYM; 609 } else { 610 ret &= ~KMX61_REG_INC1_BIT_IEN; 611 if (device & KMX61_ACC) 612 ret &= ~KMX61_REG_INC1_BIT_DRDYA; 613 if (device & KMX61_MAG) 614 ret &= ~KMX61_REG_INC1_BIT_DRDYM; 615 } 616 ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_INC1, ret); 617 if (ret < 0) { 618 dev_err(&data->client->dev, "Error writing reg_int_ctrl1\n"); 619 return ret; 620 } 621 622 ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_CTRL1); 623 if (ret < 0) { 624 dev_err(&data->client->dev, "Error reading reg_ctrl1\n"); 625 return ret; 626 } 627 628 if (status) 629 ret |= KMX61_REG_CTRL1_BIT_DRDYE; 630 else 631 ret &= ~KMX61_REG_CTRL1_BIT_DRDYE; 632 633 ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_CTRL1, ret); 634 if (ret < 0) { 635 dev_err(&data->client->dev, "Error writing reg_ctrl1\n"); 636 return ret; 637 } 638 639 return kmx61_set_mode(data, mode, KMX61_ACC | KMX61_MAG, true); 640} 641 642static int kmx61_chip_update_thresholds(struct kmx61_data *data) 643{ 644 int ret; 645 646 ret = i2c_smbus_write_byte_data(data->client, 647 KMX61_REG_WUF_TIMER, 648 data->wake_duration); 649 if (ret < 0) { 650 dev_err(&data->client->dev, "Error writing reg_wuf_timer\n"); 651 return ret; 652 } 653 654 ret = i2c_smbus_write_byte_data(data->client, 655 KMX61_REG_WUF_THRESH, 656 data->wake_thresh); 657 if (ret < 0) 658 dev_err(&data->client->dev, "Error writing reg_wuf_thresh\n"); 659 660 return ret; 661} 662 663static int kmx61_setup_any_motion_interrupt(struct kmx61_data *data, 664 bool status) 665{ 666 u8 mode; 667 int ret; 668 669 ret = kmx61_get_mode(data, &mode, KMX61_ACC | KMX61_MAG); 670 if (ret < 0) 671 return ret; 672 673 ret = kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, true); 674 if (ret < 0) 675 return ret; 676 677 ret = kmx61_chip_update_thresholds(data); 678 if (ret < 0) 679 return ret; 680 681 ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_INC1); 682 if (ret < 0) { 683 dev_err(&data->client->dev, "Error reading reg_inc1\n"); 684 return ret; 685 } 686 if (status) 687 ret |= (KMX61_REG_INC1_BIT_IEN | KMX61_REG_INC1_BIT_WUFS); 688 else 689 ret &= ~(KMX61_REG_INC1_BIT_IEN | KMX61_REG_INC1_BIT_WUFS); 690 691 ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_INC1, ret); 692 if (ret < 0) { 693 dev_err(&data->client->dev, "Error writing reg_inc1\n"); 694 return ret; 695 } 696 697 ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_CTRL1); 698 if (ret < 0) { 699 dev_err(&data->client->dev, "Error reading reg_ctrl1\n"); 700 return ret; 701 } 702 703 if (status) 704 ret |= KMX61_REG_CTRL1_BIT_WUFE | KMX61_REG_CTRL1_BIT_BTSE; 705 else 706 ret &= ~(KMX61_REG_CTRL1_BIT_WUFE | KMX61_REG_CTRL1_BIT_BTSE); 707 708 ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_CTRL1, ret); 709 if (ret < 0) { 710 dev_err(&data->client->dev, "Error writing reg_ctrl1\n"); 711 return ret; 712 } 713 mode |= KMX61_ACT_STBY_BIT; 714 return kmx61_set_mode(data, mode, KMX61_ACC | KMX61_MAG, true); 715} 716 717/** 718 * kmx61_set_power_state() - set power state for kmx61 @device 719 * @data: kmx61 device private pointer 720 * @on: power state to be set for @device 721 * @device: bitmask indicating device for which @on state needs to be set 722 * 723 * Notice that when ACC power state needs to be set to ON and MAG is in 724 * OPERATION then we know that kmx61_runtime_resume was already called 725 * so we must set ACC OPERATION mode here. The same happens when MAG power 726 * state needs to be set to ON and ACC is in OPERATION. 727 */ 728static int kmx61_set_power_state(struct kmx61_data *data, bool on, u8 device) 729{ 730#ifdef CONFIG_PM 731 int ret; 732 733 if (device & KMX61_ACC) { 734 if (on && !data->acc_ps && !data->mag_stby) { 735 ret = kmx61_set_mode(data, 0, KMX61_ACC, true); 736 if (ret < 0) 737 return ret; 738 } 739 data->acc_ps = on; 740 } 741 if (device & KMX61_MAG) { 742 if (on && !data->mag_ps && !data->acc_stby) { 743 ret = kmx61_set_mode(data, 0, KMX61_MAG, true); 744 if (ret < 0) 745 return ret; 746 } 747 data->mag_ps = on; 748 } 749 750 if (on) 751 ret = pm_runtime_resume_and_get(&data->client->dev); 752 else 753 ret = pm_runtime_put_autosuspend(&data->client->dev); 754 if (ret < 0) { 755 dev_err(&data->client->dev, 756 "Failed: kmx61_set_power_state for %d, ret %d\n", 757 on, ret); 758 759 return ret; 760 } 761#endif 762 return 0; 763} 764 765static int kmx61_read_measurement(struct kmx61_data *data, u8 base, u8 offset) 766{ 767 int ret; 768 u8 reg = base + offset * 2; 769 770 ret = i2c_smbus_read_word_data(data->client, reg); 771 if (ret < 0) 772 dev_err(&data->client->dev, "failed to read reg at %x\n", reg); 773 774 return ret; 775} 776 777static int kmx61_read_raw(struct iio_dev *indio_dev, 778 struct iio_chan_spec const *chan, int *val, 779 int *val2, long mask) 780{ 781 int ret; 782 u8 base_reg; 783 struct kmx61_data *data = kmx61_get_data(indio_dev); 784 785 switch (mask) { 786 case IIO_CHAN_INFO_RAW: 787 switch (chan->type) { 788 case IIO_ACCEL: 789 base_reg = KMX61_ACC_XOUT_L; 790 break; 791 case IIO_MAGN: 792 base_reg = KMX61_MAG_XOUT_L; 793 break; 794 default: 795 return -EINVAL; 796 } 797 mutex_lock(&data->lock); 798 799 ret = kmx61_set_power_state(data, true, chan->address); 800 if (ret) { 801 mutex_unlock(&data->lock); 802 return ret; 803 } 804 805 ret = kmx61_read_measurement(data, base_reg, chan->scan_index); 806 if (ret < 0) { 807 kmx61_set_power_state(data, false, chan->address); 808 mutex_unlock(&data->lock); 809 return ret; 810 } 811 *val = sign_extend32(ret >> chan->scan_type.shift, 812 chan->scan_type.realbits - 1); 813 ret = kmx61_set_power_state(data, false, chan->address); 814 815 mutex_unlock(&data->lock); 816 if (ret) 817 return ret; 818 return IIO_VAL_INT; 819 case IIO_CHAN_INFO_SCALE: 820 switch (chan->type) { 821 case IIO_ACCEL: 822 *val = 0; 823 *val2 = kmx61_uscale_table[data->range]; 824 return IIO_VAL_INT_PLUS_MICRO; 825 case IIO_MAGN: 826 /* 14 bits res, 1465 microGauss per magn count */ 827 *val = 0; 828 *val2 = 1465; 829 return IIO_VAL_INT_PLUS_MICRO; 830 default: 831 return -EINVAL; 832 } 833 case IIO_CHAN_INFO_SAMP_FREQ: 834 if (chan->type != IIO_ACCEL && chan->type != IIO_MAGN) 835 return -EINVAL; 836 837 mutex_lock(&data->lock); 838 ret = kmx61_get_odr(data, val, val2, chan->address); 839 mutex_unlock(&data->lock); 840 if (ret) 841 return -EINVAL; 842 return IIO_VAL_INT_PLUS_MICRO; 843 } 844 return -EINVAL; 845} 846 847static int kmx61_write_raw(struct iio_dev *indio_dev, 848 struct iio_chan_spec const *chan, int val, 849 int val2, long mask) 850{ 851 int ret; 852 struct kmx61_data *data = kmx61_get_data(indio_dev); 853 854 switch (mask) { 855 case IIO_CHAN_INFO_SAMP_FREQ: 856 if (chan->type != IIO_ACCEL && chan->type != IIO_MAGN) 857 return -EINVAL; 858 859 mutex_lock(&data->lock); 860 ret = kmx61_set_odr(data, val, val2, chan->address); 861 mutex_unlock(&data->lock); 862 return ret; 863 case IIO_CHAN_INFO_SCALE: 864 switch (chan->type) { 865 case IIO_ACCEL: 866 if (val != 0) 867 return -EINVAL; 868 mutex_lock(&data->lock); 869 ret = kmx61_set_scale(data, val2); 870 mutex_unlock(&data->lock); 871 return ret; 872 default: 873 return -EINVAL; 874 } 875 default: 876 return -EINVAL; 877 } 878} 879 880static int kmx61_read_event(struct iio_dev *indio_dev, 881 const struct iio_chan_spec *chan, 882 enum iio_event_type type, 883 enum iio_event_direction dir, 884 enum iio_event_info info, 885 int *val, int *val2) 886{ 887 struct kmx61_data *data = kmx61_get_data(indio_dev); 888 889 *val2 = 0; 890 switch (info) { 891 case IIO_EV_INFO_VALUE: 892 *val = data->wake_thresh; 893 return IIO_VAL_INT; 894 case IIO_EV_INFO_PERIOD: 895 *val = data->wake_duration; 896 return IIO_VAL_INT; 897 default: 898 return -EINVAL; 899 } 900} 901 902static int kmx61_write_event(struct iio_dev *indio_dev, 903 const struct iio_chan_spec *chan, 904 enum iio_event_type type, 905 enum iio_event_direction dir, 906 enum iio_event_info info, 907 int val, int val2) 908{ 909 struct kmx61_data *data = kmx61_get_data(indio_dev); 910 911 if (data->ev_enable_state) 912 return -EBUSY; 913 914 switch (info) { 915 case IIO_EV_INFO_VALUE: 916 data->wake_thresh = val; 917 return IIO_VAL_INT; 918 case IIO_EV_INFO_PERIOD: 919 data->wake_duration = val; 920 return IIO_VAL_INT; 921 default: 922 return -EINVAL; 923 } 924} 925 926static int kmx61_read_event_config(struct iio_dev *indio_dev, 927 const struct iio_chan_spec *chan, 928 enum iio_event_type type, 929 enum iio_event_direction dir) 930{ 931 struct kmx61_data *data = kmx61_get_data(indio_dev); 932 933 return data->ev_enable_state; 934} 935 936static int kmx61_write_event_config(struct iio_dev *indio_dev, 937 const struct iio_chan_spec *chan, 938 enum iio_event_type type, 939 enum iio_event_direction dir, 940 bool state) 941{ 942 struct kmx61_data *data = kmx61_get_data(indio_dev); 943 int ret = 0; 944 945 if (state && data->ev_enable_state) 946 return 0; 947 948 mutex_lock(&data->lock); 949 950 if (!state && data->motion_trig_on) { 951 data->ev_enable_state = false; 952 goto err_unlock; 953 } 954 955 ret = kmx61_set_power_state(data, state, KMX61_ACC); 956 if (ret < 0) 957 goto err_unlock; 958 959 ret = kmx61_setup_any_motion_interrupt(data, state); 960 if (ret < 0) { 961 kmx61_set_power_state(data, false, KMX61_ACC); 962 goto err_unlock; 963 } 964 965 data->ev_enable_state = state; 966 967err_unlock: 968 mutex_unlock(&data->lock); 969 970 return ret; 971} 972 973static int kmx61_acc_validate_trigger(struct iio_dev *indio_dev, 974 struct iio_trigger *trig) 975{ 976 struct kmx61_data *data = kmx61_get_data(indio_dev); 977 978 if (data->acc_dready_trig != trig && data->motion_trig != trig) 979 return -EINVAL; 980 981 return 0; 982} 983 984static int kmx61_mag_validate_trigger(struct iio_dev *indio_dev, 985 struct iio_trigger *trig) 986{ 987 struct kmx61_data *data = kmx61_get_data(indio_dev); 988 989 if (data->mag_dready_trig != trig) 990 return -EINVAL; 991 992 return 0; 993} 994 995static const struct iio_info kmx61_acc_info = { 996 .read_raw = kmx61_read_raw, 997 .write_raw = kmx61_write_raw, 998 .attrs = &kmx61_acc_attribute_group, 999 .read_event_value = kmx61_read_event, 1000 .write_event_value = kmx61_write_event, 1001 .read_event_config = kmx61_read_event_config, 1002 .write_event_config = kmx61_write_event_config, 1003 .validate_trigger = kmx61_acc_validate_trigger, 1004}; 1005 1006static const struct iio_info kmx61_mag_info = { 1007 .read_raw = kmx61_read_raw, 1008 .write_raw = kmx61_write_raw, 1009 .attrs = &kmx61_mag_attribute_group, 1010 .validate_trigger = kmx61_mag_validate_trigger, 1011}; 1012 1013 1014static int kmx61_data_rdy_trigger_set_state(struct iio_trigger *trig, 1015 bool state) 1016{ 1017 int ret = 0; 1018 u8 device; 1019 1020 struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig); 1021 struct kmx61_data *data = kmx61_get_data(indio_dev); 1022 1023 mutex_lock(&data->lock); 1024 1025 if (!state && data->ev_enable_state && data->motion_trig_on) { 1026 data->motion_trig_on = false; 1027 goto err_unlock; 1028 } 1029 1030 if (data->acc_dready_trig == trig || data->motion_trig == trig) 1031 device = KMX61_ACC; 1032 else 1033 device = KMX61_MAG; 1034 1035 ret = kmx61_set_power_state(data, state, device); 1036 if (ret < 0) 1037 goto err_unlock; 1038 1039 if (data->acc_dready_trig == trig || data->mag_dready_trig == trig) 1040 ret = kmx61_setup_new_data_interrupt(data, state, device); 1041 else 1042 ret = kmx61_setup_any_motion_interrupt(data, state); 1043 if (ret < 0) { 1044 kmx61_set_power_state(data, false, device); 1045 goto err_unlock; 1046 } 1047 1048 if (data->acc_dready_trig == trig) 1049 data->acc_dready_trig_on = state; 1050 else if (data->mag_dready_trig == trig) 1051 data->mag_dready_trig_on = state; 1052 else 1053 data->motion_trig_on = state; 1054err_unlock: 1055 mutex_unlock(&data->lock); 1056 1057 return ret; 1058} 1059 1060static void kmx61_trig_reenable(struct iio_trigger *trig) 1061{ 1062 struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig); 1063 struct kmx61_data *data = kmx61_get_data(indio_dev); 1064 int ret; 1065 1066 ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_INL); 1067 if (ret < 0) 1068 dev_err(&data->client->dev, "Error reading reg_inl\n"); 1069} 1070 1071static const struct iio_trigger_ops kmx61_trigger_ops = { 1072 .set_trigger_state = kmx61_data_rdy_trigger_set_state, 1073 .reenable = kmx61_trig_reenable, 1074}; 1075 1076static irqreturn_t kmx61_event_handler(int irq, void *private) 1077{ 1078 struct kmx61_data *data = private; 1079 struct iio_dev *indio_dev = data->acc_indio_dev; 1080 int ret; 1081 1082 ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_INS1); 1083 if (ret < 0) { 1084 dev_err(&data->client->dev, "Error reading reg_ins1\n"); 1085 goto ack_intr; 1086 } 1087 1088 if (ret & KMX61_REG_INS1_BIT_WUFS) { 1089 ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_INS2); 1090 if (ret < 0) { 1091 dev_err(&data->client->dev, "Error reading reg_ins2\n"); 1092 goto ack_intr; 1093 } 1094 1095 if (ret & KMX61_REG_INS2_BIT_XN) 1096 iio_push_event(indio_dev, 1097 IIO_MOD_EVENT_CODE(IIO_ACCEL, 1098 0, 1099 IIO_MOD_X, 1100 IIO_EV_TYPE_THRESH, 1101 IIO_EV_DIR_FALLING), 1102 0); 1103 1104 if (ret & KMX61_REG_INS2_BIT_XP) 1105 iio_push_event(indio_dev, 1106 IIO_MOD_EVENT_CODE(IIO_ACCEL, 1107 0, 1108 IIO_MOD_X, 1109 IIO_EV_TYPE_THRESH, 1110 IIO_EV_DIR_RISING), 1111 0); 1112 1113 if (ret & KMX61_REG_INS2_BIT_YN) 1114 iio_push_event(indio_dev, 1115 IIO_MOD_EVENT_CODE(IIO_ACCEL, 1116 0, 1117 IIO_MOD_Y, 1118 IIO_EV_TYPE_THRESH, 1119 IIO_EV_DIR_FALLING), 1120 0); 1121 1122 if (ret & KMX61_REG_INS2_BIT_YP) 1123 iio_push_event(indio_dev, 1124 IIO_MOD_EVENT_CODE(IIO_ACCEL, 1125 0, 1126 IIO_MOD_Y, 1127 IIO_EV_TYPE_THRESH, 1128 IIO_EV_DIR_RISING), 1129 0); 1130 1131 if (ret & KMX61_REG_INS2_BIT_ZN) 1132 iio_push_event(indio_dev, 1133 IIO_MOD_EVENT_CODE(IIO_ACCEL, 1134 0, 1135 IIO_MOD_Z, 1136 IIO_EV_TYPE_THRESH, 1137 IIO_EV_DIR_FALLING), 1138 0); 1139 1140 if (ret & KMX61_REG_INS2_BIT_ZP) 1141 iio_push_event(indio_dev, 1142 IIO_MOD_EVENT_CODE(IIO_ACCEL, 1143 0, 1144 IIO_MOD_Z, 1145 IIO_EV_TYPE_THRESH, 1146 IIO_EV_DIR_RISING), 1147 0); 1148 } 1149 1150ack_intr: 1151 ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_CTRL1); 1152 if (ret < 0) 1153 dev_err(&data->client->dev, "Error reading reg_ctrl1\n"); 1154 1155 ret |= KMX61_REG_CTRL1_BIT_RES; 1156 ret = i2c_smbus_write_byte_data(data->client, KMX61_REG_CTRL1, ret); 1157 if (ret < 0) 1158 dev_err(&data->client->dev, "Error writing reg_ctrl1\n"); 1159 1160 ret = i2c_smbus_read_byte_data(data->client, KMX61_REG_INL); 1161 if (ret < 0) 1162 dev_err(&data->client->dev, "Error reading reg_inl\n"); 1163 1164 return IRQ_HANDLED; 1165} 1166 1167static irqreturn_t kmx61_data_rdy_trig_poll(int irq, void *private) 1168{ 1169 struct kmx61_data *data = private; 1170 1171 if (data->acc_dready_trig_on) 1172 iio_trigger_poll(data->acc_dready_trig); 1173 if (data->mag_dready_trig_on) 1174 iio_trigger_poll(data->mag_dready_trig); 1175 1176 if (data->motion_trig_on) 1177 iio_trigger_poll(data->motion_trig); 1178 1179 if (data->ev_enable_state) 1180 return IRQ_WAKE_THREAD; 1181 return IRQ_HANDLED; 1182} 1183 1184static irqreturn_t kmx61_trigger_handler(int irq, void *p) 1185{ 1186 struct iio_poll_func *pf = p; 1187 struct iio_dev *indio_dev = pf->indio_dev; 1188 struct kmx61_data *data = kmx61_get_data(indio_dev); 1189 int bit, ret, i = 0; 1190 u8 base; 1191 s16 buffer[8] = { }; 1192 1193 if (indio_dev == data->acc_indio_dev) 1194 base = KMX61_ACC_XOUT_L; 1195 else 1196 base = KMX61_MAG_XOUT_L; 1197 1198 mutex_lock(&data->lock); 1199 iio_for_each_active_channel(indio_dev, bit) { 1200 ret = kmx61_read_measurement(data, base, bit); 1201 if (ret < 0) { 1202 mutex_unlock(&data->lock); 1203 goto err; 1204 } 1205 buffer[i++] = ret; 1206 } 1207 mutex_unlock(&data->lock); 1208 1209 iio_push_to_buffers(indio_dev, buffer); 1210err: 1211 iio_trigger_notify_done(indio_dev->trig); 1212 1213 return IRQ_HANDLED; 1214} 1215 1216static struct iio_dev *kmx61_indiodev_setup(struct kmx61_data *data, 1217 const struct iio_info *info, 1218 const struct iio_chan_spec *chan, 1219 int num_channels, 1220 const char *name) 1221{ 1222 struct iio_dev *indio_dev; 1223 1224 indio_dev = devm_iio_device_alloc(&data->client->dev, sizeof(data)); 1225 if (!indio_dev) 1226 return ERR_PTR(-ENOMEM); 1227 1228 kmx61_set_data(indio_dev, data); 1229 1230 indio_dev->channels = chan; 1231 indio_dev->num_channels = num_channels; 1232 indio_dev->name = name; 1233 indio_dev->modes = INDIO_DIRECT_MODE; 1234 indio_dev->info = info; 1235 1236 return indio_dev; 1237} 1238 1239static struct iio_trigger *kmx61_trigger_setup(struct kmx61_data *data, 1240 struct iio_dev *indio_dev, 1241 const char *tag) 1242{ 1243 struct iio_trigger *trig; 1244 int ret; 1245 1246 trig = devm_iio_trigger_alloc(&data->client->dev, 1247 "%s-%s-dev%d", 1248 indio_dev->name, 1249 tag, 1250 iio_device_id(indio_dev)); 1251 if (!trig) 1252 return ERR_PTR(-ENOMEM); 1253 1254 trig->ops = &kmx61_trigger_ops; 1255 iio_trigger_set_drvdata(trig, indio_dev); 1256 1257 ret = iio_trigger_register(trig); 1258 if (ret) 1259 return ERR_PTR(ret); 1260 1261 return trig; 1262} 1263 1264static int kmx61_probe(struct i2c_client *client) 1265{ 1266 const struct i2c_device_id *id = i2c_client_get_device_id(client); 1267 int ret; 1268 struct kmx61_data *data; 1269 const char *name = NULL; 1270 1271 data = devm_kzalloc(&client->dev, sizeof(*data), GFP_KERNEL); 1272 if (!data) 1273 return -ENOMEM; 1274 1275 i2c_set_clientdata(client, data); 1276 data->client = client; 1277 1278 mutex_init(&data->lock); 1279 1280 if (id) 1281 name = id->name; 1282 else 1283 return -ENODEV; 1284 1285 data->acc_indio_dev = 1286 kmx61_indiodev_setup(data, &kmx61_acc_info, 1287 kmx61_acc_channels, 1288 ARRAY_SIZE(kmx61_acc_channels), 1289 name); 1290 if (IS_ERR(data->acc_indio_dev)) 1291 return PTR_ERR(data->acc_indio_dev); 1292 1293 data->mag_indio_dev = 1294 kmx61_indiodev_setup(data, &kmx61_mag_info, 1295 kmx61_mag_channels, 1296 ARRAY_SIZE(kmx61_mag_channels), 1297 name); 1298 if (IS_ERR(data->mag_indio_dev)) 1299 return PTR_ERR(data->mag_indio_dev); 1300 1301 ret = kmx61_chip_init(data); 1302 if (ret < 0) 1303 return ret; 1304 1305 if (client->irq > 0) { 1306 ret = devm_request_threaded_irq(&client->dev, client->irq, 1307 kmx61_data_rdy_trig_poll, 1308 kmx61_event_handler, 1309 IRQF_TRIGGER_RISING, 1310 "kmx61_event", 1311 data); 1312 if (ret) 1313 goto err_chip_uninit; 1314 1315 data->acc_dready_trig = 1316 kmx61_trigger_setup(data, data->acc_indio_dev, 1317 "dready"); 1318 if (IS_ERR(data->acc_dready_trig)) { 1319 ret = PTR_ERR(data->acc_dready_trig); 1320 goto err_chip_uninit; 1321 } 1322 1323 data->mag_dready_trig = 1324 kmx61_trigger_setup(data, data->mag_indio_dev, 1325 "dready"); 1326 if (IS_ERR(data->mag_dready_trig)) { 1327 ret = PTR_ERR(data->mag_dready_trig); 1328 goto err_trigger_unregister_acc_dready; 1329 } 1330 1331 data->motion_trig = 1332 kmx61_trigger_setup(data, data->acc_indio_dev, 1333 "any-motion"); 1334 if (IS_ERR(data->motion_trig)) { 1335 ret = PTR_ERR(data->motion_trig); 1336 goto err_trigger_unregister_mag_dready; 1337 } 1338 1339 ret = iio_triggered_buffer_setup(data->acc_indio_dev, 1340 &iio_pollfunc_store_time, 1341 kmx61_trigger_handler, 1342 NULL); 1343 if (ret < 0) { 1344 dev_err(&data->client->dev, 1345 "Failed to setup acc triggered buffer\n"); 1346 goto err_trigger_unregister_motion; 1347 } 1348 1349 ret = iio_triggered_buffer_setup(data->mag_indio_dev, 1350 &iio_pollfunc_store_time, 1351 kmx61_trigger_handler, 1352 NULL); 1353 if (ret < 0) { 1354 dev_err(&data->client->dev, 1355 "Failed to setup mag triggered buffer\n"); 1356 goto err_buffer_cleanup_acc; 1357 } 1358 } 1359 1360 ret = pm_runtime_set_active(&client->dev); 1361 if (ret < 0) 1362 goto err_buffer_cleanup_mag; 1363 1364 pm_runtime_enable(&client->dev); 1365 pm_runtime_set_autosuspend_delay(&client->dev, KMX61_SLEEP_DELAY_MS); 1366 pm_runtime_use_autosuspend(&client->dev); 1367 1368 ret = iio_device_register(data->acc_indio_dev); 1369 if (ret < 0) { 1370 dev_err(&client->dev, "Failed to register acc iio device\n"); 1371 goto err_pm_cleanup; 1372 } 1373 1374 ret = iio_device_register(data->mag_indio_dev); 1375 if (ret < 0) { 1376 dev_err(&client->dev, "Failed to register mag iio device\n"); 1377 goto err_iio_unregister_acc; 1378 } 1379 1380 return 0; 1381 1382err_iio_unregister_acc: 1383 iio_device_unregister(data->acc_indio_dev); 1384err_pm_cleanup: 1385 pm_runtime_dont_use_autosuspend(&client->dev); 1386 pm_runtime_disable(&client->dev); 1387err_buffer_cleanup_mag: 1388 if (client->irq > 0) 1389 iio_triggered_buffer_cleanup(data->mag_indio_dev); 1390err_buffer_cleanup_acc: 1391 if (client->irq > 0) 1392 iio_triggered_buffer_cleanup(data->acc_indio_dev); 1393err_trigger_unregister_motion: 1394 iio_trigger_unregister(data->motion_trig); 1395err_trigger_unregister_mag_dready: 1396 iio_trigger_unregister(data->mag_dready_trig); 1397err_trigger_unregister_acc_dready: 1398 iio_trigger_unregister(data->acc_dready_trig); 1399err_chip_uninit: 1400 kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, true); 1401 return ret; 1402} 1403 1404static void kmx61_remove(struct i2c_client *client) 1405{ 1406 struct kmx61_data *data = i2c_get_clientdata(client); 1407 1408 iio_device_unregister(data->acc_indio_dev); 1409 iio_device_unregister(data->mag_indio_dev); 1410 1411 pm_runtime_disable(&client->dev); 1412 pm_runtime_set_suspended(&client->dev); 1413 1414 if (client->irq > 0) { 1415 iio_triggered_buffer_cleanup(data->acc_indio_dev); 1416 iio_triggered_buffer_cleanup(data->mag_indio_dev); 1417 iio_trigger_unregister(data->acc_dready_trig); 1418 iio_trigger_unregister(data->mag_dready_trig); 1419 iio_trigger_unregister(data->motion_trig); 1420 } 1421 1422 mutex_lock(&data->lock); 1423 kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, true); 1424 mutex_unlock(&data->lock); 1425} 1426 1427static int kmx61_suspend(struct device *dev) 1428{ 1429 int ret; 1430 struct kmx61_data *data = i2c_get_clientdata(to_i2c_client(dev)); 1431 1432 mutex_lock(&data->lock); 1433 ret = kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, 1434 false); 1435 mutex_unlock(&data->lock); 1436 1437 return ret; 1438} 1439 1440static int kmx61_resume(struct device *dev) 1441{ 1442 u8 stby = 0; 1443 struct kmx61_data *data = i2c_get_clientdata(to_i2c_client(dev)); 1444 1445 if (data->acc_stby) 1446 stby |= KMX61_ACC_STBY_BIT; 1447 if (data->mag_stby) 1448 stby |= KMX61_MAG_STBY_BIT; 1449 1450 return kmx61_set_mode(data, stby, KMX61_ACC | KMX61_MAG, true); 1451} 1452 1453static int kmx61_runtime_suspend(struct device *dev) 1454{ 1455 struct kmx61_data *data = i2c_get_clientdata(to_i2c_client(dev)); 1456 int ret; 1457 1458 mutex_lock(&data->lock); 1459 ret = kmx61_set_mode(data, KMX61_ALL_STBY, KMX61_ACC | KMX61_MAG, true); 1460 mutex_unlock(&data->lock); 1461 1462 return ret; 1463} 1464 1465static int kmx61_runtime_resume(struct device *dev) 1466{ 1467 struct kmx61_data *data = i2c_get_clientdata(to_i2c_client(dev)); 1468 u8 stby = 0; 1469 1470 if (!data->acc_ps) 1471 stby |= KMX61_ACC_STBY_BIT; 1472 if (!data->mag_ps) 1473 stby |= KMX61_MAG_STBY_BIT; 1474 1475 return kmx61_set_mode(data, stby, KMX61_ACC | KMX61_MAG, true); 1476} 1477 1478static const struct dev_pm_ops kmx61_pm_ops = { 1479 SYSTEM_SLEEP_PM_OPS(kmx61_suspend, kmx61_resume) 1480 RUNTIME_PM_OPS(kmx61_runtime_suspend, kmx61_runtime_resume, NULL) 1481}; 1482 1483static const struct i2c_device_id kmx61_id[] = { 1484 { "kmx611021" }, 1485 { } 1486}; 1487 1488MODULE_DEVICE_TABLE(i2c, kmx61_id); 1489 1490static struct i2c_driver kmx61_driver = { 1491 .driver = { 1492 .name = "kmx61", 1493 .pm = pm_ptr(&kmx61_pm_ops), 1494 }, 1495 .probe = kmx61_probe, 1496 .remove = kmx61_remove, 1497 .id_table = kmx61_id, 1498}; 1499 1500module_i2c_driver(kmx61_driver); 1501 1502MODULE_AUTHOR("Daniel Baluta <daniel.baluta@intel.com>"); 1503MODULE_DESCRIPTION("KMX61 accelerometer/magnetometer driver"); 1504MODULE_LICENSE("GPL v2");
~/
Read-only file preview.